12 #include <yarp/os/Time.h>
14 #include <yarp/os/LogStream.h>
18 #include "EoAnalogSensors.h"
19 #include "EoProtocolAS.h"
20 #include "EOconstarray.h"
23 #include "FeatureInterface.h"
28 using namespace yarp::os;
30 using namespace yarp::sig;
38 #define GET_privData(x) (*((static_cast<eo_imu_privData*>(x))))
45 embObjIMU::embObjIMU()
51 embObjIMU::~embObjIMU()
57 std::string embObjIMU::getBoardInfo(
void)
const
62 std::pair<size_t, eOas_sensor_t> embObjIMU::getGyroSubIndex(
size_t sens_index)
const
64 std::pair<size_t, eOas_sensor_t> ret;
65 if (sens_index >=
GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr))
67 sens_index -=
GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr);
68 if (sens_index >=
GET_privData(mPriv).sens.getNumOfSensors(eoas_gyros_st_l3g4200d))
70 sens_index -=
GET_privData(mPriv).sens.getNumOfSensors(eoas_gyros_st_l3g4200d);
71 ret.second = eoas_gyros_mtb_ext;
75 ret.second = eoas_gyros_st_l3g4200d;
80 ret.second = eoas_imu_gyr;
82 ret.first = sens_index;
85 std::pair<size_t, eOas_sensor_t> embObjIMU::getAccSubIndex(
size_t sens_index)
const
87 std::pair<size_t, eOas_sensor_t> ret;
88 if (sens_index >=
GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc))
90 sens_index -=
GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc);
91 if (sens_index >=
GET_privData(mPriv).sens.getNumOfSensors(eoas_accel_mtb_int))
93 sens_index -=
GET_privData(mPriv).sens.getNumOfSensors(eoas_accel_mtb_int);
94 ret.second = eoas_accel_mtb_ext;
98 ret.second = eoas_accel_mtb_int;
103 ret.second = eoas_imu_acc;
105 ret.first = sens_index;
108 void embObjIMU::cleanup(
void)
115 bool embObjIMU::open(yarp::os::Searchable &config)
119 if(!
GET_privData(mPriv).prerareEthService(config,
this))
128 if(!
GET_privData(mPriv).res->verifyEPprotocol(eoprot_endpoint_analogsensors))
135 const eOmn_serv_parameter_t* servparam = &servCfg.
ethservice;
137 if(
false ==
GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_inertials3, servparam, 5.0))
139 yError() << getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate() ";
149 if(
false ==
GET_privData(mPriv).sendConfing2board(servCfg))
163 if(
false ==
GET_privData(mPriv).res->serviceStart(eomn_serv_category_inertials3))
165 yError() << getBoardInfo() <<
"open() fails to start as service.... cannot continue";
173 yDebug() << getBoardInfo() <<
"open() correctly starts service";
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))
188 yError() << getBoardInfo() <<
"open() fails to command the start transmission of the configured inertials";
199 bool embObjIMU::close()
209 size_t embObjIMU::getNrOfThreeAxisGyroscopes()
const
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);
216 yarp::dev::MAS_status embObjIMU::getThreeAxisGyroscopeStatus(
size_t sens_index)
const
218 auto gyroSubIndex = getGyroSubIndex(sens_index);
219 return GET_privData(mPriv).sensorState_eo2yarp(gyroSubIndex.second,
GET_privData(mPriv).sens.getSensorStatus(gyroSubIndex.first, gyroSubIndex.second));
222 bool embObjIMU::getThreeAxisGyroscopeName(
size_t sens_index, std::string &name)
const
224 auto gyroSubIndex = getGyroSubIndex(sens_index);
225 return GET_privData(mPriv).sens.getSensorName(gyroSubIndex.first, gyroSubIndex.second, name);
228 bool embObjIMU::getThreeAxisGyroscopeFrameName(
size_t sens_index, std::string &frameName)
const
230 auto gyroSubIndex = getGyroSubIndex(sens_index);
231 return GET_privData(mPriv).sens.getSensorFrameName(gyroSubIndex.first, gyroSubIndex.second, frameName);
234 bool embObjIMU::getThreeAxisGyroscopeMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
236 auto gyroSubIndex = getGyroSubIndex(sens_index);
237 return GET_privData(mPriv).sens.getSensorMeasure(gyroSubIndex.first, gyroSubIndex.second,
out, timestamp);
240 size_t embObjIMU::getNrOfThreeAxisLinearAccelerometers()
const
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);
247 yarp::dev::MAS_status embObjIMU::getThreeAxisLinearAccelerometerStatus(
size_t sens_index)
const
249 if (sens_index >= getNrOfThreeAxisLinearAccelerometers()) {
250 yError() << getBoardInfo() <<
"getThreeAxisLinearAccelerometerStatus: index out of range";
251 return MAS_status::MAS_ERROR;
253 auto accSubIndex = getAccSubIndex(sens_index);
254 return GET_privData(mPriv).sensorState_eo2yarp(accSubIndex.second,
GET_privData(mPriv).sens.getSensorStatus(accSubIndex.first, accSubIndex.second));
257 bool embObjIMU::getThreeAxisLinearAccelerometerName(
size_t sens_index, std::string &name)
const
259 if (sens_index >= getNrOfThreeAxisLinearAccelerometers())
261 yError() << getBoardInfo() <<
"getThreeAxisLinearAccelerometerName: index out of range";
264 auto accSubIndex = getAccSubIndex(sens_index);
265 return GET_privData(mPriv).sens.getSensorName(accSubIndex.first, accSubIndex.second, name);
268 bool embObjIMU::getThreeAxisLinearAccelerometerFrameName(
size_t sens_index, std::string &frameName)
const
270 if (sens_index >= getNrOfThreeAxisLinearAccelerometers())
272 yError() << getBoardInfo() <<
"getThreeAxisLinearAccelerometerFrameName: index out of range";
275 auto accSubIndex = getAccSubIndex(sens_index);
276 return GET_privData(mPriv).sens.getSensorFrameName(accSubIndex.first, accSubIndex.second, frameName);
279 bool embObjIMU::getThreeAxisLinearAccelerometerMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
281 if (sens_index >= getNrOfThreeAxisLinearAccelerometers())
283 yError() << getBoardInfo() <<
"getThreeAxisLinearAccelerometerMeasure: index out of range";
286 auto accSubIndex = getAccSubIndex(sens_index);
287 return GET_privData(mPriv).sens.getSensorMeasure(accSubIndex.first, accSubIndex.second,
out, timestamp);
290 size_t embObjIMU::getNrOfThreeAxisMagnetometers()
const
292 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_mag);
295 yarp::dev::MAS_status embObjIMU::getThreeAxisMagnetometerStatus(
size_t sens_index)
const
297 if (sens_index >= getNrOfThreeAxisMagnetometers())
299 yError() << getBoardInfo() <<
"getThreeAxisMagnetometerStatus: index out of range";
300 return MAS_status::MAS_ERROR;
302 return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_mag,
GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_mag));
305 bool embObjIMU::getThreeAxisMagnetometerName(
size_t sens_index, std::string &name)
const
307 if (sens_index >= getNrOfThreeAxisMagnetometers())
309 yError() << getBoardInfo() <<
"getThreeAxisMagnetometerName: index out of range";
312 return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_mag, name);
315 bool embObjIMU::getThreeAxisMagnetometerFrameName(
size_t sens_index, std::string &frameName)
const
317 if (sens_index >= getNrOfThreeAxisMagnetometers())
319 yError() << getBoardInfo() <<
"getThreeAxisMagnetometerFrameName: index out of range";
322 return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_mag, frameName);
325 bool embObjIMU::getThreeAxisMagnetometerMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
327 if (sens_index >= getNrOfThreeAxisMagnetometers())
329 yError() << getBoardInfo() <<
"getThreeAxisMagnetometerMeasure: index out of range";
332 return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_mag,
out, timestamp);
335 size_t embObjIMU::getNrOfOrientationSensors()
const
337 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_eul);
340 yarp::dev::MAS_status embObjIMU::getOrientationSensorStatus(
size_t sens_index)
const
342 if (sens_index >= getNrOfOrientationSensors())
344 yError() << getBoardInfo() <<
"getOrientationSensorStatus: index out of range";
345 return MAS_status::MAS_ERROR;
347 return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_eul,
GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_eul));
350 bool embObjIMU::getOrientationSensorName(
size_t sens_index, std::string &name)
const
352 if (sens_index >= getNrOfOrientationSensors())
354 yError() << getBoardInfo() <<
"getOrientationSensorName: index out of range";
357 return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_eul, name);
360 bool embObjIMU::getOrientationSensorFrameName(
size_t sens_index, std::string &frameName)
const
362 if (sens_index >= getNrOfOrientationSensors())
364 yError() << getBoardInfo() <<
"getOrientationSensorFrameName: index out of range";
367 return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_eul, frameName);
370 bool embObjIMU::getOrientationSensorMeasureAsRollPitchYaw(
size_t sens_index, yarp::sig::Vector& rpy_out,
double& timestamp)
const
372 if (sens_index >= getNrOfOrientationSensors())
374 yError() << getBoardInfo() <<
"getOrientationSensorMeasureAsRollPitchYaw: index out of range";
377 return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_eul, rpy_out, timestamp);
382 bool embObjIMU::initialised()
394 #undef DEBUG_PRINT_NONVALIDDATA
395 bool embObjIMU::update(eOprotID32_t id32,
double timestamp,
void* rxdata)
397 eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
399 EOconstarray* arrayofvalues = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(&i3s->arrayofdata));
401 uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
403 for(
int i=0; i<numofIntem2update; i++)
405 eOas_inertial3_data_t *
data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
408 yError() << getBoardInfo() <<
"update(): I have to update " << numofIntem2update <<
"items, but the " << i <<
"-th item is null.";
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",
424 eoas_sensor2string(
static_cast<eOas_sensor_t
>(
data->typeofsensor)),
427 data->status.general);
432 if(type == eoas_imu_status)
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++)
440 if(
GET_privData(mPriv).maps.getIndex(
static_cast<eOas_sensor_t
>(t), canbus, canaddress, i))
442 GET_privData(mPriv).sens.updateStatus(
static_cast<eOas_sensor_t
>(t), i,
data->status);
458 void embObjIMU::updateDebugPrints(eOprotID32_t id32,
double timestamp,
void* rxdata)
461 static double prevtime = yarp::os::Time::now();
463 double delta = yarp::os::Time::now() - prevtime;
464 double millidelta = 1000.0 *delta;
465 long milli =
static_cast<long>(millidelta);
468 eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
470 EOconstarray* arrayofvalues = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(&i3s->arrayofdata));
472 uint8_t
n = eo_constarray_Size(arrayofvalues);
479 yDebug() <<
"embObjIMU::update(): received" <<
n <<
"values after" << milli <<
"milli";
481 for(
int i=0; i<
n; i++)
483 eOas_inertial3_data_t *
data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
486 yDebug() <<
"embObjIMU::update(): NULL";
493 yDebug(
"value[%i] is: seq = %d, timestamp = %d, type = %s, id = %d, v= ((%d), %d, %d, %d), status = %x, pos = %d",
497 eoas_sensor2string(
static_cast<eOas_sensor_t
>(type)),
500 data->status.general,
@ iethres_analoginertial3
eOmn_serv_parameter_t ethservice