iCub-main
eo_imu_privData.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  * Author: Valentina Gaggero
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 #include "eo_imu_privData.h"
9 #include <stdexcept>
10 
11 #include "EoProtocolAS.h"
12 #include "EOnv_hid.h"
13 #include "EOconstarray.h"
14 
15 using namespace yarp::dev;
16 
17 
18 
20 {
21  memset(canpositionmap, 0xff, sizeof(canpositionmap));
22 }
23 
25 
27 {
28  std::uint8_t numberof[eoas_sensors_numberof];
29  memset(numberof, 0, sizeof(numberof));
30 
31  // now we list the service config to fill the map withe proper indices.
32 
33  const eOas_inertial3_arrayof_descriptors_t* tmp = &servCfg.ethservice.configuration.data.as.inertial3.arrayofdescriptor;
34  EOconstarray* array = eo_constarray_Load(reinterpret_cast<const EOarray*>(tmp));
35  uint8_t size = eo_constarray_Size(array);
36  for(uint8_t i=0; i<size; i++)
37  {
38  eOas_inertial3_descriptor_t *des = (eOas_inertial3_descriptor_t*)eo_constarray_At(array, i);
39  if(nullptr != des)
40  {
41  // use des.
42  if(des->typeofsensor < eoas_sensors_numberof)
43  {
44  if(des->on.any.place == eobrd_place_can)
45  {
46  canpositionmap[des->typeofsensor][des->on.can.port][des->on.can.addr] = numberof[des->typeofsensor];
47  numberof[des->typeofsensor]++;
48  }
49  else if(des->on.any.place == eobrd_place_eth)
50  {
51  ethpositionmap[des->typeofsensor] = numberof[des->typeofsensor];
52  numberof[des->typeofsensor]++;
53  }
54  }
55  }
56  else
57  {
58  return false;
59  }
60  }
61  return true;
62 }
63 
64 bool PositionMaps::getIndex(const eOas_inertial3_data_t* data, uint8_t& index, eOas_sensor_t& type)
65 {
66  uint8_t canbus, canaddress;
67  type = static_cast<eOas_sensor_t>(data->typeofsensor);
68  if (type != eoas_gyros_st_l3g4200d) {
69  if (!getCanAddress(data, canbus, canaddress))
70  return false;
71  }
72  if(data->typeofsensor >= eoas_sensors_numberof)
73  { // it is not a valid index
74  return false;
75  }
76  if(type == eoas_gyros_st_l3g4200d) {
77  index = ethpositionmap[data->typeofsensor];
78  }
79  else {
80  index = canpositionmap[data->typeofsensor][canbus][canaddress];
81  }
82 
83  return (0xff == index) ? false : true;
84 }
85 
86 bool PositionMaps::getIndex(eOas_sensor_t type, uint8_t canbus, uint8_t canaddress, uint8_t& index)
87 {
88  // It is the ems, it is an eth board
89  if(type == eoas_gyros_st_l3g4200d) {
90  return false;
91  }
92  if(canbus >= eOcanports_number)
93  {
94  return false;
95  }
96 
97  if(canaddress > 0x0f)
98  return false;
99 
100  index = canpositionmap[type][canbus][canaddress];
101  return (0xff == index) ? false : true;
102 }
103 
104 
105 bool PositionMaps::getCanAddress(const eOas_inertial3_data_t *data, uint8_t &canbus, uint8_t &canaddress)
106 {
107  if(nullptr == data)
108  return false;
109  auto type = static_cast<eOas_sensor_t>(data->typeofsensor);
110  // It is the ems, it is an eth board
111  if (eoas_gyros_st_l3g4200d == type) {
112  return false;
113  }
114  canbus = data->id >> 4;
115 
116  if(canbus >= eOcanports_number)
117  {
118  return false;
119  }
120 
121  canaddress = data->id & 0x0f;
122 
123  return true;
124 }
125 
126 
128 {
129  errorstring = "embObjIMU";
130 
131  mysens.resize(eoas_sensors_numberof);
132  for(int t=0; t<eoas_sensors_numberof; t++)
133  { mysens[t].resize(0); }
134 }
135 
136 
137 void SensorsData::init(servConfigImu_t &servCfg, string error_string)
138 {
139  errorstring = error_string;
140 
141  const eOas_inertial3_arrayof_descriptors_t* tmp = &servCfg.ethservice.configuration.data.as.inertial3.arrayofdescriptor;
142  EOconstarray* array = eo_constarray_Load(reinterpret_cast<const EOarray*>(tmp));
143  uint8_t size = eo_constarray_Size(array);
144  for(uint8_t i=0; i<size; i++)
145  {
146  eOas_inertial3_descriptor_t *des = (eOas_inertial3_descriptor_t*)eo_constarray_At(array, i);
147  if(nullptr != des)
148  {
149  // use des.
150  if(des->typeofsensor < eoas_sensors_numberof)
151  {
152  sensorInfo_t newSensor;
153  if (servCfg.sensorName[i].empty()) {
154  newSensor.name = servCfg.id[i];
155  }
156  else {
157  newSensor.name = servCfg.sensorName[i];
158  }
159  newSensor.framename = newSensor.name;
160  switch (des->typeofsensor) {
161  case eoas_imu_acc:
162  newSensor.conversionFactor = 100.0; // 1 m/sec2 = 100 binary units
163  break;
164  case eoas_imu_mag:
165  newSensor.conversionFactor = 16.0 * 1000000.0; // 1 microT = 16 binary units
166  break;
167  case eoas_imu_gyr:
168  newSensor.conversionFactor = 16.0; // 1 degree/sec = 16 binary units
169  case eoas_imu_eul:
170  newSensor.conversionFactor = 16.0; // 1 degree = 16 binary units
171  }
172  if(des->typeofsensor == eoas_imu_qua)
173  newSensor.values.resize(4);
174  else
175  newSensor.values.resize(3);
176  newSensor.state = 0;
177  mysens[des->typeofsensor].push_back(newSensor);
178  }
179  }
180  }
181 }
182 
183 bool SensorsData::outOfRangeErrorHandler(const std::out_of_range& oor) const
184 {
185  yError() << errorstring.c_str() << "Out of Range error: " << oor.what();
186  return false;
187 }
188 
189 size_t SensorsData::getNumOfSensors(eOas_sensor_t type) const
190 {
191  std::lock_guard<std::mutex> lck (mutex);
192  return mysens[type].size();
193 }
194 
195 uint8_t SensorsData::getSensorStatus(size_t sens_index, eOas_sensor_t type) const
196 {
197  try
198  {
199  std::lock_guard<std::mutex> lck (mutex);
200 // sensorInfo_t info = mysens[type].at(sens_index);
201 // return info.state;
202  return mysens[type].at(sens_index).state;
203  }
204  catch (const std::out_of_range& oor)
205  {
207  return yarp::dev::MAS_ERROR;
208  }
209 
210 }
211 bool SensorsData::getSensorName(size_t sens_index, eOas_sensor_t type, std::string &name) const
212 {
213  try
214  {
215  std::lock_guard<std::mutex> lck (mutex);
216  name = mysens[type].at(sens_index).name;
217  return true;
218  }
219  catch (const std::out_of_range& oor)
220  {
221  return outOfRangeErrorHandler(oor);
222  }
223 
224 }
225 bool SensorsData::getSensorFrameName(size_t sens_index, eOas_sensor_t type, std::string &frameName) const
226 {
227  try
228  {
229  std::lock_guard<std::mutex> lck (mutex);
230  frameName = mysens[type].at(sens_index).framename;
231  return true;
232  }
233  catch (const std::out_of_range& oor)
234  {
235  return outOfRangeErrorHandler(oor);
236  }
237 }
238 
239 
240 bool SensorsData::getSensorMeasure(size_t sens_index, eOas_sensor_t type, yarp::sig::Vector& out, double& timestamp) const
241 {
242  try
243  { std::lock_guard<std::mutex> lck (mutex);
244  out = mysens[type].at(sens_index).values;
245  timestamp = mysens[type].at(sens_index).timestamp;
246  }
247  catch (const std::out_of_range& oor)
248  {
249  return outOfRangeErrorHandler(oor);
250  }
251  return true;
252 
253 }
254 
255 bool SensorsData::update(eOas_sensor_t type, uint8_t index, eOas_inertial3_data_t *newdata)
256 {
257  std::lock_guard<std::mutex> lck (mutex);
258 
259  sensorInfo_t *info = &(mysens[type][index]);
260 
261  info->values[0] = newdata->x / info->conversionFactor;
262  info->values[1] = newdata->y / info->conversionFactor;
263  info->values[2] = newdata->z / info->conversionFactor;
264  info->timestamp = yarp::os::Time::now();
265 
266  return true;
267 
268 }
269 
270 bool SensorsData::updateStatus(eOas_sensor_t type, uint8_t index, eOas_inertial3_sensorstatus_t &status)
271 {
272  std::lock_guard<std::mutex> lck (mutex);
273 
274  sensorInfo_t *info = &(mysens[type][index]);
275 
276  switch(type)
277  {
278  case eoas_imu_acc:
279  case eoas_accel_mtb_int:
280  case eoas_accel_mtb_ext:
281  info->state = status.calib.acc;
282  break;
283 
284  case eoas_imu_mag:
285  info->state = status.calib.mag;
286  break;
287 
288  case eoas_imu_gyr:
289  case eoas_gyros_st_l3g4200d:
290  case eoas_gyros_mtb_ext:
291  info->state = status.calib.gyr;
292  break;
293  case eoas_imu_eul:
294  case eoas_imu_qua:
295  case eoas_imu_lia:
296  info->state = status.calib.acc + status.calib.gyr + status.calib.mag;
297  break;
298  default:
299  info->state = status.general;
300  break;
301  }
302 
303  //yError() << "UPDATE STATUS OF SENSOR " << index << "with type "<< eoas_sensor2string(type) <<"with value " << status.general <<"info.state= " << info->state;
304  return true;
305 
306 }
307 
309 {;}
311 {;}
312 
313 bool eo_imu_privData::fromConfig(yarp::os::Searchable &config, servConfigImu_t &servCfg)
314 {
315  ServiceParser* parser = new ServiceParser;
316  bool ret = parser->parseService(config, servCfg);
317  delete parser;
318  if(!ret)
319  yError() << "embObjIMU" << getBoardInfo() << ": missing some configuration parameter. Check logs and your config file.";
320  return ret;;
321 }
322 
324 {
325  eOas_inertial3_config_t cfg ={0};
326  cfg.datarate = servCfg.acquisitionrate;
327  cfg.enabled = 0;
328 
329  for(size_t i=0; i<servCfg.inertials.size(); i++)
330  {
331  eo_common_word_bitset(&cfg.enabled, i);
332  }
333 
334  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial3, 0, eoprot_tag_as_inertial3_config);
335  if(false == res->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
336  {
337  yError() << "embObjIMU" << getBoardInfo() << "FATAL: sendConfing2board() had an error while calling setcheckRemoteValue() for config the board";
338  return false;
339  }
340  else
341  {
343  {
344  yError() << "embObjIMU" << getBoardInfo() << "sendConfing2board() correctly configured enabled sensors with period" << cfg.datarate;
345  }
346  }
347 
348  return true;
349 }
350 
351 // configure regular rops
353 {
354  vector<eOprotID32_t> id32v(0);
355  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial3, 0, eoprot_tag_as_inertial3_status);
356  // put it inside vector
357  id32v.push_back(id32);
358 
359  if(false == res->serviceSetRegulars(eomn_serv_category_inertials3, id32v))
360  {
361  yError() << "embObjIMU" << getBoardInfo() <<"initRegulars() fails to add its variables to regulars: cannot proceed any further";
362  return false;
363  }
364  else
365  {
366  if(isVerbose())
367  {
368  yError() << "embObjIMU" << getBoardInfo() <<"initRegulars() added" << id32v.size() << " regular rops to the board";
369  char nvinfo[128];
370  for (size_t r = 0; r<id32v.size(); r++)
371  {
372  uint32_t item = id32v.at(r);
373  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
374  yDebug() << "\t it added regular rop for" << nvinfo;
375  }
376  }
377  }
378 
379  return true;
380 }
381 
382 
383 
384 yarp::dev::MAS_status eo_imu_privData::sensorState_eo2yarp(eOas_sensor_t type, uint8_t eo_state)
385 {
386  debugPrintStateNotOK(type, eo_state);
387  //In the future I'll put here a translation of state from firmware to yarp interface.
388  //Currently I return always ok for testing porpouse
389 
390 
391  return yarp::dev::MAS_OK;
392 }
393 
394 
395 #undef DEBUG_PRINT_STATE
396 #undef DEBUG_PRINT_STATE_FULL
397 
398 void eo_imu_privData::debugPrintStateNotOK(eOas_sensor_t type, uint8_t eo_state)
399 {
400  static int count[eoas_sensors_numberof]={0};
401 
402  count[type]++;
403  if(count[type]< 100) //the print there is about every 1 second
404  return;
405 
406 #if defined(DEBUG_PRINT_STATE)
407  switch(type)
408  {
409  case eoas_imu_acc:
410  case eoas_imu_mag:
411  case eoas_imu_gyr:
412  if(0 == eo_state)
413  {
414  yError() << getBoardInfo() << "sensor " << eoas_sensor2string(type) << "is not calibrated because its calib status reg is =" << eo_state<< "(reg: 0 = not calib, 3 = fully calib)";
415  }
416 #if defined(DEBUG_PRINT_STATE_FULL)
417  else if(1 == eo_state)
418  {
419  yWarning() << getBoardInfo() << "sensor " << eoas_sensor2string(type) << "is poorly calibrated because its calib status reg is =" << eo_state<< "(reg: 0 = not calib, 3 = fully calib)";
420  }
421  else
422  {
423  yInfo() << getBoardInfo() << "sensor " << eoas_sensor2string(type) << "is calibrated because its calib status reg is =" << eo_state<< "(reg: 0 = not calib, 3 = fully calib)";
424  }
425 #endif
426  break;
427  case eoas_imu_eul:
428  case eoas_imu_qua:
429  case eoas_imu_lia:
430  if(eo_state < 3)
431  {
432  yError() << getBoardInfo() << "sensor " << eoas_sensor2string(type) << "is not calibrated because sum of the 3 calib status regs is =" << eo_state<< "(reg: 0 = not calib, 3=fully calib)";
433  }
434 #if defined(DEBUG_PRINT_STATE_FULL)
435  else if(eo_state < 6)
436  {
437  yWarning() << getBoardInfo() << "sensor " << eoas_sensor2string(type) << "is poorly calibrated because sum of the 3 calib status regs is =" << eo_state<< "(reg: 0 = not calib, 3 = fully calib)";
438  }
439  else
440  {
441  yInfo() << getBoardInfo() << "sensor " << eoas_sensor2string(type) << "is calibrated because sum of the 3 calib status regs is =" << eo_state<< "(reg: 0 = not calib, 3 = fully calib)";
442  }
443 #endif
444  break;
445  }
446 #endif //defined(DEBUG_PRINT_STATE)
447 
448  count[type]=0;
449 
450 }
@ data
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 serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
bool init(servConfigImu_t &servCfg)
bool getIndex(const eOas_inertial3_data_t *data, uint8_t &index, eOas_sensor_t &type)
static bool getCanAddress(const eOas_inertial3_data_t *data, uint8_t &canbus, uint8_t &canaddress)
bool outOfRangeErrorHandler(const std::out_of_range &oor) const
void init(servConfigImu_t &servCfg, string error_string)
bool getSensorFrameName(size_t sens_index, eOas_sensor_t type, std::string &frameName) const
bool getSensorMeasure(size_t sens_index, eOas_sensor_t type, yarp::sig::Vector &out, double &timestamp) const
bool getSensorName(size_t sens_index, eOas_sensor_t type, std::string &name) const
bool updateStatus(eOas_sensor_t type, uint8_t index, eOas_inertial3_sensorstatus_t &status)
bool update(eOas_sensor_t type, uint8_t index, eOas_inertial3_data_t *newdata)
size_t getNumOfSensors(eOas_sensor_t type) const
uint8_t getSensorStatus(size_t sens_index, eOas_sensor_t type) const
eth::AbstractEthResource * res
void debugPrintStateNotOK(eOas_sensor_t type, uint8_t eo_state)
yarp::dev::MAS_status sensorState_eo2yarp(eOas_sensor_t type, uint8_t eo_state)
eo_imu_privData(std::string name)
bool fromConfig(yarp::os::Searchable &config, servConfigImu_t &servCfg)
bool sendConfing2board(servConfigImu_t &servCfg)
out
Definition: sine.m:8
double conversionFactor
std::string framename
std::string name
yarp::sig::Vector values
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:62
std::vector< eOas_inertial3_descriptor_t > inertials
Definition: serviceParser.h:64
std::vector< std::string > id
Definition: serviceParser.h:65
std::vector< std::string > sensorName
Definition: serviceParser.h:66