Skip to content

Commit

Permalink
embObjIMU: add setting
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene committed Jan 30, 2023
1 parent 4d77a66 commit 25cc99c
Show file tree
Hide file tree
Showing 4 changed files with 61 additions and 42 deletions.
10 changes: 8 additions & 2 deletions src/libraries/icubmod/embObjIMU/eo_imu_privData.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,13 @@ void SensorsData::init(servConfigImu_t &servCfg, string error_string)
{
sensorInfo_t newSensor;
newSensor.name = servCfg.id[i];
newSensor.framename = newSensor.name;
if (servCfg.sensorName.empty()) {
newSensor.sensorName = newSensor.name;
}
else {
newSensor.sensorName = servCfg.sensorName;
}
newSensor.framename = newSensor.sensorName;
if(des->typeofsensor == eoas_imu_qua)
newSensor.values.resize(4);
else
Expand Down Expand Up @@ -182,7 +188,7 @@ bool SensorsData::getSensorName(size_t sens_index, eOas_sensor_t type, std::stri
try
{
std::lock_guard<std::mutex> lck (mutex);
name = mysens[type].at(sens_index).name;
name = mysens[type].at(sens_index).sensorName;
return true;
}
catch (const std::out_of_range& oor)
Expand Down
1 change: 1 addition & 0 deletions src/libraries/icubmod/embObjIMU/eo_imu_privData.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ typedef struct
yarp::sig::Vector values;
uint8_t state;
double timestamp;
std::string sensorName;
} sensorInfo_t;

class yarp::dev::SensorsData
Expand Down
86 changes: 48 additions & 38 deletions src/libraries/icubmod/embObjLib/serviceParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -469,7 +469,7 @@ bool ServiceParser::convert(std::string const &fromstring, eObrd_location_t &loc
{
//Add here parsing for local port (both PWM port (P7, P8, etc and "index_proximal"for hand))
//in xml file we have: LOC:P7 or LOC:index_proximal

}
else
{
Expand Down Expand Up @@ -744,7 +744,7 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type)
Bottle b_PROPERTIES_SENSORS_boardType;
if((eomn_serv_AS_inertials3 == type) || (eomn_serv_AS_pos == type))
{

b_PROPERTIES_SENSORS_boardType = Bottle(b_PROPERTIES_SENSORS.findGroup("boardType"));
if(b_PROPERTIES_SENSORS_boardType.isNull())
{
Expand Down Expand Up @@ -874,7 +874,7 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type)
{
// then we need to parse four values: type, rotation, offset, invertDirection

// type
// type
if(false == b_PROPERTIES_SENSORS_pos_CALIBRATION_type.isNull())
{
parse_POS_CALIB_type(b_PROPERTIES_SENSORS_pos_CALIBRATION_type.get(i+1).asString(), item.pos.calibration.type, formaterror);
Expand Down Expand Up @@ -939,6 +939,15 @@ bool ServiceParser::check_analog(Searchable &config, eOmn_serv_type_t type)
yError() << "ServiceParser::check_analog() cannot find SETTINGS.enabledSensors";
return false;
}
Bottle b_SETTINGS_sensorName = Bottle(b_SETTINGS.findGroup("sensorName"));
if(b_SETTINGS_sensorName.isNull())
{
yWarning() << "ServiceParser::check_analog() cannot find SETTINGS.sensorName";
}
else
{
convert(b_SETTINGS_sensorName.get(1).asString(), as_service.settings.sensorName, formaterror);
}

size_t s = b_SETTINGS_enabledSensors.size();
size_t numenabledsensors = (0 == s) ? (0) : (s - 1) ; // first position of bottle contains the tag "enabledSensors"
Expand Down Expand Up @@ -1465,7 +1474,6 @@ bool ServiceParser::parseService(Searchable &config, servConfigMais_t &maisconfi
maisconfig.ethservice.configuration.data.as.mais.canloc.addr = themais_sensor.location.can.addr;
maisconfig.ethservice.configuration.data.as.mais.canloc.insideindex = eobrd_caninsideindex_none;


return true;
}

Expand Down Expand Up @@ -1522,30 +1530,30 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon
yError() << "ServiceParser::parseService() has received an invalid SERVICE group for strain";
return false;
}

// now we extract values ... so far we dont make many checks ... we just assume the vector<> are of size 1.
servCanBoard_t thestrain_props = as_service.properties.canboards.at(0);
servAnalogSensor_t thestrain_sensor = as_service.settings.enabledsensors.at(0);

// first check we do is about thestrain_props.type
if((eobrd_cantype_strain2 != thestrain_props.type) && (eobrd_cantype_strain2c != thestrain_props.type))
{
yError() << "ServiceParser::parseService() for embObjFTsensor has detected an invalid type of board. it should be a eobrd_strain2 or a eobrd_strain2c but is a:" << eoboards_type2string2(eoboards_cantype2type(thestrain_props.type), eobool_false);
return false;
}

ftconfig.acquisitionrate = as_service.settings.acquisitionrate;
ftconfig.useCalibration = as_strain_settings.useCalibration;
ftconfig.nameOfStrain = thestrain_sensor.id;
ftconfig.frameName = thestrain_sensor.frameName;

memset(&ftconfig.ethservice.configuration, 0, sizeof(ftconfig.ethservice.configuration));

ftconfig.ethservice.configuration.type = eomn_serv_AS_strain;
ftconfig.ethservice.configuration.data.as.strain.boardtype.type = thestrain_props.type;
memcpy(&ftconfig.ethservice.configuration.data.as.strain.boardtype.protocol, &thestrain_props.protocol, sizeof(eObrd_protocolversion_t));
memcpy(&ftconfig.ethservice.configuration.data.as.strain.boardtype.firmware, &thestrain_props.firmware, sizeof(eObrd_firmwareversion_t));

// second check we do is about thestrain_sensor.location
if(eobrd_place_can != thestrain_sensor.location.any.place)
{
Expand All @@ -1557,7 +1565,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon
ftconfig.ethservice.configuration.data.as.strain.canloc.insideindex = eobrd_caninsideindex_none;



Bottle b_SERVICE(config.findGroup("SERVICE")); //b_SERVICE and b_SETTINGS could not be null, otherwise parseService function would have returned false
Bottle b_SETTINGS = Bottle(b_SERVICE.findGroup("SETTINGS"));
Bottle b_SETTINGS_temp = Bottle(b_SETTINGS.findGroup("temperature-acquisitionRate"));
Expand All @@ -1571,7 +1579,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon
ftconfig.temperatureAcquisitionrate = b_SETTINGS_temp.get(1).asInt32();
//TODO: chek that the acquisition rate is inside a reasonable range
}

return true;
}

Expand Down Expand Up @@ -1642,10 +1650,10 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)
yError() << "ServiceParser::parseService(IMU) has received an invalid SERVICE group for IMU";
return false;
}


