iCub-main
embObjStrain.cpp
Go to the documentation of this file.
1 
2 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
3 
4 /*
5 * Copyright (C) 2012 Robotcub Consortium
6 * Author: Alberto Cardellino
7 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
8 *
9 */
10 
11 // general purpose stuff.
12 #include <string>
13 #include <iostream>
14 #include <string.h>
15 
16 // Yarp Includes
17 #include <yarp/os/Time.h>
18 #include <yarp/os/Log.h>
19 #include <yarp/os/LogStream.h>
20 #include <stdarg.h>
21 #include <stdio.h>
22 #include <yarp/dev/PolyDriver.h>
23 #include <ace/config.h>
24 #include <ace/Log_Msg.h>
25 
26 
27 // specific to this device driver.
28 #include <embObjStrain.h>
29 #include <ethManager.h>
30 #include <yarp/os/LogStream.h>
31 #include "EoAnalogSensors.h"
32 #include "EOnv_hid.h"
33 
34 #include "EoProtocol.h"
35 #include "EoProtocolMN.h"
36 #include "EoProtocolAS.h"
37 
38 #include <yarp/os/NetType.h>
39 #include <yarp/conf/environment.h>
40 
41 #ifdef WIN32
42 #pragma warning(once:4355)
43 #endif
44 
45 
46 
47 using namespace yarp;
48 using namespace yarp::os;
49 using namespace yarp::dev;
50 
51 
52 inline bool NOT_YET_IMPLEMENTED(const char *txt)
53 {
54  yWarning() << std::string(txt) << " not yet implemented for embObjStrain\n";
55  return false;
56 }
57 
58 
59 bool embObjStrain::fromConfig(yarp::os::Searchable &_config)
60 {
61  if(false == parser->parseService(_config, serviceConfig))
62  {
63  return false;
64  }
65 
66  return true;
67 }
68 
69 
70 embObjStrain::embObjStrain()
71 {
72  serviceConfig.useCalibration = false;
73  serviceConfig.acquisitionrate = 0;
74  memset(&serviceConfig.ethservice, 0, sizeof(serviceConfig.ethservice));
75 
76  timeStamp = 0;
77 
78  counterSat=0;
79  counterError=0;
80  counterTimeout=0;
81 
82  status = IAnalogSensor::AS_OK;
83 
84  opened = false;
85 
86  analogdata.resize(0);
87  offset.resize(0);
88  scaleFactor.resize(0);
89 
90  scaleFactorIsFilled = false;
91 
92  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
93  if (tmp != "")
94  {
95  verbosewhenok = (bool)(yarp::conf::numeric::from_string(tmp, 0U));
96  }
97  else
98  {
99  verbosewhenok = false;
100  }
101 
102  parser = NULL;
103  res = NULL;
104 
105 }
106 
107 
108 embObjStrain::~embObjStrain()
109 {
110  analogdata.resize(0);
111  offset.resize(0);
112  scaleFactor.resize(0);
113 
114  if(NULL != parser)
115  {
116  delete parser;
117  parser = NULL;
118  }
119 }
120 
121 
122 bool embObjStrain::initialised()
123 {
124  return opened;
125 }
126 
127 
128 bool embObjStrain::open(yarp::os::Searchable &config)
129 {
130  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
131 
132  ethManager = eth::TheEthManager::instance();
133  if(NULL == ethManager)
134  {
135  yFatal() << "embObjStrain::open() fails to instantiate ethManager";
136  return false;
137  }
138 
139 
140  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
141  {
142  yError() << "embObjStrain::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
143  return false;
144  }
145  // add specific info about this device ...
146 
147  // when we split this device in three ones ... _as_type will be lost and we can move code tagged with tag__XXX0123_ in here
148 
149 
150  // - now all other things
151 
152  if(NULL == parser)
153  {
154  parser = new ServiceParser;
155  }
156 
157  // read stuff from config file
158  if(!fromConfig(config))
159  {
160  yError() << "embObjStrain for BOARD w/ IP" << boardIPstring << "is missing some configuration parameter. Check logs and your config file.";
161  return false;
162  }
163 
164  // and prepare analogdata + scaleFactor
165  {
166  // they must be of size: strain_Channels, and 0.0-initted / 1.0-initted
167  analogdata.resize(strain_Channels, 0.0);
168  offset.resize(strain_Channels, 0.0);
169  scaleFactor.resize(strain_Channels, 1.0);
170  }
171 
172 
173  // some other things ... tag__XXX0123_
174 
175 
176 
177  // -- instantiate EthResource etc.
178 
179  res = ethManager->requestResource2(this, config);
180  if(NULL == res)
181  {
182  yError() << "embObjStrain::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
183  return false;
184  }
185 
186  printServiceConfig();
187 
188  if(!res->verifyEPprotocol(eoprot_endpoint_analogsensors))
189  {
190  cleanup();
191  return false;
192  }
193 
194 
195 #if defined(EMBOBJSTRAIN_USESERVICEPARSER)
196  const eOmn_serv_parameter_t* servparam = &serviceConfig.ethservice;
197 #else
198  const eOmn_serv_parameter_t* servparam = NULL;
199 #endif
200 
201  if(false == res->serviceVerifyActivate(eomn_serv_category_strain, servparam, 5.0))
202  {
203  yError() << "embObjStrain::open() has an error in call of ethResources::serviceVerifyActivate() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
204  printServiceConfig();
205  cleanup();
206  return false;
207  }
208 
209 
210  printServiceConfig();
211 
212  // we always prepare the fullscales.
213  if(false == fillScaleFactor())
214  {
215  yError() << "embObjStrain::open() has failed in calling embObjStrain::fillScaleFactor()";
216  return false;
217  }
218 
219  if(false == sendConfig2Strain())
220  {
221  cleanup();
222  return false;
223  }
224 
225  if(false == initRegulars())
226  {
227  cleanup();
228  return false;
229  }
230 
231 
232  if(false == res->serviceStart(eomn_serv_category_strain))
233  {
234  yError() << "embObjStrain::open() fails to start service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
235  cleanup();
236  return false;
237  }
238  else
239  {
240  if(verbosewhenok)
241  {
242  yDebug() << "embObjStrain::open() correctly starts as service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
243  }
244  }
245 
246  opened = true;
247  return true;
248 }
249 
250 
251 bool embObjStrain::sendConfig2Strain(void)
252 {
253  eOas_strain_config_t strainConfig = {0};
254 
255  strainConfig.datarate = serviceConfig.acquisitionrate;
256  strainConfig.signaloncefullscale = eobool_false;
257  strainConfig.mode = (true == serviceConfig.useCalibration) ? (eoas_strainmode_txcalibrateddatacontinuously) : (eoas_strainmode_txuncalibrateddatacontinuously);
258 
259  // version with read-back
260 
261  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config);
262 
263  if(false == res->setcheckRemoteValue(id32, &strainConfig, 10, 0.010, 0.050))
264  {
265  yError() << "FATAL: embObjStrain::sendConfig2Strain() had an error while calling setcheckRemoteValue() for strain config in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
266  return false;
267  }
268  else
269  {
270  if(verbosewhenok)
271  {
272  yDebug() << "embObjStrain::sendConfig2Strain() correctly configured strain coinfig in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
273  }
274  }
275 
276  return true;
277 }
278 
279 
280 
281 //#warning --> marco.accame: review function embObjStrain::fillScaleFactor() as in comment below
282 // it is better to change the behaviour of the function so that: 1. we send the request, 2. we wait for the sig<> and unblock a mutex
283 // current implementation relies on a wait of 1 sec and check of non-zero length of an array: smart but not the rigth way to do it.
284 
285 // EVEN better: in serviceVerifyActivate() we allow the retrieval of a parameter which the ETH board sends back. in such a param it is contained
286 // the fullscales values ...
287 
288 // marco.accame on 09 jan 2018: much better using an ask(id32_fullscale) and making this variable proxied inside the ETH board ...
289 
290 bool embObjStrain::fillScaleFactor()
291 {
292  // if we already have set the scalefactor ...
293  if(true == scaleFactorIsFilled)
294  {
295  return true;
296  }
297 
298  // at first we set the scale factors to 1, so that we are sure they have a safe value. it redundant, as we already set it to 1.0
299  for(size_t i = 0; i<scaleFactor.size(); i++)
300  {
301  scaleFactor[i] = 1.0f;
302  }
303 
304  // if we dont need calibration we are done
305  if(false == serviceConfig.useCalibration)
306  {
307  if(verbosewhenok)
308  {
309  yDebug() << "embObjStrain::fillScaleFactor(): we DONT use calibration, thus all scale factors are set to 1.0";
310  }
311 
312  scaleFactorIsFilled = true;
313  return true;
314  }
315 
316  // if we need calibration, then we need to ask the fullscales directly to the strain
317 
318 
319  // marco.accame on 11 apr 2014:
320  // added the code under ifdef 1. the reason is that one cannot rely on validity of data structures inside the EOnv, as in protocol v2 the requirement is that
321  // the initialisation is not specialised and is ... all zeros. if the user wants to init to proper values must redefine the relevant INIT funtion.
322  // in this case, the eoprot_fun_INIT_as_strain_status_fullscale().
323  // extern void eoprot_fun_INIT_as_strain_status_fullscale(const EOnv* nv)
324  // {
325  // eOas_arrayofupto12bytes_t fullscale_values = {0};
326  // eo_array_New(6, 2, &fullscale_values); // itemsize = 2, capacity = 6
327  // eo_nv_Set(nv, &fullscale_values, eobool_true, eo_nv_upd_dontdo);
328  // }
329  // moreover, even if properly initted, it is required to set the size to 0 because the size being not 0 is the check of reception of a message.
330 
331 
332  bool gotFullScaleValues = false;
333 
334 
335  // Check initial size of array... it should be zero.
336  int timeout, NVsize;
337  EOnv tmpNV;
338  EOnv *p_tmpNV = NULL;
339  eOas_arrayofupto12bytes_t fullscale_values = {0};
340  // force it to be an empty array of itemsize 2 and capacity 6.
341  // the reason is that the eoprot_tag_as_strain_status_fullscale contains 3 forces and 3 torques each of 2 bytes. see eOas_strain_status_t in EoAnalogSensors.h
342  eo_array_New(6, 2, &fullscale_values);
343 
344  eOprotID32_t id32_fullscale = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_status_fullscale);
345 
346 
347  // at first we impose that the local value of fullscales is zero.
348  // we also force the change because this variable is readonly
349  const bool overrideROprotection = true;
350  res->setLocalValue(id32_fullscale, &fullscale_values, overrideROprotection);
351 
352 
353  // Prepare analog sensor
354  eOas_strain_config_t strainConfig = {0};
355  strainConfig.datarate = serviceConfig.acquisitionrate;
356  strainConfig.mode = eoas_strainmode_acquirebutdonttx;
357  strainConfig.signaloncefullscale = eobool_true;
358 
359  eOprotID32_t id32_strain_config = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config);
360 
361  timeout = 5;
362 
363  // wait for response
364  while(!gotFullScaleValues && (timeout != 0))
365  {
366  res->setRemoteValue(id32_strain_config, &strainConfig);
367  SystemClock::delaySystem(1.0);
368  // read fullscale values
369  res->getLocalValue(id32_fullscale, &fullscale_values);
370  // If data arrives, size is bigger than zero
371  //#warning --> marco.accame says: to wait for 1 sec and read size is ok. a different way is to ... wait for a semaphore incremented by the reply of the board. think of it!
372  NVsize = eo_array_Size((EOarray *)&fullscale_values);
373 
374  if(0 != NVsize)
375  {
376  gotFullScaleValues = true;
377  break;
378  }
379 
380  timeout--;
381  if(verbosewhenok)
382  {
383  yWarning() << "embObjStrain::fillScaleFactor(): for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": full scale val not arrived yet... retrying in 1 sec";
384  }
385  }
386 
387  if((false == gotFullScaleValues) && (0 == timeout))
388  {
389  yError() << "embObjStrain::fillScaleFactor(): ETH Analog sensor: request for calibration parameters timed out for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
390  return false;
391  }
392 
393  if((strain_Channels != NVsize))
394  {
395  yError() << "Analog sensor Calibration data has a different size from channels number in configuration file for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << "Aborting";
396  return false;
397  }
398 
399 
400  if(gotFullScaleValues)
401  {
402  if(verbosewhenok)
403  {
404  yWarning() << "embObjStrain::fillScaleFactor() detected that already has full scale values for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
405  yDebug() << "embObjStrain::fillScaleFactor(): Fullscale values for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << "are: size=" << eo_array_Size((EOarray *)&fullscale_values) << " numchannel=" << strain_Channels;
406  }
407 
408  for (size_t i = 0; i<scaleFactor.size(); i++)
409  {
410  // Get the k-th element of the array as a 2 bytes msg
411  uint8_t *msg = (uint8_t *) eo_array_At((EOarray *) &fullscale_values, i);
412  if(NULL == msg)
413  {
414  yError() << "I don't receive data for channel " << i;
415  return false;
416  }
417  // Got from CanBusMotionControl... here order of bytes seems inverted with respect to calibratedValues or uncalibratedValues (see callback of can strain messages inside the FW of ETHBOARD)
418  scaleFactor[i] = ((uint16_t)(msg[0]<<8) | msg[1]);
419  //yError() << " scale factor[" << i << "] = " << scaleFactor[i];
420  if(verbosewhenok)
421  {
422  yDebug() << "embObjStrain::fillScaleFactor(): channel " << i << "full scale value " << scaleFactor[i];
423  }
424  }
425 
426  scaleFactorIsFilled = true;
427  }
428 
429  return scaleFactorIsFilled;
430 }
431 
432 
433 bool embObjStrain::initRegulars()
434 {
435  // configure regular rops
436 
437  vector<eOprotID32_t> id32v(0);
438  eOprotID32_t id32 = eo_prot_ID32dummy;
439 
440  // we need to choose the id32 to put inside the vector
441 
442  if(true == serviceConfig.useCalibration)
443  {
444  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_status_calibratedvalues);
445  }
446  else
447  {
448  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_status_uncalibratedvalues);
449  }
450 
451 
452  // put it inside vector
453 
454  id32v.push_back(id32);
455 
456 
457  // now we send the vector
458 
459  if(false == res->serviceSetRegulars(eomn_serv_category_strain, id32v))
460  {
461  yError() << "embObjStrain::initRegulars() fails to add its variables to regulars: cannot proceed any further";
462  return false;
463  }
464  else
465  {
466  if(verbosewhenok)
467  {
468  yDebug() << "embObjStrain::initRegulars() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
469  char nvinfo[128];
470  for (size_t r = 0; r<id32v.size(); r++)
471  {
472  uint32_t item = id32v.at(r);
473  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
474  yDebug() << "\t it added regular rop for" << nvinfo;
475  }
476  }
477  }
478 
479  return true;
480 }
481 
482 
487 int embObjStrain::read(yarp::sig::Vector &out)
488 {
489  // This method gives analogdata to the analogServer
490 
491  if(false == opened)
492  {
493  return false;
494  }
495 
496  std::lock_guard<std::mutex> lck(mtx);
497 
498  // errors are not handled for now... it'll always be OK!!
499  if (status != IAnalogSensor::AS_OK)
500  {
501  switch (status)
502  {
503  case IAnalogSensor::AS_OVF:
504  {
505  counterSat++;
506  } break;
507  case IAnalogSensor::AS_ERROR:
508  {
509  counterError++;
510  } break;
511  case IAnalogSensor::AS_TIMEOUT:
512  {
513  counterTimeout++;
514  } break;
515  default:
516  {
517  counterError++;
518  } break;
519  }
520  return status;
521  }
522 
523  out.resize(analogdata.size());
524  for (size_t k = 0; k<analogdata.size(); k++)
525  {
526  out[k] = analogdata[k]+offset[k];
527  }
528 
529  return status;
530 }
531 
532 
533 void embObjStrain::resetCounters()
534 {
535  counterSat=0;
536  counterError=0;
537  counterTimeout=0;
538 }
539 
540 
541 void embObjStrain::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
542 {
543  sat=counterSat;
544  err=counterError;
545  to=counterTimeout;
546 }
547 
548 
549 int embObjStrain::getState(int ch)
550 {
551  printf("getstate\n");
552  return AS_OK;
553 }
554 
555 
556 int embObjStrain::getChannels()
557 {
558  return strain_Channels;
559 }
560 
561 
562 int embObjStrain::calibrateSensor()
563 {
564  std::lock_guard<std::mutex> lck(mtx);
565  for (size_t i = 0; i < analogdata.size(); i++)
566  {
567  offset[i] = -analogdata[i];
568  }
569  return AS_OK;
570 }
571 
572 
573 int embObjStrain::calibrateSensor(const yarp::sig::Vector& value)
574 {
575  return AS_OK;
576 }
577 
578 
579 int embObjStrain::calibrateChannel(int ch)
580 {
581  return AS_OK;
582 }
583 
584 
585 int embObjStrain::calibrateChannel(int ch, double v)
586 {
587  return AS_OK;
588 }
589 
590 eth::iethresType_t embObjStrain::type()
591 {
593 }
594 
595 bool embObjStrain::update(eOprotID32_t id32, double timestamp, void* rxdata)
596 {
597  id32 = id32;
598  timestamp = timestamp;
599 
600  if(false == opened)
601  {
602  return false;
603  }
604 
605  // called by feat_manage_analogsensors_data() which is called by:
606  // eoprot_fun_UPDT_as_strain_status_calibratedvalues() or eoprot_fun_UPDT_as_strain_status_uncalibratedvalues()
607  // the void* parameter inside this function is a eOas_arrayofupto12bytes_t*
608  // and can be treated as a EOarray
609 
610  EOarray *array = (EOarray*)rxdata;
611  uint8_t size = eo_array_Size(array);
612  uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 2, as the code after uses this convention
613  if((0 == size) || (2 != itemsize))
614  {
615  return false;
616  }
617 
618  // lock analogdata
619  std::lock_guard<std::mutex> lck(mtx);
620 
621  for (size_t k = 0; k<analogdata.size(); k++)
622  {
623  // Get the kth element of the array as a 2 bytes msg
624  char* tmp = (char*) eo_array_At(array, k);
625  // marco.accame: i am nervous about iterating for strain_Channels instead of size of array....
626  // thus i add a protection. if k goes beyond size of array, eo_array_At() returns NULL.
627  if(NULL != tmp)
628  {
629  uint8_t msg[2] = {0};
630  memcpy(msg, tmp, 2);
631  // Got from canBusMotionControl
632  analogdata[k] = (short)( ( (((unsigned short)(msg[1]))<<8)+msg[0]) - (unsigned short) (0x8000) );
633 
634  if(true == serviceConfig.useCalibration)
635  {
636  analogdata[k] = analogdata[k]*scaleFactor[k]/float(0x8000);
637  }
638  }
639  }
640 
641  return true;
642 }
643 
644 
645 
646 bool embObjStrain::close()
647 {
648  opened = false;
649 
650  cleanup();
651  return true;
652 }
653 
654 
655 void embObjStrain::printServiceConfig(void)
656 {
657  char loc[20] = {0};
658  char fir[20] = {0};
659  char pro[20] = {0};
660  const char * boardname = (NULL != res) ? (res->getProperties().boardnameString.c_str()) : ("NOT-ASSIGNED-YET");
661  const char * ipv4 = (NULL != res) ? (res->getProperties().ipv4addrString.c_str()) : ("NOT-ASSIGNED-YET");
662  const char * boardtype = eoboards_type2string2(static_cast<eObrd_type_t>(serviceConfig.ethservice.configuration.data.as.strain.boardtype.type), eobool_true);
663 
664  parser->convert(serviceConfig.ethservice.configuration.data.as.strain.canloc, loc, sizeof(loc));
665  parser->convert(serviceConfig.ethservice.configuration.data.as.strain.boardtype.firmware, fir, sizeof(fir));
666  parser->convert(serviceConfig.ethservice.configuration.data.as.strain.boardtype.protocol, pro, sizeof(pro));
667 
668  yInfo() << "The embObjStrain device using BOARD" << boardname << " w/ IP" << ipv4 << "has the following service config:";
669  yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate;
670  yInfo() << "- useCalibration =" << serviceConfig.useCalibration;
671  yInfo() << "- STRAIN of type" << boardtype << "named" << serviceConfig.nameOfStrain << "@" << loc << "with required protocol version =" << pro << "and required firmware version =" << fir;
672 }
673 
674 
675 void embObjStrain::cleanup(void)
676 {
677  if(ethManager == NULL) return;
678 
679  int ret = ethManager->releaseResource2(res, this);
680  res = NULL;
681  if(ret == -1)
682  ethManager->killYourself();
683 }
684 
685 // eof
686 
static TheEthManager * instance()
Definition: ethManager.cpp:159
bool NOT_YET_IMPLEMENTED(const char *txt)
static int v
Definition: iCub_Sim.cpp:42
double sat(const double val, const double min, const double max)
Definition: utils.h:183
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
iethresType_t
Definition: IethResource.h:61
@ iethres_analogstrain
Definition: IethResource.h:65
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8
degrees offset
Definition: sine.m:4