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