iCub-main
embObjAnalogSensor.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 <embObjAnalogSensor.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 embObjAnalogSensor\n";
55  return false;
56 }
57 
58 // generic function that checks is key1 is present in input bottle and that the result has size elements
59 // return true/false
60 bool embObjAnalogSensor::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
61 {
62  size++; // size includes also the name of the parameter
63  Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
64  if (tmp.isNull())
65  {
66  yError ("%s not found\n", key1.c_str());
67  return false;
68  }
69 
70  if(tmp.size()!=size)
71  {
72  yError("%s incorrect number of entries\n", key1.c_str());
73  return false;
74  }
75 
76  out=tmp;
77 
78  return true;
79 }
80 
81 bool embObjAnalogSensor::fromConfig(yarp::os::Searchable &_config)
82 {
83  Bottle xtmp;
84  int format=0;
85 
86 
87  // Analog Sensor stuff
88  Bottle config = _config.findGroup("GENERAL");
89  if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
90  {
91  yError() << "embObjAnalogSensor Using default value = 0 (disabled)";
92  _period = 0;
93  _as_type=AS_Type_NONE;
94  }
95  else
96  {
97  _period = xtmp.get(1).asInt32();
98  yDebug() << "embObjAnalogSensor::fromConfig() detects embObjAnalogSensor Using value of" << _period;
99  }
100 
101  if (!extractGroup(config, xtmp, "Channels","Number of channels of the sensor", 1))
102  {
103  yWarning("embObjAnalogSensor: Using default value = 0 (disabled)\n");
104  _channels = 0;
105  _numofsensors = 0;
106  _period = 0;
107  _as_type=AS_Type_NONE;
108  }
109  else
110  {
111  _channels = xtmp.get(1).asInt32();
112  }
113 
114 #if 0
115  if (!extractGroup(config, xtmp, "NumberOfSensors","Number of sensors managed", 1))
116  {
117  yWarning("embObjAnalogSensor: Using default value = 1 for _numofsensors\n");
118  _numofsensors = 1;
119  }
120  else
121  {
122  _numofsensors = xtmp.get(1).asInt32();
123  }
124 #endif
125 
126  // Type is a new parameter describing the sensor Type using human readable string
127  if(config.check("Type") )
128  {
129  yDebug() << "Using new syntax";
130  std::string type = config.find("Type").asString();
131  if(type == "inertial")
132  {
133  _as_type=AS_Type_INERTIAL_MTB;
134  }
135  else if(type == "strain")
136  {
137  _as_type=AS_Type_STRAIN;
138  }
139  else if(type == "mais")
140  {
141  _as_type=AS_Type_MAIS;
142  }
143  else
144  {
145  _as_type=AS_Type_NONE;
146  yError() << "embObjAnalogSensor: unknown device " << type << ". Supported devices are 'mais', 'strain' and 'inertial'";
147  return false;
148  }
149  }
150  else
151  {
152  // Infer the sensor type by combining other parameters
153  yDebug() << "Using old syntax";
154 
155  if (!extractGroup(config, xtmp, "Format","data format of the Analog Sensor", 1))
156  {
157  yWarning("embObjAnalogSensor: Using default value = 0 (disabled)");
158  _channels = 0;
159  _period = 0;
160  _as_type=AS_Type_NONE;
161  }
162  else
163  {
164  format = xtmp.get(1).asInt32();
165  }
166 
167  if((_channels==NUMCHANNEL_STRAIN) && (format==FORMATDATA_STRAIN))
168  {
169  _as_type=AS_Type_STRAIN;
170  }
171  else if((_channels==NUMCHANNEL_MAIS) && (format==FORMATDATA_MAIS))
172  {
173  _as_type=AS_Type_MAIS;
174  }
175  else
176  {
177  _as_type=AS_Type_NONE;
178  yError() << "embObjAnalogSensor incorrect config!channels="<< _channels <<" format="<< format;
179  cleanup();
180  return false;
181  }
182  }
183 
184  _numofsensors = 1;
185 
186  // however, if we have a AS_Type_INERTIAL_MTB, then we may have more than one. for sure not zero.
187 
188  if(AS_Type_INERTIAL_MTB == _as_type)
189  {
190  Bottle tmp;
191  _numofsensors = 0;
192 
193  tmp = config.findGroup("enabledAccelerometers");
194  _numofsensors += (tmp.size()-1); // sensors holds strings "enabledAccelerometers" and then all the others, thus i need a -1
195 
196  tmp = config.findGroup("enabledGyroscopes");
197  _numofsensors += (tmp.size()-1); // sensors holds strings "enabledGyroscopes" and then all the others, thus i need a -1
198 
199 #if 0
200  if (!extractGroup(config, xtmp, "enabledAccelerometers", "Position of managed sensors axpressed as strings", 1))
201  {
202  yWarning("embObjAnalogSensor: cannot find enabledAccelerometers\n");
203  _numofsensors = 1;
204  }
205  else
206  {
207  _numofsensors = xtmp.size();
208  }
209 #endif
210 
211  }
212 
213 
214 
215  if(AS_Type_STRAIN == _as_type)
216  {
217  if (!extractGroup(config, xtmp, "UseCalibration","Calibration parameters are needed", 1))
218  {
219  return false;
220  }
221  else
222  {
223  _useCalibration = xtmp.get(1).asInt32();
224  }
225  }
226 
227  return true;
228 }
229 
230 
231 embObjAnalogSensor::embObjAnalogSensor(): analogdata(0)
232 {
233  _useCalibration=0;
234  _channels=0;
235  _numofsensors = 1;
236  _period=0;
237  _as_type=AS_Type_NONE;
238  //_format=ANALOG_FORMAT_NONE;
239 
240 
241 // memset(_fromInertialPos2DataIndexAccelerometers, 255, sizeof(_fromInertialPos2DataIndexAccelerometers));
242 // memset(_fromInertialPos2DataIndexGyroscopes, 255, sizeof(_fromInertialPos2DataIndexGyroscopes));
243 
244  scaleFactor=0;
245 
246  timeStamp=0;
247  counterSat=0;
248  counterError=0;
249  counterTimeout=0;
250 
251  status=IAnalogSensor::AS_OK;
252 
253  opened = false;
254 
255  std::string tmp = yarp::conf::environment::get_string("ETH_VERBOSEWHENOK");
256  if (tmp != "")
257  {
258  verbosewhenok = (bool)yarp::conf::numeric::from_string(tmp, 0U);
259  }
260  else
261  {
262  verbosewhenok = false;
263  }
264 }
265 
267 {
268  if (analogdata != NULL)
269  delete analogdata;
270  if (scaleFactor != NULL)
271  delete scaleFactor;
272 }
273 
275 {
276  return opened;
277 }
278 
279 bool embObjAnalogSensor::open(yarp::os::Searchable &config)
280 {
281  // - first thing to do is verify if the eth manager is available. then i parse info about the eth board.
282 
283  ethManager = eth::TheEthManager::instance();
284  if(NULL == ethManager)
285  {
286  yFatal() << "embObjAnalogSensor::open() fails to instantiate ethManager";
287  return false;
288  }
289 
290 
291  if(false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
292  {
293  yError() << "embObjAnalogSensor::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
294  return false;
295  }
296  // add specific info about this device ...
297 
298  // when we split this device in three ones ... _as_type will be lost and we can move code tagged with tag__XXX0123_ in here
299 
300 
301  // - now all other things
302 
303 
304  bool ret = false;
305 
306  std::string str;
307  if(config.findGroup("GENERAL").find("verbose").asBool())
308  str=config.toString().c_str();
309  else
310  str="\n";
311  yTrace() << str;
312 
313  // Read stuff from config file
314  if(!fromConfig(config))
315  {
316  yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
317  return false;
318  }
319 
320 
321  // some other things ... tag__XXX0123_
322  eOmn_serv_category_t servcategory = eomn_serv_category_none;
323  if(AS_Type_STRAIN == _as_type)
324  {
325  servcategory = eomn_serv_category_strain;
326  }
327  else if(AS_Type_MAIS == _as_type)
328  {
329  servcategory = eomn_serv_category_mais;
330  }
331  else if(AS_Type_INERTIAL_MTB == _as_type)
332  {
333  servcategory = eomn_serv_category_inertials;
334  }
335 
336 
337 
338  // -- instantiate EthResource etc.
339 
340  res = ethManager->requestResource2(this, config);
341  if(NULL == res)
342  {
343  yError() << "embObjAnalogSensor::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring << " ... unable to continue";
344  return false;
345  }
346 
347 
348  if(!res->verifyEPprotocol(eoprot_endpoint_analogsensors))
349  {
350  cleanup();
351  return false;
352  }
353 
354  if(false == res->serviceVerifyActivate(servcategory, NULL))
355  {
356  yError() << "embObjAnalogSensor::open() has an error in call of ethResources::serviceVerifyActivate() for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
357  cleanup();
358  return false;
359  }
360 
361 
362  if(_as_type == AS_Type_INERTIAL_MTB)
363  {
364  // must create data to be of capacity: _channels*_numofsensors+2.
365  // scaleFactor is not used
366  // however we do that inside
367  analogdata = new AnalogData(2+_channels*_numofsensors, 2+_channels*_numofsensors+2+1); // i keep the +1 in second argument but i dont know why
368 
369  // the first two are: number of sensors and of channels
370  double *buffer = this->analogdata->getBuffer();
371  buffer[0] = _numofsensors;
372  buffer[1] = _channels;
373  // all the others
374  }
375  else
376  {
377  analogdata = new AnalogData(_channels, _channels+1);
378  scaleFactor = new double[_channels];
379  int i=0;
380  for (i=0; i<_channels; i++) scaleFactor[i]=1;
381 
382  // Real values will be read from the sensor itself during its initalization hereafter
383  for(int i=0; i<_channels; i++)
384  {
385  scaleFactor[i]=1;
386  }
387  }
388 
389 
390 
391  // configure the service: aka, send to the remote board information about the whereabouts of the can boards mais, strain, mtb which offers the service.
392 
393  switch(_as_type)
394  {
395  case AS_Type_MAIS:
396  {
397  ret = true; // not yet implemented
398  } break;
399 
400  case AS_Type_STRAIN:
401  {
402  ret = true; // not yet implemented
403  } break;
404 
406  {
407  ret = configServiceInertials(config);
408  } break;
409 
410  default:
411  {
412  //i should not be here. if AS_Type_NONE then i should get error in fromConfig function
413  ret = false;
414  }
415 
416  }
417  if(!ret)
418  {
419  cleanup();
420  return false;
421  }
422 
423  // configure the sensor(s)
424 
425  eOprotEntity_t entity = eoprot_entity_none;
426 
427  switch(_as_type)
428  {
429  case AS_Type_MAIS:
430  {
431  entity = eoprot_entity_as_mais;
432  ret = sendConfig2Mais();
433  } break;
434 
435  case AS_Type_STRAIN:
436  {
437  entity = eoprot_entity_as_strain;
438  ret = sendConfig2Strain();
439  } break;
440 
442  {
443  entity = eoprot_entity_as_inertial;
444  ret = sendConfig2SkinInertial(config);
445  } break;
446 
447  default:
448  {
449  //i should not be here. if AS_Type_NONE then i should get error in fromConfig function
450  ret = false;
451  }
452  }
453  if(!ret)
454  {
455  cleanup();
456  return false;
457  }
458 
459  // Set variable to be signalled
460  if(! init())
461  {
462  cleanup();
463  return false;
464  }
465 
466 
467  if(false == res->serviceStart(servcategory))
468  {
469  yError() << "embObjAnalogSensor::open() fails to start as service for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": cannot continue";
470  cleanup();
471  return false;
472  }
473  else
474  {
475  if(verbosewhenok)
476  {
477  yDebug() << "embObjAnalogSensor::open() correctly starts as service of BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
478  }
479  }
480 
481 
482  if(AS_Type_INERTIAL_MTB == _as_type)
483  {
484  // start the configured sensors
485 
486  eOas_inertial_commands_t startCommand = {0};
487  startCommand.enable = 1;
488 
489  uint32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_cmmnds_enable);
490  if(false == res->setRemoteValue(id32, (uint8_t*) &startCommand))
491  {
492  yError() << "embObjAnalogSensor::open() fails to command the start transmission of the inertials";
493  cleanup();
494  return false;
495  }
496  }
497 
498  opened = true;
499  return true;
500 }
501 
502 
503 
504 bool embObjAnalogSensor::sendConfig2Strain(void)
505 {
506  eOas_strain_config_t strainConfig = {0};
507 
508  strainConfig.datarate = _period;
509  strainConfig.signaloncefullscale = eobool_false;
510 
511  if(_useCalibration)
512  {
513  if( ! getFullscaleValues() )
514  {
515  yError() << "embObjAnalogSensor::sendConfig2Strain() has failed in calling embObjAnalogSensor::getFullscaleValues()";
516  return false;
517  }
518  strainConfig.mode = eoas_strainmode_txcalibrateddatacontinuously;
519  }
520  else
521  {
522  strainConfig.mode = eoas_strainmode_txuncalibrateddatacontinuously;
523  }
524 
525 
526 #if 1
527  // version with read-back
528 
529  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config);
530 
531  if(false == res->setcheckRemoteValue(id32, &strainConfig, 10, 0.010, 0.050))
532  {
533  yError() << "FATAL: embObjAnalogSensor::sendConfig2Strain() had an error while calling setcheckRemoteValue() for strain config in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
534  return false;
535  }
536  else
537  {
538  if(verbosewhenok)
539  {
540  yDebug() << "embObjAnalogSensor::sendConfig2Strain() correctly configured strain coinfig in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
541  }
542  }
543 
544  return true;
545 
546 #else
547 
548  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config);
549  if(false == res->setRemoteValue(protoid, (uint8_t *) &strainConfig))
550  {
551  yError() << "embObjAnalogSensor::sendConfig2Strain() could not send a set message for eoprot_tag_as_strain_config";
552  }
553 
554  return true;
555 #endif
556 
557 }
558 
559 bool embObjAnalogSensor::sendConfig2Mais(void)
560 {
561 #if 1
562  // version with read-back
563 
564  eOprotID32_t id32 = eo_prot_ID32dummy;
565 
566  // -- mais datarate
567 
568  uint8_t datarate = _period;
569  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_datarate);
570 
571  if(false == res->setcheckRemoteValue(id32, &datarate, 10, 0.010, 0.050))
572  {
573  yError() << "FATAL: embObjAnalogSensor::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais datarate in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
574  return false;
575  }
576  else
577  {
578  if(verbosewhenok)
579  {
580  yDebug() << "embObjAnalogSensor::sendConfig2Mais() correctly configured mais datarate at value" << datarate << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
581  }
582  }
583 
584  // -- mais tx mode
585 
586  eOenum08_t maismode = eoas_maismode_txdatacontinuously; // use eOas_maismode_t for value BUT USE for type (their sizes can be different !!)
587  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
588 
589  if(false == res->setcheckRemoteValue(id32, &maismode, 10, 0.010, 0.050))
590  {
591  yError() << "FATAL: embObjAnalogSensor::sendConfig2Mais() had an error while calling setcheckRemoteValue() for mais mode in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
592  return false;
593  }
594  else
595  {
596  if(verbosewhenok)
597  {
598  yDebug() << "embObjAnalogSensor::sendConfig2Mais() correctly configured mais mode at value" << maismode << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
599  }
600  }
601 
602  return true;
603 
604 #else
605  uint8_t datarate = _period;
606 
607  // set mais datarate = 1millisec
608  eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_datarate);
609 
610 
611  if(false == res->setRemoteValue(protoid, &datarate))
612  {
613  yError() << "embObjAnalogSensor::sendConfig2Mais() could not send a set message fot datarate";
614  yError() << "embObjAnalogSensor::sendConfig2Mais() however did not return false. IS CORRECT?";
615  }
616 
617  // set tx mode continuosly
618  eOas_maismode_t maismode = eoas_maismode_txdatacontinuously;
619  protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_config_mode);
620 
621 
622  if(false == res->setRemoteValue(protoid, (uint8_t *) &maismode))
623  {
624  yError() << "embObjAnalogSensor::sendConfig2Mais() could not send a set message fot maismode";
625  yError() << "embObjAnalogSensor::sendConfig2Mais() however did not return false. IS CORRECT?";
626  }
627 
628 #warning --> marco.accame: maybe we can add a readback of the datarate and maismode to be sure of successful operation
629 
630  return true;
631 #endif
632 }
633 
634 
635 bool embObjAnalogSensor::configServiceInertials(Searchable& globalConfig)
636 {
637 #if 1
638  return false;
639 #else
640  // find SERVICES-INERTIALS. if not found, exit mildly from function. in a first stage the remote eth board will already be configured.
641 
642  Bottle tmp = globalConfig.findGroup("SERVICES");
643  Bottle config = tmp.findGroup("INERTIALS");
644 
645  // prepare the config of the inertial sensors: datarate and the mask of the enabled ones.
646 
647  eOas_inertial_serviceconfig_t inertialServiceConfig = {0}; // by this initialisation, there is no sensor at all in the two can buses
648 
649 
650 
651  // meglio sarebbe mettere un controllo sul fatto che ho trovato InertialsCAN1mapping e che ha size coerente
652 
653  Bottle canmap;
654  int numofentries = 0;
655 
656  // CAN1
657  canmap = config.findGroup("InertialsCAN1mapping");
658  numofentries = canmap.size()-1;
659 
660  for(int i=0; i<numofentries; i++)
661  {
662  eOas_inertial_position_t pos = eoas_inertial_pos_none;
663 
664  std::string strpos = canmap.get(i+1).asString();
665 
666  pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
667  inertialServiceConfig.canmapofsupportedsensors[0][i] = pos;
668  }
669 
670  // CAN2
671  canmap = config.findGroup("InertialsCAN2mapping");
672  numofentries = canmap.size()-1;
673 
674  for(int i=0; i<numofentries; i++)
675  {
676  eOas_inertial_position_t pos = eoas_inertial_pos_none;
677 
678  std::string strpos = canmap.get(i+1).asString();
679 
680  pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
681  inertialServiceConfig.canmapofsupportedsensors[1][i] = pos;
682  }
683 
684  // configure the service
685 
686  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_config_service);
687  if(false == res->setRemoteValueUntilVerified(id32, &inertialServiceConfig, sizeof(inertialServiceConfig), 10, 0.010, 0.050, 2))
688  {
689  yError() << "FATAL: embObjAnalogSensor::configServiceInertials() had an error while calling setRemoteValueUntilVerified() for config in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
690  return false;
691  }
692  else
693  {
694  if(verbosewhenok)
695  {
696  yDebug() << "embObjAnalogSensor::configServiceInertials() correctly configured the service in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
697  }
698  }
699 
700 
701  return true;
702 #endif
703 }
704 
705 
706 bool embObjAnalogSensor::sendConfig2SkinInertial(Searchable& globalConfig)
707 {
708 #if 1
709 
710  return false;
711 
712 #else
713  eOprotID32_t id32 = eo_prot_ID32dummy;
714 
715  // configuration specific for skin-inertial device
716 
717  Bottle config = globalConfig.findGroup("GENERAL");
718 
719 
720  // prepare the config of the inertial sensors: datarate and the mask of the enabled ones.
721 
722  eOas_inertial_sensorsconfig_t inertialSensorsConfig = {0};
723 
724  inertialSensorsConfig.accelerometers = 0;
725  inertialSensorsConfig.gyroscopes = 0;
726  inertialSensorsConfig.datarate = _period;
727 
728 
729  // meglio sarebbe mettere un controllo sul fatto che ho trovato enabledAccelerometers e che ha size coerente
730 
731  Bottle sensors;
732 
733  sensors = config.findGroup("enabledAccelerometers");
734 
735  int numofaccelerometers = sensors.size()-1; // sensors holds strings "enabledAccelerometers" and then all the others, thus i need a -1
736 
737  for(int i=0; i<numofaccelerometers; i++)
738  {
739  eOas_inertial_position_t pos = eoas_inertial_pos_none;
740 
741  std::string strpos = sensors.get(i+1).asString();
742 
743  pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
744 
745  if(eoas_inertial_pos_none != pos)
746  {
747  _fromInertialPos2DataIndexAccelerometers[pos] = i;
748  inertialSensorsConfig.accelerometers |= EOAS_ENABLEPOS(pos);
749  }
750  }
751 
752  sensors = config.findGroup("enabledGyroscopes");
753 
754  int numofgyroscopess = sensors.size()-1; // sensors holds strings "enabledGyroscopes" and then all the others, thus i need a -1
755 
756  for(int i=0; i<numofgyroscopess; i++)
757  {
758  eOas_inertial_position_t pos = eoas_inertial_pos_none;
759 
760  std::string strpos = sensors.get(i+1).asString();
761 
762  pos = getLocationOfInertialSensor(strpos); // prendi la posizione dalla stringa strpos: fai una funzione apposita
763 
764  if(eoas_inertial_pos_none != pos)
765  {
766  _fromInertialPos2DataIndexGyroscopes[pos] = numofaccelerometers+i;
767  inertialSensorsConfig.gyroscopes |= EOAS_ENABLEPOS(pos);
768  }
769  }
770 
771  // configure the sensors (datarate and position)
772 
773  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_config_sensors);
774  if(false == res->setRemoteValueUntilVerified(id32, &inertialSensorsConfig, sizeof(inertialSensorsConfig), 10, 0.010, 0.050, 2))
775  {
776  yError() << "FATAL: embObjAnalogSensor::sendConfig2SkinInertial() had an error while calling setRemoteValueUntilVerified() for config in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
777  return false;
778  }
779  else
780  {
781  if(verbosewhenok)
782  {
783  yDebug() << "embObjAnalogSensor::sendConfig2SkinInertial() correctly configured enabled sensors with period" << _period << "in BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
784  }
785  }
786 
787 
788 // // start the configured sensors
789 
790 // eOmc_inertial_commands_t startCommand = {0};
791 // startCommand.enable = 1;
792 
793 // id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_cmmnds_enable);
794 // if(false == res->setRemoteValue(id32, (uint8_t*) &startCommand))
795 // {
796 // yError() << "ethResources::sendConfig2SkinInertial() fails to command the start transmission of the inertials";
797 // return false;
798 // }
799 
800  return true;
801 #endif
802 }
803 
804 
805 #if 0
806 eOas_inertial1_position_t embObjAnalogSensor::getLocationOfInertialSensor(std::string &strpos)
807 {
808  eOas_inertial1_position_t ret = eoas_inertial1_pos_none;
809 
810  if(strpos == "none")
811  {
812  ret = eoas_inertial1_pos_none;
813  }
814 
815  else if(strpos == "l_hand")
816  {
817  ret = eoas_inertial1_pos_l_hand;
818  }
819  else if(strpos == "l_forearm_1")
820  {
821  ret = eoas_inertial1_pos_l_forearm_1;
822  }
823  else if(strpos == "l_forearm_2")
824  {
825  ret = eoas_inertial1_pos_l_forearm_2;
826  }
827  else if(strpos == "l_upper_arm_1")
828  {
829  ret = eoas_inertial1_pos_l_upper_arm_1;
830  }
831  else if(strpos == "l_upper_arm_2")
832  {
833  ret = eoas_inertial1_pos_l_upper_arm_2;
834  }
835  else if(strpos == "l_upper_arm_3")
836  {
837  ret = eoas_inertial1_pos_l_upper_arm_3;
838  }
839  else if(strpos == "l_upper_arm_4")
840  {
841  ret = eoas_inertial1_pos_l_upper_arm_4;
842  }
843  else if(strpos == "l_foot_1")
844  {
845  ret = eoas_inertial1_pos_l_foot_1;
846  }
847  else if(strpos == "l_foot_2")
848  {
849  ret = eoas_inertial1_pos_l_foot_2;
850  }
851  else if(strpos == "l_lower_leg_1")
852  {
853  ret = eoas_inertial1_pos_l_lower_leg_1;
854  }
855  else if(strpos == "l_lower_leg_2")
856  {
857  ret = eoas_inertial1_pos_l_lower_leg_2;
858  }
859  else if(strpos == "l_lower_leg_3")
860  {
861  ret = eoas_inertial1_pos_l_lower_leg_3;
862  }
863  else if(strpos == "l_lower_leg_4")
864  {
865  ret = eoas_inertial1_pos_l_lower_leg_4;
866  }
867  else if(strpos == "l_upper_leg_1")
868  {
869  ret = eoas_inertial1_pos_l_upper_leg_1;
870  }
871  else if(strpos == "l_upper_leg_2")
872  {
873  ret = eoas_inertial1_pos_l_upper_leg_2;
874  }
875  else if(strpos == "l_upper_leg_3")
876  {
877  ret = eoas_inertial1_pos_l_upper_leg_3;
878  }
879  else if(strpos == "l_upper_leg_4")
880  {
881  ret = eoas_inertial1_pos_l_upper_leg_4;
882  }
883  else if(strpos == "l_upper_leg_5")
884  {
885  ret = eoas_inertial1_pos_l_upper_leg_5;
886  }
887  else if(strpos == "l_upper_leg_6")
888  {
889  ret = eoas_inertial1_pos_l_upper_leg_6;
890  }
891  else if(strpos == "l_upper_leg_7")
892  {
893  ret = eoas_inertial1_pos_l_upper_leg_7;
894  }
895 
896  else if(strpos == "r_hand")
897  {
898  ret = eoas_inertial1_pos_r_hand;
899  }
900  else if(strpos == "r_forearm_1")
901  {
902  ret = eoas_inertial1_pos_r_forearm_1;
903  }
904  else if(strpos == "r_forearm_2")
905  {
906  ret = eoas_inertial1_pos_r_forearm_2;
907  }
908  else if(strpos == "r_upper_arm_1")
909  {
910  ret = eoas_inertial1_pos_r_upper_arm_1;
911  }
912  else if(strpos == "r_upper_arm_2")
913  {
914  ret = eoas_inertial1_pos_r_upper_arm_2;
915  }
916  else if(strpos == "r_upper_arm_3")
917  {
918  ret = eoas_inertial1_pos_r_upper_arm_3;
919  }
920  else if(strpos == "r_upper_arm_4")
921  {
922  ret = eoas_inertial1_pos_r_upper_arm_4;
923  }
924  else if(strpos == "r_foot_1")
925  {
926  ret = eoas_inertial1_pos_r_foot_1;
927  }
928  else if(strpos == "r_foot_2")
929  {
930  ret = eoas_inertial1_pos_r_foot_2;
931  }
932  else if(strpos == "r_lower_leg_1")
933  {
934  ret = eoas_inertial1_pos_r_lower_leg_1;
935  }
936  else if(strpos == "r_lower_leg_2")
937  {
938  ret = eoas_inertial1_pos_r_lower_leg_2;
939  }
940  else if(strpos == "r_lower_leg_3")
941  {
942  ret = eoas_inertial1_pos_r_lower_leg_3;
943  }
944  else if(strpos == "r_lower_leg_4")
945  {
946  ret = eoas_inertial1_pos_r_lower_leg_4;
947  }
948  else if(strpos == "r_upper_leg_1")
949  {
950  ret = eoas_inertial1_pos_r_upper_leg_1;
951  }
952  else if(strpos == "r_upper_leg_2")
953  {
954  ret = eoas_inertial1_pos_r_upper_leg_2;
955  }
956  else if(strpos == "r_upper_leg_3")
957  {
958  ret = eoas_inertial1_pos_r_upper_leg_3;
959  }
960  else if(strpos == "r_upper_leg_4")
961  {
962  ret = eoas_inertial1_pos_r_upper_leg_4;
963  }
964  else if(strpos == "r_upper_leg_5")
965  {
966  ret = eoas_inertial1_pos_r_upper_leg_5;
967  }
968  else if(strpos == "r_upper_leg_6")
969  {
970  ret = eoas_inertial1_pos_r_upper_leg_6;
971  }
972  else if(strpos == "r_upper_leg_7")
973  {
974  ret = eoas_inertial1_pos_r_upper_leg_7;
975  }
976 
977 
978  return(ret);
979 
980 }
981 #endif
982 
983 //#warning --> marco.accame: review function embObjAnalogSensor::getFullscaleValues() as in comment below
984 // 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
985 // 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.
986 bool embObjAnalogSensor::getFullscaleValues()
987 {
988  // marco.accame on 11 apr 2014:
989  // 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
990  // the initialisation is not specialised and is ... all zeros. if the user wants to init to proper values must redefine the relevant INIT funtion.
991  // in this case, the eoprot_fun_INIT_as_strain_status_fullscale().
992  // extern void eoprot_fun_INIT_as_strain_status_fullscale(const EOnv* nv)
993  // {
994  // eOas_arrayofupto12bytes_t fullscale_values = {0};
995  // eo_array_New(6, 2, &fullscale_values); // itemsize = 2, capacity = 6
996  // eo_nv_Set(nv, &fullscale_values, eobool_true, eo_nv_upd_dontdo);
997  // }
998  // 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.
999 
1000 
1001 
1002  // Check initial size of array... it should be zero.
1003  bool gotFullScaleValues = false;
1004  int timeout, NVsize;
1005  EOnv tmpNV;
1006  EOnv *p_tmpNV = NULL;
1007  eOas_arrayofupto12bytes_t fullscale_values = {0};
1008  // force it to be an empty array of itemsize 2 and capacity 6.
1009  // 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
1010  eo_array_New(6, 2, &fullscale_values);
1011 
1012  eOprotID32_t protoid_fullscale = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_status_fullscale);
1013 
1014 
1015  // at first we impose that the local value of fullscales is zero.
1016  // we also force the change because this variable is readonly
1017  const bool overrideROprotection = true;
1018  res->setLocalValue(protoid_fullscale, &fullscale_values, overrideROprotection);
1019 
1020 
1021  // Prepare analog sensor
1022  eOas_strain_config_t strainConfig = {0};
1023  strainConfig.datarate = _period;
1024  strainConfig.mode = eoas_strainmode_acquirebutdonttx;
1025  strainConfig.signaloncefullscale = eobool_true;
1026 
1027  eOprotID32_t protoid_strain_config = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config);
1028 
1029  timeout = 5;
1030 
1031  // wait for response
1032  while(!gotFullScaleValues && (timeout != 0))
1033  {
1034  res->setRemoteValue(protoid_strain_config, &strainConfig);
1035  SystemClock::delaySystem(1.0);
1036  // read fullscale values
1037  res->getLocalValue(protoid_fullscale, &fullscale_values);
1038  // If data arrives, size is bigger than zero
1039  //#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!
1040  NVsize = eo_array_Size((EOarray *)&fullscale_values);
1041 
1042  if(0 != NVsize)
1043  {
1044  gotFullScaleValues = true;
1045  break;
1046  }
1047 
1048  timeout--;
1049  if(verbosewhenok)
1050  {
1051  yWarning() << "embObjAnalogSensor::getFullscaleValues(): for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << ": full scale val not arrived yet... retrying in 1 sec";
1052  }
1053  }
1054 
1055  if((false == gotFullScaleValues) && (0 == timeout))
1056  {
1057  yError() << "embObjAnalogSensor::getFullscaleValues(): ETH Analog sensor: request for calibration parameters timed out for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
1058  return false;
1059  }
1060 
1061  if((NVsize != _channels))
1062  {
1063  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";
1064  return false;
1065  }
1066 
1067  uint8_t *msg;
1068 
1069 
1070  if(gotFullScaleValues)
1071  {
1072  if(verbosewhenok)
1073  {
1074  yWarning() << "embObjAnalogSensor::getFullscaleValues() detected that already has full scale values for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString;
1075  yDebug() << "embObjAnalogSensor::getFullscaleValues(): Fullscale values for BOARD" << res->getProperties().boardnameString << "IP" << res->getProperties().ipv4addrString << "are: size=" << eo_array_Size((EOarray *)&fullscale_values) << " numchannel=" << _channels;
1076  }
1077 
1078  for(int i=0; i<_channels; i++)
1079  {
1080  // Get the k-th element of the array as a 2 bytes msg
1081  msg = (uint8_t *) eo_array_At((EOarray *) &fullscale_values, i);
1082  if(NULL == msg)
1083  {
1084  yError() << "I don't receive data for channel " << i;
1085  return false;
1086  }
1087  // Got from CanBusMotionControl... here order of bytes seems inverted with respect to calibratedValues or uncalibratedValues (see callback)
1088  scaleFactor[i]= 0;
1089  scaleFactor[i]= ((uint16_t)(msg[0]<<8) | msg[1]);
1090  //scaleFactor[i]=i;
1091  //yError() << " scale factor[" << i << "] = " << scaleFactor[i];
1092  if(verbosewhenok)
1093  {
1094  yDebug() << "embObjAnalogSensor::getFullscaleValues(): channel " << i << "full scale value " << scaleFactor[i];
1095  }
1096  }
1097  }
1098 
1099  return true;
1100 }
1101 
1102 bool embObjAnalogSensor::init()
1103 {
1104  yTrace();
1105 
1106  // - configure regular rops
1107  vector<eOprotID32_t> id32v(0);
1108  eOprotID32_t protoid = eo_prot_ID32dummy;
1109  eOmn_serv_category_t servcategory = eomn_serv_category_none;
1110 
1111  // we need to choose the protoid to put inside the vector
1112 
1113  switch(_as_type)
1114  {
1115  case AS_Type_MAIS:
1116  {
1117  servcategory = eomn_serv_category_mais;
1118 
1119  protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_mais, 0, eoprot_tag_as_mais_status_the15values);
1120 
1121  } break;
1122 
1123  case AS_Type_STRAIN:
1124  {
1125  servcategory = eomn_serv_category_strain;
1126 
1127  if(_useCalibration)
1128  protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_status_calibratedvalues);
1129  else
1130  protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_status_uncalibratedvalues);
1131  } break;
1132 
1133  case AS_Type_INERTIAL_MTB:
1134  {
1135  servcategory = eomn_serv_category_inertials;
1136  protoid = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_status);
1137  } break;
1138 
1139  default:
1140  {
1141  yError() << "embObjAnalogSensor: unknown device, cannot set regular ROPs";
1142  protoid = eo_prot_ID32dummy;
1143  return false;
1144  }
1145  }
1146 
1147  // put it inside vector
1148  id32v.push_back(protoid);
1149 
1150 
1151  if(false == res->serviceSetRegulars(servcategory, id32v))
1152  {
1153  yError() << "embObjAnalogSensor::init() fails to add its variables to regulars: cannot proceed any further";
1154  return false;
1155  }
1156  else
1157  {
1158  if(verbosewhenok)
1159  {
1160  yDebug() << "embObjAnalogSensor::init() added" << id32v.size() << "regular rops to BOARD" << res->getProperties().boardnameString << "with IP" << res->getProperties().ipv4addrString;
1161  char nvinfo[128];
1162  for (size_t r = 0; r<id32v.size(); r++)
1163  {
1164  uint32_t id32 = id32v.at(r);
1165  eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
1166  yDebug() << "\t it added regular rop for" << nvinfo;
1167  }
1168  }
1169  }
1170 
1171  SystemClock::delaySystem(0.005); // 5 ms (m.a.a-delay: before it was 0)
1172 
1173 
1174  return true;
1175 }
1176 
1177 
1182 int embObjAnalogSensor::read(yarp::sig::Vector &out)
1183 {
1184  // This method gives analogdata to the analogServer
1185 
1186  std::lock_guard<std::mutex> lck(mtx);
1187 
1188  if (!analogdata)
1189  {
1190  return false;
1191  }
1192 
1193  // errors are not handled for now... it'll always be OK!!
1194  if (status!=IAnalogSensor::AS_OK)
1195  {
1196  switch (status)
1197  {
1198  case IAnalogSensor::AS_OVF:
1199  {
1200  counterSat++;
1201  } break;
1202  case IAnalogSensor::AS_ERROR:
1203  {
1204  counterError++;
1205  } break;
1206  case IAnalogSensor::AS_TIMEOUT:
1207  {
1208  counterTimeout++;
1209  } break;
1210  default:
1211  {
1212  counterError++;
1213  } break;
1214  }
1215  return status;
1216  }
1217 
1218  out.resize(analogdata->size());
1219  for(int k=0;k<analogdata->size();k++)
1220  {
1221  out[k]=(*analogdata)[k];
1222  }
1223 
1224  return status;
1225 }
1226 
1227 
1228 void embObjAnalogSensor::resetCounters()
1229 {
1230  counterSat=0;
1231  counterError=0;
1232  counterTimeout=0;
1233 }
1234 
1235 
1236 void embObjAnalogSensor::getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
1237 {
1238  sat=counterSat;
1239  err=counterError;
1240  to=counterTimeout;
1241 }
1242 
1243 
1245 {
1246  printf("getstate\n");
1247  return AS_OK;
1248 }
1249 
1251 {
1252  return analogdata->size();
1253 }
1254 
1256 {
1257  return AS_OK;
1258 }
1259 
1260 int embObjAnalogSensor::calibrateSensor(const yarp::sig::Vector& value)
1261 {
1262  return AS_OK;
1263 }
1264 
1266 {
1267  return AS_OK;
1268 }
1269 
1271 {
1272  return AS_OK;
1273 }
1274 
1276 {
1278 
1279  switch(_as_type)
1280  {
1281  case AS_Type_MAIS:
1282  {
1284  } break;
1285 
1286  case AS_Type_STRAIN:
1287  {
1289  } break;
1290 
1291  case AS_Type_INERTIAL_MTB:
1292  {
1294  } break;
1295 
1296  default:
1297  {
1298  ret = eth::iethres_none;
1299  }
1300  }
1301 
1302  return ret;
1303 }
1304 
1305 bool embObjAnalogSensor::update(eOprotID32_t id32, double timestamp, void* rxdata)
1306 {
1307  bool ret;
1308 
1309 //#warning --> marco.accame: retrieve the entity from id32 and see is it is mais or strain or inertial.
1310  switch(_as_type)
1311  {
1312  case AS_Type_MAIS:
1313  {
1314  ret = fillDatOfMais(rxdata);
1315  } break;
1316 
1317  case AS_Type_STRAIN:
1318  {
1319  ret = fillDatOfStrain(rxdata);
1320  } break;
1321 
1322  case AS_Type_INERTIAL_MTB:
1323  {
1324  ret = fillDatOfInertial(rxdata);
1325  } break;
1326 
1327  default:
1328  {
1329  //i should not be here. if AS_Type_NONE then i should get error in fromConfig function
1330  ret = false;
1331  }
1332  }
1333 
1334  return ret;
1335 }
1336 
1337 
1338 
1339 bool embObjAnalogSensor::fillDatOfStrain(void *as_array_raw)
1340 {
1341  // called by embObjAnalogSensor::fillData() which is called by handle_AS_data() which is called by handle_data() which is called by:
1342  // eoprot_fun_UPDT_as_strain_status_calibratedvalues() or eoprot_fun_UPDT_as_strain_status_uncalibratedvalues()
1343  // the void* parameter inside this function is a eOas_arrayofupto12bytes_t*
1344  // and can be treated as a EOarray
1345  EOarray *array = (EOarray*)as_array_raw;
1346  uint8_t size = eo_array_Size(array);
1347  uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 2, as the code after uses this convention
1348  if(0 == size)
1349  {
1350  return false;
1351  }
1352 
1353  // lock analogdata
1354  std::lock_guard<std::mutex> lck(mtx);
1355 
1356  double *_buffer = this->analogdata->getBuffer();
1357 
1358  if(NULL == _buffer)
1359  {
1360  return false;
1361  }
1362 
1363 
1364  for(int k=0; k<_channels; k++)
1365  {
1366  // Get the kth element of the array as a 2 bytes msg
1367  char* tmp = (char*) eo_array_At(array, k);
1368  // marco.accame: i am nervous about iterating for _channels instead of size of array....
1369  // thus i add a protection. if k goes beyond size of array, eo_array_At() returns NULL.
1370  if(NULL != tmp)
1371  {
1372  uint8_t msg[2] = {0};
1373  memcpy(msg, tmp, 2);
1374  // Got from canBusMotionControl
1375  _buffer[k]= (short)( ( (((unsigned short)(msg[1]))<<8)+msg[0]) - (unsigned short) (0x8000) );
1376 
1377  if (_useCalibration == 1)
1378  {
1379  _buffer[k]=_buffer[k]*scaleFactor[k]/float(0x8000);
1380  }
1381  }
1382  }
1383 
1384  return true;
1385 }
1386 
1387 
1388 bool embObjAnalogSensor::fillDatOfMais(void *as_array_raw)
1389 {
1390  // called by embObjAnalogSensor::fillData() which is called by handle_AS_data() which is called by handle_data() which is called by:
1391  // eoprot_fun_UPDT_as_mais_status_the15values()
1392  // the void* parameter inside this function is a eOas_arrayofupto36bytes_t*
1393  // and can be treated as a EOarray
1394 
1395  EOarray *array = (EOarray*)as_array_raw;
1396  uint8_t size = eo_array_Size(array);
1397  uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 1, as the code after uses this convention
1398  if(0 == size)
1399  {
1400  return false;
1401  }
1402 
1403  std::lock_guard<std::mutex> lck(mtx);
1404 
1405  double *_buffer = this->analogdata->getBuffer();
1406 
1407  if(NULL == _buffer)
1408  {
1409  return false;
1410  }
1411 
1412  //NOTE: here i suppose that size of array is equal to num of channel or to 0 if sensor did not sent something
1413  //this is an agreement with firmware.
1414  for(int k=0; k<size; k++)
1415  {
1416  uint8_t val = *((uint8_t*)eo_array_At(array, k)); // marco.accame: i see that we treat the array as if containing items of 1 byte.
1417  // Get the kth element of the array
1418  _buffer[k] = (double)val;
1419  }
1420 
1421  return true;
1422 }
1423 
1424 
1425 bool embObjAnalogSensor::fillDatOfInertial(void *inertialdata)
1426 {
1427  eOas_inertial_status_t *status = (eOas_inertial_status_t*) inertialdata;
1428 
1429  return true;
1430 
1431 #if 0
1432 
1433  if((eoas_inertial_pos_none == status->data.position) || (status->data.position >= eoas_inertial_pos_max_numberof))
1434  { // we dont have any info to manage or the received position is WRONG
1435  return(true);
1436  }
1437 
1438  std::lock_guard<std::mutex> lck(mtx);
1439 
1440  double *_buffer = this->analogdata->getBuffer();
1441 
1442  if(NULL == _buffer)
1443  {
1444  return false;
1445  }
1446 
1447 
1448  uint8_t dataindex = 255;
1449 
1450  if(eoas_inertial_type_accelerometer == status->data.type)
1451  {
1452  dataindex = _fromInertialPos2DataIndexAccelerometers[status->data.position];
1453  }
1454  else if(eoas_inertial_type_gyroscope == status->data.type)
1455  {
1456  dataindex = _fromInertialPos2DataIndexGyroscopes[status->data.position];
1457  }
1458 
1459 
1460 
1461  if(255 != dataindex)
1462  {
1463  // we now use dataindex to offset the array.
1464  uint8_t firstpos = 2 + dataindex*6;
1465 
1466  _buffer[firstpos+0] = (double) status->data.position;
1467  _buffer[firstpos+1] = (double) status->data.type;
1468  _buffer[firstpos+2] = (double) status->data.timestamp;
1469 
1470  _buffer[firstpos+3] = (double) status->data.x;
1471  _buffer[firstpos+4] = (double) status->data.y;
1472  _buffer[firstpos+5] = (double) status->data.z;
1473  }
1474 
1475  return true;
1476 #endif
1477 }
1478 
1480 {
1481  cleanup();
1482  return true;
1483 }
1484 
1485 void embObjAnalogSensor::cleanup(void)
1486 {
1487  if(ethManager == NULL) return;
1488 
1489  int ret = ethManager->releaseResource2(res, this);
1490  res = NULL;
1491  if(ret == -1)
1492  ethManager->killYourself();
1493 }
1494 
1495 // eof
1496 
double * getBuffer()
virtual const Properties & getProperties()=0
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 setRemoteValue(const eOprotID32_t id32, void *value)=0
virtual bool verifyEPprotocol(eOprot_endpoint_t ep)=0
virtual bool getLocalValue(const eOprotID32_t id32, void *value)=0
virtual bool setLocalValue(eOprotID32_t id32, const void *value, bool overrideROprotection=false)=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
int releaseResource2(eth::AbstractEthResource *ethresource, IethResource *interface)
Definition: ethManager.cpp:409
bool verifyEthBoardInfo(yarp::os::Searchable &cfgtotal, eOipv4addr_t &boardipv4, string boardipv4string, string boardname)
Definition: ethManager.cpp:283
static bool killYourself()
Definition: ethManager.cpp:178
static TheEthManager * instance()
Definition: ethManager.cpp:159
eth::AbstractEthResource * requestResource2(IethResource *interface, yarp::os::Searchable &cfgtotal)
Definition: ethManager.cpp:336
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
bool open(yarp::os::Searchable &config)
virtual int calibrateChannel(int ch, double v)
virtual eth::iethresType_t type()
virtual int read(yarp::sig::Vector &out)
Read a vector from the sensor.
bool NOT_YET_IMPLEMENTED(const char *txt)
const dReal * pos
Definition: iCub_Sim.cpp:62
static int v
Definition: iCub_Sim.cpp:42
double sat(const double val, const double min, const double max)
Definition: utils.h:183
iethresType_t
Definition: IethResource.h:61
@ iethres_analogmais
Definition: IethResource.h:64
@ iethres_analoginertial
Definition: IethResource.h:69
@ iethres_none
Definition: IethResource.h:62
@ iethres_analogstrain
Definition: IethResource.h:65
Copyright (C) 2008 RobotCub Consortium.
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
out
Definition: sine.m:8