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 void embObjIMU::cleanup(
void)
69 bool embObjIMU::open(yarp::os::Searchable &config)
73 if(!
GET_privData(mPriv).prerareEthService(config,
this))
82 if(!
GET_privData(mPriv).res->verifyEPprotocol(eoprot_endpoint_analogsensors))
89 const eOmn_serv_parameter_t* servparam = &servCfg.
ethservice;
91 if(
false ==
GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_inertials3, servparam, 5.0))
93 yError() << getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate() ";
111 if(
false ==
GET_privData(mPriv).sendConfing2board(servCfg))
125 if(
false ==
GET_privData(mPriv).res->serviceStart(eomn_serv_category_inertials3))
127 yError() << getBoardInfo() <<
"open() fails to start as service.... cannot continue";
135 yDebug() << getBoardInfo() <<
"open() correctly starts service";
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))
150 yError() << getBoardInfo() <<
"open() fails to command the start transmission of the configured inertials";
161 bool embObjIMU::close()
171 size_t embObjIMU::getNrOfThreeAxisGyroscopes()
const
173 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_gyr);
176 yarp::dev::MAS_status embObjIMU::getThreeAxisGyroscopeStatus(
size_t sens_index)
const
178 return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_gyr,
GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_gyr));
181 bool embObjIMU::getThreeAxisGyroscopeName(
size_t sens_index, std::string &name)
const
183 return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_gyr, name);
186 bool embObjIMU::getThreeAxisGyroscopeFrameName(
size_t sens_index, std::string &frameName)
const
188 return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_gyr, frameName);
191 bool embObjIMU::getThreeAxisGyroscopeMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
193 return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_gyr,
out, timestamp);
196 size_t embObjIMU::getNrOfThreeAxisLinearAccelerometers()
const
198 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_acc);
201 yarp::dev::MAS_status embObjIMU::getThreeAxisLinearAccelerometerStatus(
size_t sens_index)
const
203 return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_acc,
GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_acc));
206 bool embObjIMU::getThreeAxisLinearAccelerometerName(
size_t sens_index, std::string &name)
const
208 return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_acc, name);
211 bool embObjIMU::getThreeAxisLinearAccelerometerFrameName(
size_t sens_index, std::string &frameName)
const
213 return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_acc, frameName);
216 bool embObjIMU::getThreeAxisLinearAccelerometerMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
218 return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_acc,
out, timestamp);
221 size_t embObjIMU::getNrOfThreeAxisMagnetometers()
const
223 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_mag);
226 yarp::dev::MAS_status embObjIMU::getThreeAxisMagnetometerStatus(
size_t sens_index)
const
228 return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_mag,
GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_mag));
231 bool embObjIMU::getThreeAxisMagnetometerName(
size_t sens_index, std::string &name)
const
233 return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_mag, name);
236 bool embObjIMU::getThreeAxisMagnetometerFrameName(
size_t sens_index, std::string &frameName)
const
238 return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_mag, frameName);
241 bool embObjIMU::getThreeAxisMagnetometerMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
243 return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_mag,
out, timestamp);
246 size_t embObjIMU::getNrOfOrientationSensors()
const
248 return GET_privData(mPriv).sens.getNumOfSensors(eoas_imu_eul);
251 yarp::dev::MAS_status embObjIMU::getOrientationSensorStatus(
size_t sens_index)
const
253 return GET_privData(mPriv).sensorState_eo2yarp(eoas_imu_eul,
GET_privData(mPriv).sens.getSensorStatus(sens_index, eoas_imu_eul));
256 bool embObjIMU::getOrientationSensorName(
size_t sens_index, std::string &name)
const
258 return GET_privData(mPriv).sens.getSensorName(sens_index, eoas_imu_eul, name);
261 bool embObjIMU::getOrientationSensorFrameName(
size_t sens_index, std::string &frameName)
const
263 return GET_privData(mPriv).sens.getSensorFrameName(sens_index, eoas_imu_eul, frameName);
266 bool embObjIMU::getOrientationSensorMeasureAsRollPitchYaw(
size_t sens_index, yarp::sig::Vector& rpy_out,
double& timestamp)
const
268 return GET_privData(mPriv).sens.getSensorMeasure(sens_index, eoas_imu_eul, rpy_out, timestamp);
273 bool embObjIMU::initialised()
285 #undef DEBUG_PRINT_NONVALIDDATA
286 bool embObjIMU::update(eOprotID32_t id32,
double timestamp,
void* rxdata)
288 eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
290 EOconstarray* arrayofvalues = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(&i3s->arrayofdata));
292 uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
294 for(
int i=0; i<numofIntem2update; i++)
296 eOas_inertial3_data_t *
data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
299 yError() << getBoardInfo() <<
"update(): I have to update " << numofIntem2update <<
"items, but the " << i <<
"-th item is null.";
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",
315 eoas_sensor2string(
static_cast<eOas_sensor_t
>(
data->typeofsensor)),
318 data->status.general);
323 if(type == eoas_imu_status)
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++)
331 if(
GET_privData(mPriv).maps.getIndex(
static_cast<eOas_sensor_t
>(t), canbus, canaddress, i))
333 GET_privData(mPriv).sens.updateStatus(
static_cast<eOas_sensor_t
>(t), i,
data->status);
349 void embObjIMU::updateDebugPrints(eOprotID32_t id32,
double timestamp,
void* rxdata)
352 static double prevtime = yarp::os::Time::now();
354 double delta = yarp::os::Time::now() - prevtime;
355 double millidelta = 1000.0 *delta;
356 long milli =
static_cast<long>(millidelta);
359 eOas_inertial3_status_t *i3s = (eOas_inertial3_status_t*)rxdata;
361 EOconstarray* arrayofvalues = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(&i3s->arrayofdata));
363 uint8_t
n = eo_constarray_Size(arrayofvalues);
370 yDebug() <<
"embObjIMU::update(): received" <<
n <<
"values after" << milli <<
"milli";
372 for(
int i=0; i<
n; i++)
374 eOas_inertial3_data_t *
data = (eOas_inertial3_data_t*) eo_constarray_At(arrayofvalues, i);
377 yDebug() <<
"embObjIMU::update(): NULL";
384 yDebug(
"value[%i] is: seq = %d, timestamp = %d, type = %s, id = %d, v= ((%d), %d, %d, %d), status = %x, pos = %d",
388 eoas_sensor2string(
static_cast<eOas_sensor_t
>(type)),
391 data->status.general,
@ iethres_analoginertial3
eOmn_serv_parameter_t ethservice
imuConvFactors_t convFactors