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 void embObjIMU::cleanup(void)
63 {
64  GET_privData(mPriv).cleanup(static_cast <eth::IethResource*> (this));
65 }
66 
67 
68 
69 bool embObjIMU::open(yarp::os::Searchable &config)
70 {
71  // - first thing to do is verify if the eth manager is available then i parse info about the eth board.
72 
73  if(! GET_privData(mPriv).prerareEthService(config, this))
74  return false;
75 
76  // read stuff from config file
77 
78  servConfigImu_t servCfg;
79  if(!GET_privData(mPriv).fromConfig(config, servCfg))
80  return false;
81 
82  if(!GET_privData(mPriv).res->verifyEPprotocol(eoprot_endpoint_analogsensors))
83  {
84  cleanup();
85  return false;
86  }
87 
88 
89  const eOmn_serv_parameter_t* servparam = &servCfg.ethservice;
90 
91  if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_inertials3, servparam, 5.0))
92  {
93  yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate() ";
94  cleanup();
95  return false;
96  }
97 
98  //init conversion factor
99  //TODO: currently the conversion factors are not read from xml files, but configured here.
100  //please read IMUbosh datasheet for more information
101 
102  servCfg.convFactors.accFactor = 100.0; // 1 m/sec2 = 100 binary units
103  servCfg.convFactors.magFactor = 16.0 * 1000000.0; // 1 microT = 16 binary units
104  servCfg.convFactors.gyrFactor = 16.0; // 1 degree/sec = 16 binary units
105  servCfg.convFactors.eulFactor = 16.0; // 1 degree = 16 binary units
106  //eul angles don't need a conversion.
107  GET_privData(mPriv).sens.measConverter.Initialize(servCfg.convFactors.accFactor, servCfg.convFactors.gyrFactor, servCfg.convFactors.magFactor, servCfg.convFactors.eulFactor);
108 
109  // configure the sensor(s)
110 
111  if(false == GET_privData(mPriv).sendConfing2board(servCfg))
112  {
113  cleanup();
114  return false;
115  }
116 
117 
118  if(false == GET_privData(mPriv).initRegulars())
119  {
120  cleanup();
121  return false;
122  }
123 
124 
125  if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_inertials3))
126  {
127  yError() << getBoardInfo() << "open() fails to start as service.... cannot continue";
128  cleanup();
129  return false;
130  }
131  else
132  {
133  if(GET_privData(mPriv).isVerbose())
134  {
135  yDebug() << getBoardInfo() << "open() correctly starts service";
136  }
137  }
138 
139  // build data structure used to handle rx packets
140  GET_privData(mPriv).maps.init(servCfg);
141 
142 
143  { // start the configured sensors. so far, we must keep it in here. later on we can remove this command
144 
145  uint8_t enable = 1;
146 
147  eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial3, 0, eoprot_tag_as_inertial3_cmmnds_enable);
148  if(false == GET_privData(mPriv).res->setRemoteValue(id32, &enable))
149  {
150  yError() << getBoardInfo() << "open() fails to command the start transmission of the configured inertials";
151  cleanup();
152  return false;
153  }
154  }
155 
156  GET_privData(mPriv).sens.init(servCfg, getBoardInfo());
157  GET_privData(mPriv).setOpen(true);
158  return true;
159 }
160 
161 bool embObjIMU::close()
162 {
163  cleanup();
164  return true;
165 }
166 
167 
168 
169 
170 
171 size_t embObjIMU::getNrOfThreeAxisGyroscopes() const
172 {
173  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr);
174 }
175 
176 yarp::dev::MAS_status embObjIMU::getThreeAxisGyroscopeStatus(size_t sens_index) const
177 {
178  return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_gyr, GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_gyr));
179 }
180 
181 bool embObjIMU::getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const
182 {
183  return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_gyr, name);
184 }
185 
186 bool embObjIMU::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const
187 {
188  return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_gyr, frameName);
189 }
190 
191 bool embObjIMU::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
192 {
193  return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_gyr, out, timestamp);
194 }
195 
196 size_t embObjIMU::getNrOfThreeAxisLinearAccelerometers() const
197 {
198  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc);
199 }
200 
201 yarp::dev::MAS_status embObjIMU::getThreeAxisLinearAccelerometerStatus(size_t sens_index) const
202 {
203  return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_acc, GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_acc));
204 }
205 
206 bool embObjIMU::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const
207 {
208  return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_acc, name);
209 }
210 
211 bool embObjIMU::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const
212 {
213  return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_acc, frameName);
214 }
215 
216 bool embObjIMU::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
217 {
218  return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_acc, out, timestamp);
219 }
220 
221 size_t embObjIMU::getNrOfThreeAxisMagnetometers() const
222 {
223  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_mag);
224 }
225 
226 yarp::dev::MAS_status embObjIMU::getThreeAxisMagnetometerStatus(size_t sens_index) const
227 {
228  return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_mag, GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_mag));
229 }
230 
231 bool embObjIMU::getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const
232 {
233  return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_mag, name);
234 }
235 
236 bool embObjIMU::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const
237 {
238  return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_mag, frameName);
239 }
240 
241 bool embObjIMU::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
242 {
243  return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_mag, out, timestamp);
244 }
245 
246 size_t embObjIMU::getNrOfOrientationSensors() const
247 {
248  return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_eul);
249 }
250 
251 yarp::dev::MAS_status embObjIMU::getOrientationSensorStatus(size_t sens_index) const
252 {
253  return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_eul, GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_eul));
254 }
255 
256 bool embObjIMU::getOrientationSensorName(size_t sens_index, std::string &name) const
257 {
258  return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_eul, name);
259 }
260 
261 bool embObjIMU::getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const
262 {
263  return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_eul, frameName);
264 }
265 
266 bool embObjIMU::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy_out, double& timestamp) const
267 {
268  return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_eul, rpy_out, timestamp);
269 
270 }
271 
272 
273 bool embObjIMU::initialised()
274 {
275  return GET_privData(mPriv).behFlags.opened;
276 }
277 
278 eth::iethresType_t embObjIMU::type()
279 {
281 }
282 
283 
284 
285 #undef DEBUG_PRINT_NONVALIDDATA
286 bool embObjIMU::update(eOprotID32_t id32, double timestamp, void* rxdata)
287 {
288  eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
289 
290  EOconstarray* arrayofvalues = eo_constarray_Load(reinterpret_cast<const EOarray*>(&i3s->arrayofdata));
291 
292  uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
293 
294  for(int i=0; i<numofIntem2update; i++)
295  {
296  eOas_inertial3_data_t *data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
297  if(data == NULL)
298  {
299  yError() << getBoardInfo() << "update(): I have to update " << numofIntem2update << "items, but the " << i << "-th item is null.";
300  continue;
301  //NOTE: I signal this strange situation with an arror for debug porpouse...maybe we can convert in in warning when the device is stable....
302  }
303  uint8_t index;
304  eOas_sensor_t type;
305  bool validdata = GET_privData(mPriv).maps.getIndex(data, index, type);
306 
307  if(!validdata)
308  {
309 
310 #if defined(DEBUG_PRINT_NONVALIDDATA)
311  yError("NOT VALID value[%i] is: seq = %d, timestamp = %d, type = %s, id = %d, v= ((%d), %d, %d, %d), status = %x",
312  i,
313  data->seq,
314  data->timestamp,
315  eoas_sensor2string(static_cast<eOas_sensor_t>(data->typeofsensor)),
316  data->id,
317  data->w, data->x, data->y, data->z,
318  data->status.general);
319 #endif
320  continue;
321  }
322 
323  if(type == eoas_imu_status)
324  {
325  //updateAllsensorOnSameBoad(data)
326  uint8_t canbus, canaddress, i;
327  PositionMaps::getCanAddress(data, canbus, canaddress);
328  for (uint8_t t=eoas_imu_acc; t<=eoas_imu_status; t++)
329  {
330  uint8_t i;
331  if(GET_privData(mPriv).maps.getIndex(static_cast<eOas_sensor_t>(t), canbus, canaddress, i))
332  {
333  GET_privData(mPriv).sens.updateStatus(static_cast<eOas_sensor_t>(t), i, data->status);
334  //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;
335  }
336 
337  }
338  }
339  else
340  {
341  GET_privData(mPriv).sens.update(type, index, data);
342  }
343  }
344  return true;
345 
346 }
347 
348 //this function can be called inside update function to print the received data
349 void embObjIMU::updateDebugPrints(eOprotID32_t id32, double timestamp, void* rxdata)
350 {
351  static int prog = 1;
352  static double prevtime = yarp::os::Time::now();
353 
354  double delta = yarp::os::Time::now() - prevtime;
355  double millidelta = 1000.0 *delta;
356  long milli = static_cast<long>(millidelta);
357 
358 
359  eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
360 
361  EOconstarray* arrayofvalues = eo_constarray_Load(reinterpret_cast<const EOarray*>(&i3s->arrayofdata));
362 
363  uint8_t n = eo_constarray_Size(arrayofvalues);
364 
365  if(n > 0)
366  {
367  prog++;
368  // if(0 == (prog%20))
369  {
370  yDebug() << "embObjIMU::update(): received" << n << "values after" << milli << "milli";
371 
372  for(int i=0; i<n; i++)
373  {
374  eOas_inertial3_data_t *data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
375  if(NULL == data)
376  {
377  yDebug() << "embObjIMU::update(): NULL";
378  }
379  else
380  {
381  uint8_t pos = 0xff;
382  eOas_sensor_t type;
383  GET_privData(mPriv).maps.getIndex(data, pos, type);
384  yDebug("value[%i] is: seq = %d, timestamp = %d, type = %s, id = %d, v= ((%d), %d, %d, %d), status = %x, pos = %d",
385  i,
386  data->seq,
387  data->timestamp,
388  eoas_sensor2string(static_cast<eOas_sensor_t>(type)),
389  data->id,
390  data->w, data->x, data->y, data->z,
391  data->status.general,
392  pos);
393  }
394  }
395 
396  }
397 
398  }
399 }
400 
401 // eof
402 
@ data
int n
#define GET_privData(x)
Definition: embObjIMU.cpp:38
const dReal * pos
Definition: iCub_Sim.cpp:62
iethresType_t
Definition: IethResource.h:61
@ iethres_analoginertial3
Definition: IethResource.h:71
out
Definition: sine.m:8
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:70
imuConvFactors_t convFactors
Definition: serviceParser.h:74