From 27c02ef28b9a17be5ac5f8f7d59315729531d7e8 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Thu, 16 Nov 2023 13:43:21 +0100 Subject: [PATCH] small improvements in eobObjParser (wip) --- .../embObjMotionControl.cpp | 62 +++++++++++-------- .../embObjMotionControl/embObjMotionControl.h | 9 ++- .../embObjMotionControl/eomcParser.cpp | 39 +++++------- .../icubmod/embObjMotionControl/eomcParser.h | 5 +- 4 files changed, 55 insertions(+), 60 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 09ab497e29..d35d9713ca 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -99,7 +99,6 @@ bool embObjMotionControl::alloc(int nj) _gearbox_M2J = allocAndCheck(nj); _gearbox_E2J = allocAndCheck(nj); _deadzone = allocAndCheck(nj); - _temperatureValues = allocAndCheck(nj); _foc_based_info= allocAndCheck(nj); _trj_pids= new eomc::PidInfo[nj]; //_dir_pids= new eomc::PidInfo[nj]; @@ -150,7 +149,6 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_gearbox_M2J); checkAndDestroy(_gearbox_E2J); checkAndDestroy(_deadzone); - checkAndDestroy(_temperatureValues); checkAndDestroy(_impedance_limits); checkAndDestroy(checking_motiondone); checkAndDestroy(_ref_command_positions); @@ -226,7 +224,6 @@ embObjMotionControl::embObjMotionControl() : _gearbox_M2J = 0; _gearbox_E2J = 0; _deadzone = 0; - _temperatureValues = NULL; opened = 0; _trj_pids = NULL; //_dir_pids = NULL; @@ -988,15 +985,15 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) for (j = 0; j < _njoints; j++) { - if (((_foc_based_info[j].temperatureSensorType != motor_temperature_sensor_none ) || (_foc_based_info[j].hasTempSensor != 0)) && ((_temperatureLimits[j].hardwareTemperatureLimit == 0) || (_temperatureLimits[j].warningTemperatureLimit == 0))) + if (((_temperatureSensorsVector.at(j)->getType() != motor_temperature_sensor_none )) && ((_temperatureLimits[j].hardwareTemperatureLimit == 0) || (_temperatureLimits[j].warningTemperatureLimit == 0))) { yError() << "In" << getBoardInfo() << "joint" << j << ": inconsistent configuration, please update it. If Temperature limits are not set then TemperatureSensorType must be NONE or not set and/or HasTempSensor must be zero. Aborting..."; return false; } - if (_foc_based_info[j].temperatureSensorType == motor_temperature_sensor_none) + if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_none) { - yWarning() << "embObjMC " << getBoardInfo() << "joint " << j << " has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly"; + yInfo() << "embObjMC " << getBoardInfo() << "joint " << j << " has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly"; } } } @@ -1576,10 +1573,10 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_joint_status_debug == tag) && (joint < mcdiagnostics.ports.size())) { - eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core); + eOprotID32_t id32sc = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, joint, eoprot_tag_mc_joint_status_core); eOmc_joint_status_core_t jcore = {}; - res->getLocalValue(id32, &jcore); + res->getLocalValue(id32sc, &jcore); int32_t *debug32 = reinterpret_cast(rxdata); // write into relevant port @@ -1596,7 +1593,7 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda } } - if((eoprot_entity_mc_motor == ent) && (eoprot_tag_mc_motor_status_basic == tag)) + if((eoprot_entity_mc_joint == ent) && (eoprot_tag_mc_motor_status_basic == tag)) { if(false == initialised()) return true; @@ -1605,26 +1602,37 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda if((_temperatureSensorsVector.at(motor)->getType() == motor_temperature_sensor_none)) return true; - eOmc_motor_status_basic_t *mc_motor_status_basic = reinterpret_cast(rxdata); - if((double)mc_motor_status_basic->mot_temperature != temperatureErrorValue_s) //I get a valid value + eOmc_motor_status_basic_t mc_motor_status_basic = {}; + id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, eoprot_tag_mc_motor_status_basic); + + if (!res->getLocalValue(id32, &mc_motor_status_basic)) + { + yError() << getBoardInfo() << "getLocalValue() cannot retrieve motor" << motor << "status basic"; + return true; //do we need to actually return false here since is eth error + } + + //eOmc_motor_status_basic_t *mc_motor_status_basic = reinterpret_cast(rxdata); + + if((double)mc_motor_status_basic.mot_temperature != temperatureErrorValue_s) //I get a valid value { - double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((double)mc_motor_status_basic->mot_temperature); + double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((double)mc_motor_status_basic.mot_temperature); + if (tmp > _temperatureLimits[motor].warningTemperatureLimit) { if(! _temperatureExceededLimitWatchdog.at(motor).isStarted()) { - yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[℃] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[℃] ). Processes not stopped but it i strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging"; - _temperatureExceededLimitWatchdog.at(motor).start(); + yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[ ℃ ] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[ ℃ ] ). Processes not stopped but it is strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging"; + _temperatureExceededLimitWatchdog.at(motor).start(); } else + { + if(_temperatureExceededLimitWatchdog.at(motor).isExpired()) { - if(_temperatureExceededLimitWatchdog.at(motor).isExpired()) - { - yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[℃] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[℃] ) again!. Processes not stopped but it i strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging"; - _temperatureExceededLimitWatchdog.at(motor).start(); - } - _temperatureExceededLimitWatchdog.at(motor).decrement(); + yWarning() << getBoardInfo() << "Motor" << motor << "The temperature (" << tmp << "[ ℃ ] ) exceeds the warning limit (" << _temperatureLimits[motor].warningTemperatureLimit << "[ ℃ ] ) again!. Processes not stopped but it is strongly recommended decreasing motor usage or reducing currents and PWMs to not risk motor damaging"; + _temperatureExceededLimitWatchdog.at(motor).start(); } + _temperatureExceededLimitWatchdog.at(motor).increment(); + } } else { @@ -1636,16 +1644,16 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda if(! _temperatureSensorErrorWatchdog.at(motor).isStarted()) { yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << motor << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable"; - _temperatureSensorErrorWatchdog.at(motor).start(); + _temperatureSensorErrorWatchdog.at(motor).start(); } else { + _temperatureSensorErrorWatchdog.at(motor).increment(); if( _temperatureSensorErrorWatchdog.at(motor).isExpired()) { - yError()<< getBoardInfo() << "Motor" << motor << "gets " << _temperatureSensorErrorWatchdog.at(motor).getCount() << "reading errors in " << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds"; + yError()<< getBoardInfo() << "Motor" << motor << "failed to read temperature for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds"; _temperatureSensorErrorWatchdog.at(motor).start(); } - _temperatureSensorErrorWatchdog.at(motor).decrement(); } } } @@ -3361,11 +3369,11 @@ bool embObjMotionControl::getTemperatureSensorTypeRaw(int j, std::string& ret) { // refresh cached value when reading data from the EMS ret = "NONE"; - if (_foc_based_info[j].temperatureSensorType = motor_temperature_sensor_pt100) + if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_pt100) { ret = "PT100"; } - else if (_foc_based_info[j].temperatureSensorType = motor_temperature_sensor_pt1000) + else if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_pt1000) { ret = "PT1000"; } @@ -4801,13 +4809,13 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) bool ret = res->getLocalValue(protid, &status); if(!ret) { - yError() << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue() for " << getBoardInfo() << " Setting mot_temperature" << m << "to" << *val; + yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue()"; return ret; } if (((double)status.mot_temperature) == temperatureErrorValue_s) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated { - 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"; + yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "cannot read Temperature" << *val << "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 40de8906b0..ef62da57c2 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -119,8 +119,8 @@ double _time; public: -Watchdog(): _count(1000), _isStarted(false), _threshold(1000), _time(0){;} -Watchdog(uint32_t threshold):_count(threshold), _isStarted(false),_threshold(threshold), _time(0){;} +Watchdog(): _count(0), _isStarted(false), _threshold(500), _time(0){;} +Watchdog(uint32_t threshold):_count(0), _isStarted(false),_threshold(threshold), _time(0){;} ~Watchdog() = default; Watchdog(const Watchdog& other) = default; Watchdog(Watchdog&& other) noexcept = default; @@ -129,9 +129,9 @@ Watchdog& operator=(Watchdog&& other) noexcept = default; bool isStarted(){return _isStarted;} -void start() {_count = _threshold; _time = yarp::os::Time::now();} +void start() {_count = 0; _time = yarp::os::Time::now(); _isStarted = true;} bool isExpired() {return (_count > _threshold);} -void decrement() {_count--;} +void increment() {++_count;} void clear(){_isStarted=false;} double getStartTime() {return _time;} uint32_t getCount() {return _count; } @@ -236,7 +236,6 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, double * _gearbox_M2J; /** the gearbox ratio motor to joint */ double * _gearbox_E2J; /** the gearbox ratio encoder to joint */ double * _deadzone; - double * _temperatureValues; std::vector _kalman_params; /** Kalman filter parameters */ std::vector> _temperatureSensorsVector; diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index b023aa97ce..da064150f5 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -1376,23 +1376,9 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints, false)) { - yWarning("In board %s group TemperatureSensorType not filled. Setting it to NONE as default value. If needed update the configuration files accordingly", _boardname.c_str()) ; - for (i = 0; i < (unsigned)_njoints; i++) - { - foc_based_info[i].temperatureSensorType = motor_temperature_sensor_none; - foc_based_info[i].hasTempSensor = 0; - temperatureSensorsVector.at(i) = std::make_unique(); - } - - if (!extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints, false)) - { - yWarning("In board %s group HasTempSensor not filled. Setting it to 0 as default value. If needed update the configuration files accordingly", _boardname.c_str()); - for (i = 0; i < (unsigned)_njoints; i++) - foc_based_info[i].hasTempSensor = 0; - } - else + // 1. check if I have old config + if (extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints, false)) { - yWarning() << "ATTENTION HasTempSensor will be soon DEPRECATED in favour of TemperatureSensorType. Currently kept for backward compatibility but update your configuration files if using a Temperature Sensor"; for (i = 1; i < xtmp.size(); i++) { if (xtmp.get(i).asInt32() != 0) @@ -1400,13 +1386,18 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI yError() << "In " << _boardname << "entry" << i << ": inconsistent configuration. HasTempSensor cannot be used alone. Will be soon deprecated. Use TemperatureSensorType in 2FOC group and set Temperature limits in LIMITS group." ; return false; } - else - { - foc_based_info[i - 1].hasTempSensor = xtmp.get(i).asInt32(); - } } } - } + + // if I'm here all joints have HasTempSensor =0 + for (i = 0; i < _njoints; i++) + { + foc_based_info[i].hasTempSensor = 0; + temperatureSensorsVector.at(i) = std::make_unique(); + yWarning()<< _boardname << "ATTENTION HasTempSensor will be soon DEPRECATED in favour of TemperatureSensorType (PT100, PT1000, NONE(=default)). Currently kept for backward compatibility but update your configuration files if using a Temperature Sensor"; + } + + } else { for (i = 1; i < xtmp.size(); i++) @@ -1415,7 +1406,6 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI if(s == "PT100") { foc_based_info[i - 1].hasTempSensor = 1; - foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt100; temperatureSensorsVector.at(i-1) = std::make_unique(); } @@ -1423,14 +1413,13 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI { foc_based_info[i - 1].hasTempSensor = 1; - foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt1000; temperatureSensorsVector.at(i-1) = std::make_unique(); } else { - yWarning("Not available or Not supported TemperatureSensorType: %s. Setting NONE as default", s.c_str()); + if(s != "NONE")//if sis == NONE the warning is not correct + yWarning("Not available or Not supported TemperatureSensorType: %s. Setting NONE as default", s.c_str()); foc_based_info[i - 1].hasTempSensor = 0; - foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_none; temperatureSensorsVector.at(i-1) = std::make_unique(); } } diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index ae62e67518..88839859bc 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -256,14 +256,14 @@ class TemperatureSensorNONE : public ITemperatureSensor TemperatureSensorNONE() { - yError("Private varibales NOT DEFINED for class TemperatureSensorNONE"); + yInfo("Private varibales NOT DEFINED for class TemperatureSensorNONE"); } ~TemperatureSensorNONE() = default; virtual double convertTempCelsiusToRaw(const double temperature) override { - yError("convertTempCelsiusToRaw METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); + yInfo("convertTempCelsiusToRaw METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); return 0; } @@ -425,7 +425,6 @@ typedef struct { bool hasHallSensor; bool hasTempSensor; - motor_temperatureSensorTypeEnum_t temperatureSensorType; // uses a typedef enum bool hasRotorEncoder; bool hasRotorEncoderIndex; int rotorIndexOffset;