From 3f195741d223b8b4bfd474d3a0bd66b47797e311 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Fri, 10 Nov 2023 18:08:56 +0100 Subject: [PATCH] Add method for getting temperature update from callback --- .../icubmod/embObjLib/FeatureInterface.cpp | 32 ++++++++++ .../icubmod/embObjLib/FeatureInterface.h | 2 + .../EoProtocolMC_fun_userdef.c | 1 + .../embObjMotionControl.cpp | 61 +++++++++++-------- .../embObjMotionControl/embObjMotionControl.h | 1 + .../icubmod/embObjMotionControl/eomcParser.h | 2 +- 6 files changed, 74 insertions(+), 25 deletions(-) diff --git a/src/libraries/icubmod/embObjLib/FeatureInterface.cpp b/src/libraries/icubmod/embObjLib/FeatureInterface.cpp index 886cb42736..78b6173241 100755 --- a/src/libraries/icubmod/embObjLib/FeatureInterface.cpp +++ b/src/libraries/icubmod/embObjLib/FeatureInterface.cpp @@ -242,6 +242,38 @@ eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, vo return eobool_true; } +eObool_t feat_manage_motor_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *rxdata) +{ + IethResource* mc = nullptr; + + if (_interface2ethManager == nullptr) + { + return eobool_false; + } + + mc = _interface2ethManager->getInterface(ipv4, id32); + + if (nullptr == mc) + { + char ipinfo[20]; + char nvinfo[128]; + eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo)); + eoprot_ID2information(id32, nvinfo, sizeof(nvinfo)); + yDebug("feat_manage_motor_data() fails to get a handle of embObjMotionControl for IP = %s and NV = %s", ipinfo, nvinfo); + return eobool_false; + } + + if(mc->initialised() == false) + { + return eobool_false; + } + else + { + mc->getMotorTemperature(id32, yarp::os::Time::now(), rxdata); + } + + return eobool_true; +} void* feat_MC_handler_get(eOipv4addr_t ipv4, eOprotID32_t id32) { diff --git a/src/libraries/icubmod/embObjLib/FeatureInterface.h b/src/libraries/icubmod/embObjLib/FeatureInterface.h index 009bc84e4f..b2a2d2854d 100755 --- a/src/libraries/icubmod/embObjLib/FeatureInterface.h +++ b/src/libraries/icubmod/embObjLib/FeatureInterface.h @@ -51,6 +51,8 @@ eObool_t feat_manage_skin_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *array eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *data); +eObool_t feat_manage_motor_data(eOipv4addr_t ipv4, eOprotID32_t id32, void* rxdata); + void * feat_MC_handler_get(eOipv4addr_t ipv4, eOprotID32_t id32); double feat_yarp_time_now(void); diff --git a/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c b/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c index 95aa8dd206..bf75837a2e 100755 --- a/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c +++ b/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c @@ -83,6 +83,7 @@ extern void eoprot_fun_UPDT_mc_joint_status_debug(const EOnv* nv, const eOropdes extern void eoprot_fun_UPDT_mc_motor_status_basic(const EOnv* nv, const eOropdescriptor_t* rd) { + feat_manage_motor_data(eo_nv_GetIP(nv), rd->id32); } diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 5e70c02f25..83af066e15 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -137,7 +137,7 @@ bool embObjMotionControl::alloc(int nj) _jointEncs.resize(nj); _motorEncs.resize(nj); _kalman_params.resize(nj); - _temperatureSensorsVector.resize(nj); + _temperatureSensorsVector.resize(nj, nullptr); _temperatureSensorErrorWatchdog.resize(nj, 1000); // use 1000 as limit on the watchdog for the error on the temperature sensor receiving of the values - // since the ETH callback timing is 5ms by default so using 1000s we can set a checking threshould of 1 second // in which we can allow the tdb to not respond. If cannot receive response over 1s we trigger the error @@ -1006,6 +1006,14 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) } } } + else + { + for (j = 0; j < _njoints; j++) + { + temperatureSensorsVector.at(j) = std::make_unique(); + } + } + /////// [TIMEOUTS] @@ -1560,26 +1568,6 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda { // do it only if we already have opened the device std::lock_guard lck(_mutex); _encodersStamp[joint] = timestamp; - - - { - if(!getTemperaturesRaw(_temperatureValues)) - { - // yError() << "embObjMotionControl::getTemperaturesRaw failed for" << getBoardInfo(); - return false; - } - else - { - for (uint8_t i = 0; i < _njoints; i++) - { - if (_temperatureValues[i] > _temperatureLimits[i].warningTemperatureLimit) - { - yWarning() << getBoardInfo() << "At joint" << joint << "temperature limit for motor" << i << " overcame! Processes not stopped but consider to decrese motor usage or reduce currents and PWMs to not risk motor damaging"; - } - } - } - } - } @@ -1633,6 +1621,31 @@ bool embObjMotionControl::getEntityName(uint32_t entityId, std::string &entityNa } +bool embObjMotionControl::getMotorTemperature(eOprotID32_t id32, double timestamp, void *rxdata) +{ + // use this function to update the values cached in the class using data received by the remote boards via the network callbacks + // this function will specifically get the motor data (for now only the temperature) + size_t motor = eoprot_ID2index(id32); + + if(true == initialised()) + { + if(!getTemperaturesRaw(_temperatureValues)) + { + yError() << "embObjMotionControl::getTemperaturesRaw failed for" << getBoardInfo() << "at motor" << motor; + return false; + } + else + { + if (_temperatureValues[motor] > _temperatureLimits[motor].warningTemperatureLimit) + { + yWarning() << getBoardInfo() << "Temperature limit for motor" << motor << " overcame! Processes not stopped but consider to decrese motor usage or reduce currents and PWMs to not risk motor damaging"; + } + } + } + + return true; +} + ///////////// PID INTERFACE bool embObjMotionControl::setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) { @@ -4761,11 +4774,11 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) { if (((double)status.mot_temperature) != -5000) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated { - if ((_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none) && (serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc)) + //if ((_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none) && (serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc)) + if(_temperatureSensorsVector.at(m) != nullptr) { *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature); } - } else { @@ -4773,7 +4786,7 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) if(_temperatureSensorErrorWatchdog.at(m) < 0) { _temperatureSensorErrorWatchdog.at(m) = 1000; - yError() << getBoardInfo() << "At protid" << protid << "In motor" << m << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable"; + yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable"; return false; } diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index a3009b6a93..fd2d71b91f 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -364,6 +364,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, virtual eth::iethresType_t type(); virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata); virtual bool getEntityName(uint32_t entityId, std::string &entityName); + virtual bool getMotorTemperature(eOprotID32_t id32, double timestamp, void *rxdata); ///////// PID INTERFACE ///////// virtual bool setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) override; diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index afb2956835..a1759223fa 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -262,7 +262,7 @@ class TemperatureSensorNONE : public ITemperatureSensor virtual double convertRawToTempCelsius(const double temperature) override { - yError("convertRawToTempCelsius METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); + //yError("convertRawToTempCelsius METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); return 0; } };