From 631b93903f2fd164fde39fe06045b7a4527b3c28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Genesio=20=28=E4=BA=8C=E3=82=B3=E3=82=B2?= =?UTF-8?q?=E3=83=8D=29?= Date: Thu, 25 May 2023 11:18:48 +0200 Subject: [PATCH] embObjFTSensor: make it handle strain board (#874) --- .../icubmod/embObjFTsensor/embObjFTsensor.cpp | 63 +++++++------- .../embObjFTsensor/eo_ftsens_privData.cpp | 86 +++++++++---------- .../icubmod/embObjLib/serviceParser.cpp | 13 ++- 3 files changed, 86 insertions(+), 76 deletions(-) diff --git a/src/libraries/icubmod/embObjFTsensor/embObjFTsensor.cpp b/src/libraries/icubmod/embObjFTsensor/embObjFTsensor.cpp index 8e96025d93..68c20823f0 100755 --- a/src/libraries/icubmod/embObjFTsensor/embObjFTsensor.cpp +++ b/src/libraries/icubmod/embObjFTsensor/embObjFTsensor.cpp @@ -95,7 +95,7 @@ bool embObjFTsensor::open(yarp::os::Searchable &config) return false; // read stuff from config file - + servConfigFTsensor_t serviceConfig; if(!GET_privData(mPriv).fromConfig(config, serviceConfig)) return false; @@ -111,9 +111,11 @@ bool embObjFTsensor::open(yarp::os::Searchable &config) //Fill temperature service data in servparamtemp: some of these data are copied from the serviceconfig of ft because both services used tha same board. eOmn_serv_parameter_t servparamtemp; - bool ret = GET_privData(mPriv).fillTemperatureEthServiceInfo(serviceConfig.ethservice, servparamtemp); - if(!ret) - return false; + if (GET_privData(mPriv).useTemperature) { + bool ret = GET_privData(mPriv).fillTemperatureEthServiceInfo(serviceConfig.ethservice, servparamtemp); + if(!ret) + return false; + } const eOmn_serv_parameter_t* servparamtemp_ptr = &servparamtemp; const eOmn_serv_parameter_t* servparamstrain = &serviceConfig.ethservice; @@ -129,12 +131,13 @@ bool embObjFTsensor::open(yarp::os::Searchable &config) cleanup(); return false; } - - if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_temperatures, servparamtemp_ptr, 5.0)) - { - yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()"; - cleanup(); - return false; + if (GET_privData(mPriv).useTemperature) { + if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_temperatures, servparamtemp_ptr, 5.0)) + { + yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()"; + cleanup(); + return false; + } } // we always prepare the fullscales. @@ -170,27 +173,25 @@ bool embObjFTsensor::open(yarp::os::Searchable &config) yDebug() << getBoardInfo() << "open() correctly starts as service strain"; } } - - if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_temperatures)) - { - yError() << getBoardInfo() << "open() fails to start service temperature"; - cleanup(); - return false; - } - else - { - if(GET_privData(mPriv).isVerbose()) + if (GET_privData(mPriv).useTemperature) { + if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_temperatures)) + { + yError() << getBoardInfo() << "open() fails to start service temperature"; + cleanup(); + return false; + } + else if(GET_privData(mPriv).isVerbose()) { yDebug() << getBoardInfo() << "open() correctly starts as service temperature"; } - } - // start the configured sensors. so far, we must keep it in here. later on we can remove this command - if(!enableTemperatureTransmission(true)) - { - yError() << getBoardInfo() << "open() fails to enable temperature transmission"; - cleanup(); - return false; + // start the configured sensors. so far, we must keep it in here. later on we can remove this command + if(!enableTemperatureTransmission(true)) + { + yError() << getBoardInfo() << "open() fails to enable temperature transmission"; + cleanup(); + return false; + } } GET_privData(mPriv).setOpen(true); @@ -206,8 +207,10 @@ bool embObjFTsensor::close() void embObjFTsensor::cleanup(void) { - // disable temperature - enableTemperatureTransmission(false); + if (GET_privData(mPriv).useTemperature) { + // disable temperature + enableTemperatureTransmission(false); + } GET_privData(mPriv).cleanup(static_cast (this)); } @@ -396,7 +399,7 @@ eth::iethresType_t embObjFTsensor::type() // ---------------------- ITemperatureSensors -------------------------------------------------------- size_t embObjFTsensor::getNrOfTemperatureSensors() const { - return 1; + return GET_privData(mPriv).useTemperature ? 1 : 0; } yarp::dev::MAS_status embObjFTsensor::getTemperatureSensorStatus(size_t sens_index) const diff --git a/src/libraries/icubmod/embObjFTsensor/eo_ftsens_privData.cpp b/src/libraries/icubmod/embObjFTsensor/eo_ftsens_privData.cpp index 258403c6bd..b07d30a990 100644 --- a/src/libraries/icubmod/embObjFTsensor/eo_ftsens_privData.cpp +++ b/src/libraries/icubmod/embObjFTsensor/eo_ftsens_privData.cpp @@ -38,23 +38,23 @@ eo_ftsens_privData::~eo_ftsens_privData() bool eo_ftsens_privData::fromConfig(yarp::os::Searchable &_config, servConfigFTsensor_t &serviceConfig) { - + ServiceParser* parser = new ServiceParser; bool ret = parser->parseService(_config, serviceConfig); delete parser; - + if(!ret) { yError() << getBoardInfo() << "is missing some configuration parameter. Check logs and your config file."; return false; } - + useCalibValues = serviceConfig.useCalibration; if(serviceConfig.temperatureAcquisitionrate > 0) useTemperature = true; devicename = serviceConfig.nameOfStrain; frameName = serviceConfig.frameName; - + if(isVerbose()) printServiceConfig(serviceConfig); return ret; @@ -79,13 +79,13 @@ bool eo_ftsens_privData::fillScaleFactor(servConfigFTsensor_t &serviceConfig) { return true; } - + // at first we set the scale factors to 1, so that we are sure they have a safe value. it redundant, as we already set it to 1.0 for(size_t i = 0; isetLocalValue(id32_fullscale, &fullscale_values, overrideROprotection); - - + + // Prepare analog sensor eOas_strain_config_t strainConfig = {0}; strainConfig.datarate = serviceConfig.acquisitionrate; strainConfig.mode = eoas_strainmode_acquirebutdonttx; strainConfig.signaloncefullscale = eobool_true; - + eOprotID32_t id32_strain_config = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config); - + timeout = 5; - + // wait for response while(!gotFullScaleValues && (timeout != 0)) { @@ -155,33 +155,33 @@ bool eo_ftsens_privData::fillScaleFactor(servConfigFTsensor_t &serviceConfig) // If data arrives, size is bigger than zero //#warning --> marco.accame says: to wait for 1 sec and read size is ok. a different way is to ... wait for a semaphore incremented by the reply of the board. think of it! NVsize = eo_array_Size((EOarray *)&fullscale_values); - + if(0 != NVsize) { gotFullScaleValues = true; break; } - + timeout--; if(isVerbose()) { yWarning() << getBoardInfo() << "filling ScaleFactor ...."; } } - + if((false == gotFullScaleValues) && (0 == timeout)) { yError() << getBoardInfo() << "fillScaleFactor(): ETH Analog sensor: request for calibration parameters timed out "; return false; } - + if((strain_Channels != NVsize)) { yError() << getBoardInfo() << "Analog sensor Calibration data has a different size from channels number in configuration file "; return false; } - - + + if(gotFullScaleValues) { if(isVerbose()) @@ -189,7 +189,7 @@ bool eo_ftsens_privData::fillScaleFactor(servConfigFTsensor_t &serviceConfig) yWarning() << getBoardInfo() << "fillScaleFactor() detected that already has full scale values"; yDebug() << getBoardInfo() << "fillScaleFactor(): Fullscale values are: size=" << eo_array_Size((EOarray *)&fullscale_values) << " numchannel=" << strain_Channels; } - + for (size_t i = 0; iconvert(serviceConfig.ethservice.configuration.data.as.strain.canloc, loc, sizeof(loc)); parser->convert(serviceConfig.ethservice.configuration.data.as.strain.boardtype.firmware, fir, sizeof(fir)); parser->convert(serviceConfig.ethservice.configuration.data.as.strain.boardtype.protocol, pro, sizeof(pro)); - + yInfo() << "The embObjFTsensor device using BOARD" << boardname << " w/ IP" << ipv4 << "has the following service config:"; yInfo() << "- acquisitionrate =" << serviceConfig.acquisitionrate; yInfo() << "- useCalibration =" << serviceConfig.useCalibration; @@ -272,15 +272,15 @@ void eo_ftsens_privData::printServiceConfig(servConfigFTsensor_t &serviceConfig) bool eo_ftsens_privData::sendConfig2Strain(servConfigFTsensor_t &serviceConfig) { eOas_strain_config_t strainConfig = {0}; - + strainConfig.datarate = serviceConfig.acquisitionrate; strainConfig.signaloncefullscale = eobool_false; strainConfig.mode = (true == serviceConfig.useCalibration) ? (eoas_strainmode_txcalibrateddatacontinuously) : (eoas_strainmode_txuncalibrateddatacontinuously); - + // version with read-back - + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, 0, eoprot_tag_as_strain_config); - + if(false == res->setcheckRemoteValue(id32, &strainConfig, 10, 0.010, 0.050)) { yError() << getBoardInfo() << "FATAL: sendConfig2Strain() had an error while calling setcheckRemoteValue() for strain config "; @@ -293,11 +293,11 @@ bool eo_ftsens_privData::sendConfig2Strain(servConfigFTsensor_t &serviceConfig) yDebug() << getBoardInfo() << "sendConfig2Strain() correctly configured strain coinfig "; } } - - //configure the service of temperature + + //configure the service of temperature id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_temperature, 0, eoprot_tag_as_temperature_config); - - + + eOas_temperature_config_t tempconfig = {0}; if(serviceConfig.temperatureAcquisitionrate > 0) { diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index 71b8282a53..99c2d7d5c1 100755 --- a/src/libraries/icubmod/embObjLib/serviceParser.cpp +++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp @@ -1537,7 +1537,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon 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)) + if( (eobrd_cantype_strain != thestrain_props.type) && (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; @@ -1570,10 +1570,17 @@ bool ServiceParser::parseService(Searchable &config, servConfigFTsensor_t &ftcon 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")); + // Only strain2 and strain2c has temperature sensors. if(b_SETTINGS_temp.isNull()) { - yError() << "ServiceParser::parseService() for embObjFTsensor device cannot find SETTINGS.temperature-acquisitionRate"; - return false; + if (eobrd_cantype_strain != thestrain_props.type) { + yError() << "ServiceParser::parseService() for embObjFTsensor device cannot find SETTINGS.temperature-acquisitionRate"; + return false; + } + else { + // If set to -1, embObjFTSensor disable the temperature. + ftconfig.temperatureAcquisitionrate = -1; + } } else {