iCub-main
Loading...
Searching...
No Matches
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
28using namespace yarp::os;
29using namespace yarp::dev;
30using namespace yarp::sig;
31
32
33
34
35
36
37
38#define GET_privData(x) (*((static_cast<eo_imu_privData*>(x))))
39
40
46{
47 mPriv = new eo_imu_privData("embObjIMU");
48
49}
50
52{
53 close();
54 delete &GET_privData(mPriv);
55}
56
57std::string embObjIMU::getBoardInfo(void) const
58{
59 return GET_privData(mPriv).getBoardInfo();
60}
61
62std::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}
85std::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}
108void embObjIMU::cleanup(void)
109{
110 GET_privData(mPriv).cleanup(static_cast <eth::IethResource*> (this));
111}
112
113
114
115bool 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
200{
201 cleanup();
202 return true;
203}
204
205
206
207
208
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
216yarp::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
222bool 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
228bool 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
234bool 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
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
247yarp::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
257bool 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
268bool 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
279bool 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
291{
292 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_mag);
293}
294
295yarp::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
305bool 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
315bool 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
325bool 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
336{
337 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_eul);
338}
339
340yarp::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
350bool 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
360bool 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
370bool 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
383{
384 return GET_privData(mPriv).behFlags.opened;
385}
386
391
392
393
394#undef DEBUG_PRINT_NONVALIDDATA
395bool 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
458void 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
static bool getCanAddress(const eOas_inertial3_data_t *data, uint8_t &canbus, uint8_t &canaddress)
virtual yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
virtual bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
virtual size_t getNrOfThreeAxisMagnetometers() const override
virtual bool getOrientationSensorName(size_t sens_index, std::string &name) const override
virtual bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
embObjIMU()
This device implements the embObjIMU sensor.
Definition embObjIMU.cpp:45
virtual yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
virtual eth::iethresType_t type()
virtual bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
virtual bool close() override
virtual bool initialised()
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
virtual yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
virtual size_t getNrOfThreeAxisGyroscopes() const override
virtual bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
virtual size_t getNrOfOrientationSensors() const override
virtual bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual bool open(yarp::os::Searchable &config) override
virtual bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
virtual bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
virtual bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
virtual size_t getNrOfThreeAxisLinearAccelerometers() const override
virtual bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
virtual bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
int n
#define GET_privData(x)
iethresType_t
@ iethres_analoginertial3
out
Definition sine.m:8
eOmn_serv_parameter_t ethservice