//check the num of type of boards. At max we have 4 board type (see eOas_inertials3_boardinfos_maxnumber)

if(as_service.properties.canboards.size() > eOas_inertials3_boardinfos_maxnumber)
{
yError() << "ServiceParser::parseService(IMU): too many type board info are configured. The max num is " << eOas_inertials3_boardinfos_maxnumber;
Expand All @@ -1654,45 +1662,47 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)

//reset configuration service
memset(&imuconfig.ethservice.configuration, 0, sizeof(imuconfig.ethservice.configuration));

//set type of service
imuconfig.ethservice.configuration.type = eomn_serv_AS_inertials3;
//get acquisition rate


//get acquisition rate
imuconfig.acquisitionrate = as_service.settings.acquisitionrate;

//get sensor name
imuconfig.sensorName = as_service.settings.sensorName;

//get enabled sensor and fill canboard array. Note that we get only the enabled sensor, not all configured sensors !!!

imuconfig.inertials.resize(0);

eOas_inertial3_setof_boardinfos_t * boardInfoSet_ptr = &imuconfig.ethservice.configuration.data.as.inertial3.setofboardinfos;
eOresult_t res = eoas_inertial3_setof_boardinfos_clear(boardInfoSet_ptr);
if(res != eores_OK)
{
yError() << "ServiceParser::parseService(IMU). Error in eoas_inertial3_setof_boardinfos_clear()";
return false;
}

