iCub-main
embObjPSC.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT)
3  *
4  * This software may be modified and distributed under the terms of the
5  * LGPL-2.1+ license. See the accompanying LICENSE file for details.
6  */
7 
8 
9 // general purpose stuff.
10 #include <string>
11 #include <iostream>
12 #include <string.h>
13 
14 // Yarp Includes
15 #include <yarp/os/Time.h>
16 #include <yarp/os/Log.h>
17 #include <yarp/os/LogStream.h>
18 #include <stdarg.h>
19 #include <stdio.h>
20 #include <yarp/dev/PolyDriver.h>
21 #include <ace/config.h>
22 #include <ace/Log_Msg.h>
23 
24 
25 // specific to this device driver.
26 #include <embObjPSC.h>
27 #include <ethManager.h>
28 #include <yarp/os/LogStream.h>
29 #include "EoAnalogSensors.h"
30 #include "EOnv_hid.h"
31 
32 #include "EoProtocol.h"
33 #include "EoProtocolMN.h"
34 #include "EoProtocolAS.h"
35 
36 #include <yarp/os/NetType.h>
37 
38 #ifdef WIN32
39 #pragma warning(once:4355)
40 #endif
41 
42 
43 
44 using namespace yarp;
45 using namespace yarp::os;
46 using namespace yarp::dev;
47 
48 
49 
50 bool embObjPSC::fromConfig(yarp::os::Searchable &config, servConfigPSC_t &serviceConfig)
51 {
52  ServiceParser parser;
53  if(false == parser.parseService(config, serviceConfig))
54  {
55  yError() << m_PDdevice.getBoardInfo() << ": missing some configuration parameter. Check logs and your config file.";
56  return false;
57  }
58 
59  return true;
60 }
61 
62 
63 embObjPSC::embObjPSC(): m_PDdevice("embObjPSC")
64 {
65 ;
66 }
67 
68 
70 {
71  close();
72 }
73 
74 
76 {
77  return m_PDdevice.isOpen();
78 }
79 
80 
81 
82 
83 bool embObjPSC::open(yarp::os::Searchable &config)
84 {
85  // 1) prepare Eth service verifing if the eth manager is available and parsing info about the eth board.
86 
87  if(! m_PDdevice.prerareEthService(config, this))
88  return false;
89 
90  // 2) read stuff from config file
91  servConfigPSC_t serviceConfig;
92  if(!fromConfig(config, serviceConfig))
93  {
94  yError() << "embObjPSC missing some configuration parameter. Check logs and your config file.";
95  return false;
96  }
97 
98  // 3) prepare data vector
99  {
100  m_data.resize(eOas_psc_data_maxnumber, 0.0);
101  }
102 
103 
104  // 4) verify analog sensor protocol and then verify-Activate the PSC service
105  if(!m_PDdevice.res->verifyEPprotocol(eoprot_endpoint_analogsensors))
106  {
107  cleanup();
108  return false;
109  }
110 
111 
112  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
113 
114  if(!m_PDdevice.res->serviceVerifyActivate(eomn_serv_category_psc, servparam, 5.0))
115  {
116  yError() << m_PDdevice.getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate() ";
117  cleanup();
118  return false;
119  }
120 
121 
122  //printServiceConfig();
123 
124 
125  if(false == sendConfig2PSCboards(serviceConfig))
126  {
127  cleanup();
128  return false;
129  }
130 
131  // Set variable to be signaled
132  if(false == initRegulars())
133  {
134  cleanup();
135  return false;
136  }
137 
138 
139  if(!m_PDdevice.res->serviceStart(eomn_serv_category_psc))
140  {
141  yError() << m_PDdevice.getBoardInfo() << "open() fails to start as service.... cannot continue";
142  cleanup();
143  return false;
144  }
145  else
146  {
147  if(m_PDdevice.isVerbose())
148  {
149  yDebug() << m_PDdevice.getBoardInfo() << "open() correctly starts service";
150  }
151  }
152 
153  //send enable command to psc boards
154  sendStart2PSCboards();
155 
156  m_PDdevice.setOpen(true);
157  return true;
158 }
159 
160 
161 bool embObjPSC::sendConfig2PSCboards(servConfigPSC_t &serviceConfig)
162 {
163  eOprotID32_t id32 = eo_prot_ID32dummy;
164 
165  eOas_psc_config_t cfg;
166  cfg.datarate = serviceConfig.acquisitionrate;
167  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_psc, 0, eoprot_tag_as_psc_config);
168 
169  if(false == m_PDdevice.res->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
170  {
171  yError() << m_PDdevice.getBoardInfo() << "FATAL error in sendConfig2PSCboards() while try to configure datarate=" << cfg.datarate;
172  return false;
173  }
174 
175  if(m_PDdevice.isVerbose())
176  {
177  yDebug() << m_PDdevice.getBoardInfo() << ": sendConfig2PSCboards() correctly configured boards with datarate=" << cfg.datarate;
178  }
179 
180  return true;
181 
182 }
183 
184 bool embObjPSC::sendStart2PSCboards(void)
185 {
186  eOprotID32_t id32 = eo_prot_ID32dummy;
187 
188  uint8_t enable=1;
189 
190  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_psc, 0, eoprot_tag_as_psc_cmmnds_enable);
191 
192  if(false == m_PDdevice.res->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
193  {
194  yError() << m_PDdevice.getBoardInfo() << "FATAL error in sendStart2PSCboards() while try to enable the boards transmission";
195  return false;
196  }
197 
198  if(m_PDdevice.isVerbose())
199  {
200  yDebug() << m_PDdevice.getBoardInfo() << ": sendStart2PSCboards() correctly enabled the boards transmission";
201  }
202 
203  return true;
204 
205 }
206 
207 
208 bool embObjPSC::initRegulars(void)
209 {
210  // configure regular rops
211 
212  vector<eOprotID32_t> id32v(0);
213  eOprotID32_t id32 = eo_prot_ID32dummy;
214 
215  // we need to choose the id32 to put inside the vector
216 
217  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_psc, 0, eoprot_tag_as_psc_status);;
218 
219  // and put it inside vector
220 
221  id32v.push_back(id32);
222 
223  // now we send the vector
224 
225  if(false == m_PDdevice.res->serviceSetRegulars(eomn_serv_category_psc, id32v))
226  {
227  yError() << m_PDdevice.getBoardInfo() << "initRegulars() fails to add its variables to regulars: cannot proceed any further";
228  return false;
229  }
230 
231  if(m_PDdevice.isVerbose())
232  {
233  yDebug() << m_PDdevice.getBoardInfo() << "initRegulars() added" << id32v.size() << "regular rops ";
234  char nvinfo[128];
235  for (size_t r = 0; r<id32v.size(); r++)
236  {
237  uint32_t item = id32v.at(r);
238  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
239  yDebug() << "\t it added regular rop for" << nvinfo;
240  }
241  }
242 
243 
244  return true;
245 }
246 
247 
253 int embObjPSC::read(yarp::sig::Vector &out)
254 {
255  // This method gives the data, received from embedded boards, to the analogServer
256 
257  if(!m_PDdevice.isOpen())
258  return AS_ERROR ;
259 
260  std::lock_guard<std::mutex> lock(m_mutex);
261 
262  // errors are not handled for now... it'll always be OK!!
263  out=m_data;
264 
265  return AS_OK;
266 }
267 
268 
269 
271 {
272  printf("getstate\n");
273  return AS_OK;
274 }
275 
276 
278 {
279  return static_cast<int>(eOas_psc_data_maxnumber);
280 }
281 
282 
284 {
285  return AS_OK;
286 }
287 
288 
289 int embObjPSC::calibrateSensor(const yarp::sig::Vector& value)
290 {
291  return AS_OK;
292 }
293 
294 
296 {
297  return AS_OK;
298 }
299 
300 
301 int embObjPSC::calibrateChannel(int ch, double v)
302 {
303  return AS_OK;
304 }
305 
306 
308 {
309  return eth::iethres_analogpsc;
310 }
311 
312 
313 bool embObjPSC::update(eOprotID32_t id32, double timestamp, void* rxdata)
314 {
315  // called by feat_manage_analogsensors_data() which is called by:
316  // eoprot_fun_UPDT_as_psc_status
317  if(!m_PDdevice.isOpen())
318  return false;
319 
320  eOas_psc_status_t *psc_st_ptr = (eOas_psc_status_t*)rxdata;
321  uint32_t sizeOfData = psc_st_ptr->arrayofdata.head.size;
322 
323  if(sizeOfData>m_data.size())
324  yWarning() << m_PDdevice.getBoardInfo() << "In update function I received more data than I had been configured to store.My size is " << m_data.size() << "while I received " << sizeOfData << "values!";
325 
326  std::lock_guard<std::mutex> lock(m_mutex);
327 
328  for(uint32_t i=0; i<sizeOfData; i++)
329  {
330  eOas_psc_data_t pscdata = psc_st_ptr->arrayofdata.data[i];
331  //psc value is in deci-degree
332  m_data[i]= pscdata.value*0.1;
333  }
334 
335  return true;
336 }
337 
338 
339 
340 
342 {
343  cleanup();
344  return true;
345 }
346 
347 
348 // void embObjPSC::printServiceConfig(void)
349 // {
350 // char loc[20] = {0};
351 // char fir[20] = {0};
352 // char pro[20] = {0};
353 //
354 // const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
355 // const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
356 //
357 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.canloc, loc, sizeof(loc));
358 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.firmware, fir, sizeof(fir));
359 // parser->convert(serviceConfig.ethservice.configuration.data.as.mais.version.protocol, pro, sizeof(pro));
360 //
361 // yInfo() << "The embObjPSC device using BOARD" << boardname << "w/ IP" << ipv4 << "has the following service config:";
362 // yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
363 // yInfo() << "- MAIS named" << serviceConfig.nameOfMais << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
364 // }
365 
366 
367 void embObjPSC::cleanup(void)
368 {
369  m_PDdevice.cleanup(static_cast <eth::IethResource*> (this));
370 }
371 
372 // eof
373 
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceVerifyActivate(eOmn_serv_category_t category, const eOmn_serv_parameter_t *param, double timeout=0.500)=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
virtual bool serviceStart(eOmn_serv_category_t category, double timeout=0.500)=0
bool prerareEthService(yarp::os::Searchable &config, eth::IethResource *interface)
void cleanup(eth::IethResource *interface)
eth::AbstractEthResource * res
virtual int calibrateChannel(int ch, double v)
Definition: embObjPSC.cpp:301
virtual int read(yarp::sig::Vector &out)
Read a vector from the sensor.
Definition: embObjPSC.cpp:253
virtual int getChannels()
Definition: embObjPSC.cpp:277
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
Definition: embObjPSC.cpp:313
bool open(yarp::os::Searchable &config)
Definition: embObjPSC.cpp:83
virtual int calibrateSensor()
Definition: embObjPSC.cpp:283
virtual int getState(int ch)
Definition: embObjPSC.cpp:270
virtual bool initialised()
Definition: embObjPSC.cpp:75
virtual eth::iethresType_t type()
Definition: embObjPSC.cpp:307
static int v
Definition: iCub_Sim.cpp:42
iethresType_t
Definition: IethResource.h:61
@ iethres_analogpsc
Definition: IethResource.h:73
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:80