11 #include "EoProtocolAS.h"
13 #include "EOconstarray.h"
21 memset(positionmap, 0xff,
sizeof(positionmap));
28 std::uint8_t numberof[eoas_sensors_numberof];
29 memset(numberof, 0,
sizeof(numberof));
33 const eOas_inertial3_arrayof_descriptors_t*
tmp = &servCfg.
ethservice.configuration.data.as.inertial3.arrayofdescriptor;
34 EOconstarray* array = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(
tmp));
35 uint8_t size = eo_constarray_Size(array);
36 for(uint8_t i=0; i<size; i++)
38 eOas_inertial3_descriptor_t *des = (eOas_inertial3_descriptor_t*)eo_constarray_At(array, i);
42 if(des->typeofsensor < eoas_sensors_numberof)
44 if(des->on.any.place == eobrd_place_can)
46 positionmap[des->typeofsensor][des->on.can.port][des->on.can.addr] = numberof[des->typeofsensor];
47 numberof[des->typeofsensor]++;
49 else if(des->on.any.place == eobrd_place_eth)
65 uint8_t canbus, canaddress;
69 if(
data->typeofsensor >= eoas_sensors_numberof)
74 index = positionmap[
data->typeofsensor][canbus][canaddress];
76 type =
static_cast<eOas_sensor_t
>(
data->typeofsensor);
77 return (0xff == index) ? false :
true;
82 if(canbus >= eOcanports_number)
90 index = positionmap[type][canbus][canaddress];
91 return (0xff == index) ? false :
true;
100 canbus =
data->id >> 4;
102 if(canbus >= eOcanports_number)
107 canaddress =
data->id & 0x0f;
115 errorstring =
"embObjIMU";
117 mysens.resize(eoas_sensors_numberof);
118 for(
int t=0; t<eoas_sensors_numberof; t++)
119 { mysens[t].resize(0); }
125 errorstring = error_string;
127 const eOas_inertial3_arrayof_descriptors_t*
tmp = &servCfg.
ethservice.configuration.data.as.inertial3.arrayofdescriptor;
128 EOconstarray* array = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(
tmp));
129 uint8_t size = eo_constarray_Size(array);
130 for(uint8_t i=0; i<size; i++)
132 eOas_inertial3_descriptor_t *des = (eOas_inertial3_descriptor_t*)eo_constarray_At(array, i);
136 if(des->typeofsensor < eoas_sensors_numberof)
139 newSensor.
name = servCfg.
id[i];
141 if(des->typeofsensor == eoas_imu_qua)
142 newSensor.
values.resize(4);
144 newSensor.
values.resize(3);
146 mysens[des->typeofsensor].push_back(newSensor);
154 yError() << errorstring.c_str() <<
"Out of Range error: " << oor.what();
160 std::lock_guard<std::mutex> lck (mutex);
161 return mysens[type].size();
168 std::lock_guard<std::mutex> lck (mutex);
171 return mysens[type].at(sens_index).state;
173 catch (
const std::out_of_range& oor)
176 return yarp::dev::MAS_ERROR;
184 std::lock_guard<std::mutex> lck (mutex);
185 name = mysens[type].at(sens_index).name;
188 catch (
const std::out_of_range& oor)
198 std::lock_guard<std::mutex> lck (mutex);
199 frameName = mysens[type].at(sens_index).framename;
202 catch (
const std::out_of_range& oor)
212 { std::lock_guard<std::mutex> lck (mutex);
213 out = mysens[type].at(sens_index).values;
218 for(
int i=0; i<
out.size(); i++)
223 {
for(
int i=0; i<
out.size(); i++)
228 {
for(
int i=0; i<
out.size(); i++)
234 for(
int i=0; i<
out.size(); i++)
240 timestamp = mysens[type].at(sens_index).timestamp;
242 catch (
const std::out_of_range& oor)
252 std::lock_guard<std::mutex> lck (mutex);
256 info->values[0] = newdata->x;
257 info->values[1] = newdata->y;
258 info->values[2] = newdata->z;
259 info->timestamp = yarp::os::Time::now();
267 std::lock_guard<std::mutex> lck (mutex);
274 info->state = status.calib.acc;
278 info->state = status.calib.mag;
282 info->state = status.calib.gyr;
287 info->state = status.calib.acc + status.calib.gyr + status.calib.mag;
290 info->state = status.general;
310 yError() <<
"embObjIMU" <<
getBoardInfo() <<
": missing some configuration parameter. Check logs and your config file.";
316 eOas_inertial3_config_t cfg ={0};
320 for(
size_t i=0; i<servCfg.
inertials.size(); i++)
322 eo_common_word_bitset(&cfg.enabled, i);
325 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial3, 0, eoprot_tag_as_inertial3_config);
328 yError() <<
"embObjIMU" <<
getBoardInfo() <<
"FATAL: sendConfing2board() had an error while calling setcheckRemoteValue() for config the board";
335 yError() <<
"embObjIMU" <<
getBoardInfo() <<
"sendConfing2board() correctly configured enabled sensors with period" << cfg.datarate;
345 vector<eOprotID32_t> id32v(0);
346 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial3, 0, eoprot_tag_as_inertial3_status);
348 id32v.push_back(id32);
352 yError() <<
"embObjIMU" <<
getBoardInfo() <<
"initRegulars() fails to add its variables to regulars: cannot proceed any further";
359 yError() <<
"embObjIMU" <<
getBoardInfo() <<
"initRegulars() added" << id32v.size() <<
" regular rops to the board";
361 for (
size_t r = 0; r<id32v.size(); r++)
363 uint32_t item = id32v.at(r);
364 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
365 yDebug() <<
"\t it added regular rop for" << nvinfo;
382 return yarp::dev::MAS_OK;
386 #undef DEBUG_PRINT_STATE
387 #undef DEBUG_PRINT_STATE_FULL
391 static int count[eoas_sensors_numberof]={0};
397 #if defined(DEBUG_PRINT_STATE)
405 yError() <<
getBoardInfo() <<
"sensor " << eoas_sensor2string(type) <<
"is not calibrated because its calib status reg is =" << eo_state<<
"(reg: 0 = not calib, 3 = fully calib)";
407 #if defined(DEBUG_PRINT_STATE_FULL)
408 else if(1 == eo_state)
410 yWarning() <<
getBoardInfo() <<
"sensor " << eoas_sensor2string(type) <<
"is poorly calibrated because its calib status reg is =" << eo_state<<
"(reg: 0 = not calib, 3 = fully calib)";
414 yInfo() <<
getBoardInfo() <<
"sensor " << eoas_sensor2string(type) <<
"is calibrated because its calib status reg is =" << eo_state<<
"(reg: 0 = not calib, 3 = fully calib)";
423 yError() <<
getBoardInfo() <<
"sensor " << eoas_sensor2string(type) <<
"is not calibrated because sum of the 3 calib status regs is =" << eo_state<<
"(reg: 0 = not calib, 3=fully calib)";
425 #if defined(DEBUG_PRINT_STATE_FULL)
426 else if(eo_state < 6)
428 yWarning() <<
getBoardInfo() <<
"sensor " << eoas_sensor2string(type) <<
"is poorly calibrated because sum of the 3 calib status regs is =" << eo_state<<
"(reg: 0 = not calib, 3 = fully calib)";
432 yInfo() <<
getBoardInfo() <<
"sensor " << eoas_sensor2string(type) <<
"is calibrated because sum of the 3 calib status regs is =" << eo_state<<
"(reg: 0 = not calib, 3 = fully calib)";
double convertEul_raw2metric(double) const
double convertAcc_raw2metric(double) const
double convertMag_raw2metric(double) const
double convertGyr_raw2metric(double) const
bool parseService(yarp::os::Searchable &config, servConfigMais_t &maisconfig)
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
bool init(servConfigImu_t &servCfg)
bool getIndex(const eOas_inertial3_data_t *data, uint8_t &index, eOas_sensor_t &type)
static bool getCanAddress(const eOas_inertial3_data_t *data, uint8_t &canbus, uint8_t &canaddress)
bool outOfRangeErrorHandler(const std::out_of_range &oor) const
void init(servConfigImu_t &servCfg, string error_string)
bool getSensorFrameName(size_t sens_index, eOas_sensor_t type, std::string &frameName) const
bool getSensorMeasure(size_t sens_index, eOas_sensor_t type, yarp::sig::Vector &out, double ×tamp) const
bool getSensorName(size_t sens_index, eOas_sensor_t type, std::string &name) const
bool updateStatus(eOas_sensor_t type, uint8_t index, eOas_inertial3_sensorstatus_t &status)
bool update(eOas_sensor_t type, uint8_t index, eOas_inertial3_data_t *newdata)
ImuMeasureConverter measConverter
size_t getNumOfSensors(eOas_sensor_t type) const
uint8_t getSensorStatus(size_t sens_index, eOas_sensor_t type) const
std::string getBoardInfo(void) const
eth::AbstractEthResource * res
void debugPrintStateNotOK(eOas_sensor_t type, uint8_t eo_state)
yarp::dev::MAS_status sensorState_eo2yarp(eOas_sensor_t type, uint8_t eo_state)
eo_imu_privData(std::string name)
bool fromConfig(yarp::os::Searchable &config, servConfigImu_t &servCfg)
bool sendConfing2board(servConfigImu_t &servCfg)
eOmn_serv_parameter_t ethservice
std::vector< eOas_inertial3_descriptor_t > inertials
std::vector< std::string > id