EOarray* array = eo_array_New(eOas_inertials3_descriptors_maxnumber, sizeof(eOas_inertial3_descriptor_t), &imuconfig.ethservice.configuration.data.as.inertial3.arrayofdescriptor);
for(size_t i=0; i<as_service.settings.enabledsensors.size(); i++)
{
servAnalogSensor_t sensor = as_service.settings.enabledsensors.at(i);
eOas_sensor_t type = sensor.type;

//TODO: temperature???
if( (eoas_imu_acc != type) && (eoas_imu_mag != type) && (eoas_imu_gyr != type) && (eoas_imu_eul != type) &&
if( (eoas_imu_acc != type) && (eoas_imu_mag != type) && (eoas_imu_gyr != type) && (eoas_imu_eul != type) &&
(eoas_imu_qua != type) && (eoas_imu_lia != type) && (eoas_imu_grv != type) && (eoas_imu_status != type) )
{
yWarning() << "ServiceParser::parseService() has detected a wrong inertial sensor:" << eoas_sensor2string(type) << " ... we drop it";
continue;
}
// if ok, i copy it inside ...

eOas_inertial3_descriptor_t des = {};
des.typeofsensor = type;
memcpy(&des.on, &sensor.location, sizeof(eObrd_location_t));

const eObrd_info_t *boardInfo_ptr = eoas_inertial3_setof_boardinfos_find(boardInfoSet_ptr, eoboards_type2cantype(sensor.boardtype));
if(nullptr == boardInfo_ptr)//if I did not already insert the borad info with type == sensor.boardtype, now I insert it
{
Expand Down Expand Up @@ -1724,13 +1734,13 @@ bool ServiceParser::parseService(Searchable &config, servConfigImu_t &imuconfig)
}
}
des.typeofboard = sensor.boardtype;

eo_array_PushBack(array, &des);
imuconfig.inertials.push_back(des);
imuconfig.id.push_back(sensor.id);
}


return true;
}

Expand Down Expand Up @@ -2325,12 +2335,12 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth
ret = false;
}
}

if(false == ret)
{
return ret;
}

if(eomc_act_foc == type)
{
// copy into todes.foc
Expand Down Expand Up @@ -2687,7 +2697,7 @@ bool ServiceParser::convert(std::string const &fromstring, eOmc_encoder_t &toenc
bool ServiceParser::check_motion(Searchable &config)
{
bool formaterror = false;

// complete format is SERVICE{ type, PROPERTIES{ ETHBOARD, CANBOARDS, MC4, MAIS, CONTROLLER, JOINTMAPPING, JOINTSETS } }
// so far, there is no SETTINGS{}.
// then, some groups are not present for some kind of SERVICE.type
Expand Down Expand Up @@ -3946,7 +3956,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
mcconfig.ethservice.configuration.type = mc_service.type;
mcconfig.ethservice.configuration.diagnosticsmode = mc_service.diagconfig.mode;
mcconfig.ethservice.configuration.diagnosticsparam = mc_service.diagconfig.par16;

switch(mc_service.type)
{
case eomn_serv_MC_foc:
Expand Down Expand Up @@ -3975,7 +3985,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
jomodes.actuator.foc.canloc.port = mc_service.properties.actuators[i].desc.foc.canloc.port;
jomodes.actuator.foc.canloc.addr = mc_service.properties.actuators[i].desc.foc.canloc.addr;
jomodes.actuator.foc.canloc.insideindex = eobrd_caninsideindex_first;

// 2. encoder1 is ...
jomodes.encoder1.type = mc_service.properties.encoder1s[i].desc.type;
jomodes.encoder1.port = mc_service.properties.encoder1s[i].desc.port;
Expand All @@ -4000,7 +4010,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = true;

} break;

case eomn_serv_MC_mc4:
{
eOmn_serv_config_data_mc_mc4_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4_based);
Expand Down Expand Up @@ -4063,7 +4073,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = true;

} break;

case eomn_serv_MC_mc4plus:
{
eOmn_serv_config_data_mc_mc4plus_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4plus_based);
Expand Down Expand Up @@ -4102,7 +4112,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = true;

} break;

case eomn_serv_MC_mc4plusmais:
{
eOmn_serv_config_data_mc_mc4plusmais_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4plusmais_based);
Expand Down Expand Up @@ -4362,7 +4372,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
ret = false;
} break;
}


return ret;
}
Expand Down
6 changes: 4 additions & 2 deletions src/libraries/icubmod/embObjLib/serviceParser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ typedef struct
std::vector<eOas_inertial3_descriptor_t> inertials; //TODO to remove because information is already stored!
std::vector<std::string> id;
imuConvFactors_t convFactors;
std::string sensorName{""};
} servConfigImu_t;


Expand Down Expand Up @@ -172,8 +173,9 @@ typedef struct

typedef struct
{
uint16_t acquisitionrate;
uint16_t acquisitionrate;
std::vector<servAnalogSensor_t> enabledsensors;
std::string sensorName{""};
} servASsettings_t;


Expand Down Expand Up @@ -379,7 +381,7 @@ class ServiceParser

bool copyjomocouplingInfo(eOmc_4jomo_coupling_t *jc_dest);


// suggestion: split check_motion() in sub-methods which parse the groups ...
};

Expand Down

0 comments on commit 25cc99c

Please sign in to comment.