iCub-main
embObjIMU.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 
9 #include <string>
10 #include <mutex>
11 #include <stdexcept>
12 #include <yarp/os/Time.h>
13 
14 #include <yarp/os/LogStream.h>
15 
16 #include <embObjIMU.h>
17 
18 #include "EoAnalogSensors.h"
19 #include "EoProtocolAS.h"
20 #include "EOconstarray.h"
21 
22 
23 #include "FeatureInterface.h"
24 #include "eo_imu_privData.h"
25 
26 
27 
28 using namespace yarp::os;
29 using namespace yarp::dev;
30 using namespace yarp::sig;
31 
32 
33 
34 
35 
36 
37 
38 #define GET_privData(x) (*((static_cast<eo_imu_privData*>(x))))
39 
40 
45 embObjIMU::embObjIMU()
46 {
47  mPriv = new eo_imu_privData("embObjIMU");
48 
49 }
50 
51 embObjIMU::~embObjIMU()
52 {
53  close();
54  delete &GET_privData(mPriv);
55 }
56 
57 std::string embObjIMU::getBoardInfo(void) const
58 {
59  return GET_privData(mPriv).getBoardInfo();
60 }
61 
62 std::pair<size_t, eOas_sensor_t> embObjIMU::getGyroSubIndex(size_t sens_index) const
63 {
64  std::pair<size_t, eOas_sensor_t> ret;
65  if (sens_index >= GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr))
66  {
67  sens_index -= GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr);
68  if (sens_index >= GET_privData(mPriv).sens.getNumOfSensors(eoas_gyros_st_l3g4200d))
69  {
70  sens_index -= GET_privData(mPriv).sens.getNumOfSensors(eoas_gyros_st_l3g4200d);
71  ret.second = eoas_gyros_mtb_ext;
72  }
73  else
74  {
75  ret.second = eoas_gyros_st_l3g4200d;
76  }
77  }
78  else
79  {
80  ret.second = eoas_imu_gyr;
81  }
82  ret.first = sens_index;
83  return ret;
84 }
85 std::pair<size_t, eOas_sensor_t> embObjIMU::getAccSubIndex(size_t sens_index) const
86 {
87  std::pair<size_t, eOas_sensor_t> ret;
88  if (sens_index >= GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc))
89  {
90  sens_index -= GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc);
91  if (sens_index >= GET_privData(mPriv).sens.getNumOfSensors(eoas_accel_mtb_int))
92  {
93  sens_index -= GET_privData(mPriv).sens.getNumOfSensors(eoas_accel_mtb_int);
94  ret.second = eoas_accel_mtb_ext;
95  }
96  else
97  {
98  ret.second = eoas_accel_mtb_int;
99  }
100  }
101  else
102  {
103  ret.second = eoas_imu_acc;
104  }
105  ret.first = sens_index;
106  return ret;
107 }
108 void embObjIMU::cleanup(void)
109 {
110  GET_privData(mPriv).cleanup(static_cast <eth::IethResource*> (this));
111 }
112 
113 
114 
115 bool embObjIMU::open(yarp::os::Searchable &config)
116 {
117  // - first thing to do is verify if the eth manager is available then i parse info about the eth board.
118 
119  if(! GET_privData(mPriv).prerareEthService(config, this))
120  return false;
121 
122  // read stuff from config file
123 
124  servConfigImu_t servCfg;
125  if(!GET_privData(mPriv).fromConfig(config, servCfg))
126  return false;
127 
128  if(!GET_privData(mPriv).res->verifyEPprotocol(eoprot_endpoint_analogsensors))
129  {
130  cleanup();
131  return false;
132  }
133 
134 
135  const eOmn_serv_parameter_t* servparam = &servCfg.ethservice;
136 
137  if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_inertials3, servparam, 5.0))
138  {
139  yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate() ";
140  cleanup();
141  return false;
142  }
143 
144  //init conversion factor
145  //TODO: currently the conversion factors are not read from xml files, but configured here.
146  //please read IMUbosh datasheet for more information
147  // configure the sensor(s)
148 
149  if(false == GET_privData(mPriv).sendConfing2board(servCfg))
150  {
151  cleanup();
152  return false;
153  }
154 
155 
156  if(false == GET_privData(mPriv).initRegulars())
157  {
158  cleanup();
159  return false;
160  }
161 
162 
163  if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_inertials3))
164  {
165  yError() << getBoardInfo() << "open() fails to start as service.... cannot continue";
166  cleanup();
167  return false;
168  }
169  else
170  {
171  if(GET_privData(mPriv).isVerbose())
172  {
173  yDebug() << getBoardInfo() << "open() correctly starts service";
174  }
175  }
176 
177  // build data structure used to handle rx packets
178  GET_privData(mPriv).maps.init(servCfg);
179 
180 
181  { // start the configured sensors. so far, we must keep it in here. later on we can remove this command
182 
183  uint8_t enable = 1;
184 
185  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial3, 0, eoprot_tag_as_inertial3_cmmnds_enable);
186  if(false == GET_privData(mPriv).res->setRemoteValue(id32, &enable))
187  {
188  yError() << getBoardInfo() << "open() fails to command the start transmission of the configured inertials";
189  cleanup();
190  return false;
191  }
192  }
193 
194  GET_privData(mPriv).sens.init(servCfg, getBoardInfo());
195  GET_privData(mPriv).setOpen(true);
196  return true;
197 }
198 
199 bool embObjIMU::close()
200 {
201  cleanup();
202  return true;
203 }
204 
205 
206 
207 
208 
209 size_t embObjIMU::getNrOfThreeAxisGyroscopes() const
210 {
211  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr) +
212  GET_privData(mPriv).sens.getNumOfSensors(eoas_gyros_st_l3g4200d) +
213  GET_privData(mPriv).sens.getNumOfSensors(eoas_gyros_mtb_ext);
214 }
215 
216 yarp::dev::MAS_status embObjIMU::getThreeAxisGyroscopeStatus(size_t sens_index) const
217 {
218  auto gyroSubIndex = getGyroSubIndex(sens_index);
219  return GET_privData(mPriv).sensorState_eo2yarp(gyroSubIndex.second, GET_privData(mPriv).sens.getSensorStatus(gyroSubIndex.first, gyroSubIndex.second));
220 }
221 
222 bool embObjIMU::getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const
223 {
224  auto gyroSubIndex = getGyroSubIndex(sens_index);
225  return GET_privData(mPriv).sens.getSensorName(gyroSubIndex.first, gyroSubIndex.second, name);
226 }
227 
228 bool embObjIMU::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const
229 {
230  auto gyroSubIndex = getGyroSubIndex(sens_index);
231  return GET_privData(mPriv).sens.getSensorFrameName(gyroSubIndex.first, gyroSubIndex.second, frameName);
232 }
233 
234 bool embObjIMU::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
235 {
236  auto gyroSubIndex = getGyroSubIndex(sens_index);
237  return GET_privData(mPriv).sens.getSensorMeasure(gyroSubIndex.first, gyroSubIndex.second, out, timestamp);
238 }
239 
240 size_t embObjIMU::getNrOfThreeAxisLinearAccelerometers() const
241 {
242  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc) +
243  GET_privData(mPriv).sens.getNumOfSensors(eoas_accel_mtb_int) +
244  GET_privData(mPriv).sens.getNumOfSensors(eoas_accel_mtb_ext);
245 }
246 
247 yarp::dev::MAS_status embObjIMU::getThreeAxisLinearAccelerometerStatus(size_t sens_index) const
248 {
249  if (sens_index >= getNrOfThreeAxisLinearAccelerometers()) {
250  yError() << getBoardInfo() << "getThreeAxisLinearAccelerometerStatus: index out of range";
251  return MAS_status::MAS_ERROR;
252  }
253  auto accSubIndex = getAccSubIndex(sens_index);
254  return GET_privData(mPriv).sensorState_eo2yarp(accSubIndex.second, GET_privData(mPriv).sens.getSensorStatus(accSubIndex.first, accSubIndex.second));
255 }
256 
257 bool embObjIMU::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const
258 {
259  if (sens_index >= getNrOfThreeAxisLinearAccelerometers())
260  {
261  yError() << getBoardInfo() << "getThreeAxisLinearAccelerometerName: index out of range";
262  return false;
263  }
264  auto accSubIndex = getAccSubIndex(sens_index);
265  return GET_privData(mPriv).sens.getSensorName(accSubIndex.first, accSubIndex.second, name);
266 }
267 
268 bool embObjIMU::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const
269 {
270  if (sens_index >= getNrOfThreeAxisLinearAccelerometers())
271  {
272  yError() << getBoardInfo() << "getThreeAxisLinearAccelerometerFrameName: index out of range";
273  return false;
274  }
275  auto accSubIndex = getAccSubIndex(sens_index);
276  return GET_privData(mPriv).sens.getSensorFrameName(accSubIndex.first, accSubIndex.second, frameName);
277 }
278 
279 bool embObjIMU::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
280 {
281  if (sens_index >= getNrOfThreeAxisLinearAccelerometers())
282  {
283  yError() << getBoardInfo() << "getThreeAxisLinearAccelerometerMeasure: index out of range";
284  return false;
285  }
286  auto accSubIndex = getAccSubIndex(sens_index);
287  return GET_privData(mPriv).sens.getSensorMeasure(accSubIndex.first, accSubIndex.second, out, timestamp);
288 }
289 
290 size_t embObjIMU::getNrOfThreeAxisMagnetometers() const
291 {
292  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_mag);
293 }
294 
295 yarp::dev::MAS_status embObjIMU::getThreeAxisMagnetometerStatus(size_t sens_index) const
296 {
297  if (sens_index >= getNrOfThreeAxisMagnetometers())
298  {
299  yError() << getBoardInfo() << "getThreeAxisMagnetometerStatus: index out of range";
300  return MAS_status::MAS_ERROR;
301  }
302  return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_mag, GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_mag));
303 }
304 
305 bool embObjIMU::getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const
306 {
307  if (sens_index >= getNrOfThreeAxisMagnetometers())
308  {
309  yError() << getBoardInfo() << "getThreeAxisMagnetometerName: index out of range";
310  return false;
311  }
312  return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_mag, name);
313 }
314 
315 bool embObjIMU::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const
316 {
317  if (sens_index >= getNrOfThreeAxisMagnetometers())
318  {
319  yError() << getBoardInfo() << "getThreeAxisMagnetometerFrameName: index out of range";
320  return false;
321  }
322  return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_mag, frameName);
323 }
324 
325 bool embObjIMU::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
326 {
327  if (sens_index >= getNrOfThreeAxisMagnetometers())
328  {
329  yError() << getBoardInfo() << "getThreeAxisMagnetometerMeasure: index out of range";
330  return false;
331  }
332  return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_mag, out, timestamp);
333 }
334 
335 size_t embObjIMU::getNrOfOrientationSensors() const
336 {
337  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_eul);
338 }
339 
340 yarp::dev::MAS_status embObjIMU::getOrientationSensorStatus(size_t sens_index) const
341 {
342  if (sens_index >= getNrOfOrientationSensors())
343  {
344  yError() << getBoardInfo() << "getOrientationSensorStatus: index out of range";
345  return MAS_status::MAS_ERROR;
346  }
347  return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_eul, GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_eul));
348 }
349 
350 bool embObjIMU::getOrientationSensorName(size_t sens_index, std::string &name) const
351 {
352  if (sens_index >= getNrOfOrientationSensors())
353  {
354  yError() << getBoardInfo() << "getOrientationSensorName: index out of range";
355  return false;
356  }
357  return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_eul, name);
358 }
359 
360 bool embObjIMU::getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const
361 {
362  if (sens_index >= getNrOfOrientationSensors())
363  {
364  yError() << getBoardInfo() << "getOrientationSensorFrameName: index out of range";
365  return false;
366  }
367  return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_eul, frameName);
368 }
369 
370 bool embObjIMU::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy_out, double& timestamp) const
371 {
372  if (sens_index >= getNrOfOrientationSensors())
373  {
374  yError() << getBoardInfo() << "getOrientationSensorMeasureAsRollPitchYaw: index out of range";
375  return false;
376  }
377  return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_eul, rpy_out, timestamp);
378 
379 }
380 
381 
382 bool embObjIMU::initialised()
383 {
384  return GET_privData(mPriv).behFlags.opened;
385 }
386 
387 eth::iethresType_t embObjIMU::type()
388 {
390 }
391 
392 
393 
394 #undef DEBUG_PRINT_NONVALIDDATA
395 bool embObjIMU::update(eOprotID32_t id32, double timestamp, void* rxdata)
396 {
397  eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
398 
399  EOconstarray* arrayofvalues = eo_constarray_Load(reinterpret_cast<const EOarray*>(&i3s->arrayofdata));
400 
401  uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
402 
403  for(int i=0; i<numofIntem2update; i++)
404  {
405  eOas_inertial3_data_t *data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
406  if(data == NULL)
407  {
408  yError() << getBoardInfo() << "update(): I have to update " << numofIntem2update << "items, but the " << i << "-th item is null.";
409  continue;
410  //NOTE: I signal this strange situation with an arror for debug porpouse...maybe we can convert in in warning when the device is stable....
411  }
412  uint8_t index;
413  eOas_sensor_t type;
414  bool validdata = GET_privData(mPriv).maps.getIndex(data, index, type);
415 
416  if(!validdata)
417  {
418 
419 #if defined(DEBUG_PRINT_NONVALIDDATA)
420  yError("NOT VALID value[%i] is: seq = %d, timestamp = %d, type = %s, id = %d, v= ((%d), %d, %d, %d), status = %x",
421  i,
422  data->seq,
423  data->timestamp,
424  eoas_sensor2string(static_cast<eOas_sensor_t>(data->typeofsensor)),
425  data->id,
426  data->w, data->x, data->y, data->z,
427  data->status.general);
428 #endif
429  continue;
430  }
431 
432  if(type == eoas_imu_status)
433  {
434  //updateAllsensorOnSameBoad(data)
435  uint8_t canbus, canaddress, i;
436  PositionMaps::getCanAddress(data, canbus, canaddress);
437  for (uint8_t t=eoas_imu_acc; t<=eoas_imu_status; t++)
438  {
439  uint8_t i;
440  if(GET_privData(mPriv).maps.getIndex(static_cast<eOas_sensor_t>(t), canbus, canaddress, i))
441  {
442  GET_privData(mPriv).sens.updateStatus(static_cast<eOas_sensor_t>(t), i, data->status);
443  //yError() << "UPDATE STATUS OF SENSOR " << i << "with type "<< eoas_sensor2string(static_cast<eOas_sensor_t>(t)) << "can port=" << canbus << "can addr=" << canaddress << "status=" << data->status.general;
444  }
445 
446  }
447  }
448  else
449  {
450  GET_privData(mPriv).sens.update(type, index, data);
451  }
452  }
453  return true;
454 
455 }
456 
457 //this function can be called inside update function to print the received data
458 void embObjIMU::updateDebugPrints(eOprotID32_t id32, double timestamp, void* rxdata)
459 {
460  static int prog = 1;
461  static double prevtime = yarp::os::Time::now();
462 
463  double delta = yarp::os::Time::now() - prevtime;
464  double millidelta = 1000.0 *delta;
465  long milli = static_cast<long>(millidelta);
466 
467 
468  eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
469 
470  EOconstarray* arrayofvalues = eo_constarray_Load(reinterpret_cast<const EOarray*>(&i3s->arrayofdata));
471 
472  uint8_t n = eo_constarray_Size(arrayofvalues);
473 
474  if(n > 0)
475  {
476  prog++;
477  // if(0 == (prog%20))
478  {
479  yDebug() << "embObjIMU::update(): received" << n << "values after" << milli << "milli";
480 
481  for(int i=0; i<n; i++)
482  {
483  eOas_inertial3_data_t *data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
484  if(NULL == data)
485  {
486  yDebug() << "embObjIMU::update(): NULL";
487  }
488  else
489  {
490  uint8_t pos = 0xff;
491  eOas_sensor_t type;
492  GET_privData(mPriv).maps.getIndex(data, pos, type);
493  yDebug("value[%i] is: seq = %d, timestamp = %d, type = %s, id = %d, v= ((%d), %d, %d, %d), status = %x, pos = %d",
494  i,
495  data->seq,
496  data->timestamp,
497  eoas_sensor2string(static_cast<eOas_sensor_t>(type)),
498  data->id,
499  data->w, data->x, data->y, data->z,
500  data->status.general,
501  pos);
502  }
503  }
504 
505  }
506 
507  }
508 }
509 
510 // eof
511 
@ data
int n
#define GET_privData(x)
Definition: embObjIMU.cpp:38
iethresType_t
Definition: IethResource.h:62
@ iethres_analoginertial3
Definition: IethResource.h:71
out
Definition: sine.m:8
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:62