From ae899c9eefa3445ead14523f2b5bd755351a036d Mon Sep 17 00:00:00 2001 From: Jacopo Losi <77933235+MSECode@users.noreply.github.com> Date: Tue, 28 Nov 2023 14:32:45 +0100 Subject: [PATCH] Feature/temperature reading (#911) --- conf/iCubFindDependencies.cmake | 4 +- .../icubmod/embObjLib/FeatureInterface.cpp | 1 - .../icubmod/embObjLib/hostTransceiver.cpp | 7 + .../EoProtocolMC_fun_userdef.c | 3 +- .../embObjMotionControl.cpp | 193 +++++++++++++--- .../embObjMotionControl/embObjMotionControl.h | 43 +++- .../embObjMotionControl/eomcParser.cpp | 111 ++++++++- .../icubmod/embObjMotionControl/eomcParser.h | 216 +++++++++++++++++- 8 files changed, 525 insertions(+), 53 deletions(-) diff --git a/conf/iCubFindDependencies.cmake b/conf/iCubFindDependencies.cmake index 3e390570e9..ccead54e52 100644 --- a/conf/iCubFindDependencies.cmake +++ b/conf/iCubFindDependencies.cmake @@ -65,8 +65,8 @@ checkandset_dependency(OpenCV) checkandset_dependency(Qt5) if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared) - if(icub_firmware_shared_VERSION VERSION_LESS 1.37.1) - message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.37.1 is required") + if(icub_firmware_shared_VERSION VERSION_LESS 1.37.2) + message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.37.2 is required") endif() endif() diff --git a/src/libraries/icubmod/embObjLib/FeatureInterface.cpp b/src/libraries/icubmod/embObjLib/FeatureInterface.cpp index 886cb42736..fdfd622023 100755 --- a/src/libraries/icubmod/embObjLib/FeatureInterface.cpp +++ b/src/libraries/icubmod/embObjLib/FeatureInterface.cpp @@ -242,7 +242,6 @@ eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, vo return eobool_true; } - void* feat_MC_handler_get(eOipv4addr_t ipv4, eOprotID32_t id32) { IethResource* h = NULL; diff --git a/src/libraries/icubmod/embObjLib/hostTransceiver.cpp b/src/libraries/icubmod/embObjLib/hostTransceiver.cpp index 440b974729..7298636a7b 100644 --- a/src/libraries/icubmod/embObjLib/hostTransceiver.cpp +++ b/src/libraries/icubmod/embObjLib/hostTransceiver.cpp @@ -847,6 +847,13 @@ void HostTransceiver::eoprot_override_mc(void) EO_INIT(.tag) eoprot_tag_mc_joint_status_addinfo_multienc, EO_INIT(.init) NULL, EO_INIT(.update) eoprot_fun_UPDT_mc_joint_status_addinfo_multienc + }, + { // joint_status_basic: used to inform the motioncontrol device that a sig<> ROP about joint status has arrived. for .. updating the same timestamps as above + EO_INIT(.endpoint) eoprot_endpoint_motioncontrol, + EO_INIT(.entity) eoprot_entity_mc_motor, + EO_INIT(.tag) eoprot_tag_mc_motor_status, + EO_INIT(.init) NULL, + EO_INIT(.update) eoprot_fun_UPDT_mc_motor_status } // motor // nothing so far diff --git a/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c b/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c index 95aa8dd206..e4c886e77b 100755 --- a/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c +++ b/src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c @@ -81,8 +81,9 @@ extern void eoprot_fun_UPDT_mc_joint_status_debug(const EOnv* nv, const eOropdes feat_manage_motioncontrol_data(eo_nv_GetIP(nv), rd->id32, (void *)rd->data); } -extern void eoprot_fun_UPDT_mc_motor_status_basic(const EOnv* nv, const eOropdescriptor_t* rd) +extern void eoprot_fun_UPDT_mc_motor_status(const EOnv* nv, const eOropdescriptor_t* rd) { + feat_manage_motioncontrol_data(eo_nv_GetIP(nv), rd->id32, (void *)rd->data); } diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index f3c0d76659..5bee762ce6 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include "embObjMotionControl.h" #include @@ -72,10 +73,7 @@ static bool nv_not_found(void) return false; } - - - - +static constexpr double const temperatureErrorValue_s = -5000; @@ -101,7 +99,7 @@ bool embObjMotionControl::alloc(int nj) _gearbox_M2J = allocAndCheck(nj); _gearbox_E2J = allocAndCheck(nj); _deadzone = allocAndCheck(nj); - _foc_based_info=allocAndCheck(nj); + _foc_based_info= allocAndCheck(nj); _trj_pids= new eomc::PidInfo[nj]; //_dir_pids= new eomc::PidInfo[nj]; _trq_pids= new eomc::TrqPidInfo [nj]; @@ -123,10 +121,10 @@ bool embObjMotionControl::alloc(int nj) _calibrated = allocAndCheck(nj); _cacheImpedance = allocAndCheck(nj); - _rotorsLimits.resize(nj); _jointsLimits.resize(nj); _currentLimits.resize(nj); + _temperatureLimits.resize(nj); _jsets.resize(nj); _joint2set.resize(nj); _timeouts.resize(nj); @@ -135,9 +133,10 @@ bool embObjMotionControl::alloc(int nj) _jointEncs.resize(nj); _motorEncs.resize(nj); _kalman_params.resize(nj); + _temperatureSensorsVector.resize(nj); + _temperatureExceededLimitWatchdog.resize(nj); + _temperatureSensorErrorWatchdog.resize(nj); - //debug purpose - return true; } @@ -161,7 +160,6 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_calibrated); checkAndDestroy(_foc_based_info); - if(_trj_pids) delete [] _trj_pids; @@ -208,6 +206,7 @@ embObjMotionControl::embObjMotionControl() : _rotorsLimits(0), _jointsLimits(0), _currentLimits(0), + _temperatureLimits(0), _jsets(0), _joint2set(0), _timeouts(0), @@ -215,7 +214,10 @@ embObjMotionControl::embObjMotionControl() : _axesInfo(0), _jointEncs(0), _motorEncs(0), - _kalman_params(0) + _kalman_params(0), + _temperatureSensorsVector(0), + _temperatureExceededLimitWatchdog(0), + _temperatureSensorErrorWatchdog(0) { _gearbox_M2J = 0; _gearbox_E2J = 0; @@ -814,7 +816,7 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) if (!_mcparser->parseAmpsToSensor(config, measConvFactors.ampsToSensor)) return false; - + //VALE: i have to parse GeneralMecGroup after parsing jointsetcfg, because inside generalmec group there is useMotorSpeedFbk that needs jointset info. if(!_mcparser->parseGearboxValues(config, _gearbox_M2J, _gearbox_E2J)) @@ -962,6 +964,9 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) if(!_mcparser->parseCurrentLimits(config, _currentLimits)) return false; + if(!_mcparser->parseTemperatureLimits(config, _temperatureLimits)) + return false; + if(!_mcparser->parseJointsLimits(config, _jointsLimits)) return false; @@ -973,9 +978,31 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) if(serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc) { std::string groupName = (static_cast(serviceConfig.ethservice.configuration.data.mc.foc_based.type) == eobrd_foc) ? "2FOC" : "AMCBLDC"; - if(!_mcparser->parseFocGroup(config, _foc_based_info, groupName)) + if(!_mcparser->parseFocGroup(config, _foc_based_info, groupName, _temperatureSensorsVector)) return false; + + for (j = 0; j < _njoints; j++) + { + 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 (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_none) + { + yInfo() << "embObjMC " << getBoardInfo() << "joint " << j << " has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly"; + } + } + } + else + { + for (j = 0; j < _njoints; j++) + { + _temperatureSensorsVector.at(j) = std::make_unique(); + } } + /////// [TIMEOUTS] @@ -1403,7 +1430,8 @@ bool embObjMotionControl::init() motor_cfg.rotorIndexOffset = _foc_based_info[logico].rotorIndexOffset; motor_cfg.rotorEncoderType = _motorEncs[logico].type; motor_cfg.pwmLimit =_rotorsLimits[logico].pwmMax; - motor_cfg.limitsofrotor.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico )); + motor_cfg.temperatureLimit = (eOmeas_temperature_t) S_16(_temperatureSensorsVector.at(logico)->convertTempCelsiusToRaw(_temperatureLimits.at(logico).hardwareTemperatureLimit)); //passing raw value not in degree + motor_cfg.limitsofrotor.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico )); motor_cfg.limitsofrotor.min = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMin, fisico )); yarp::dev::Pid tmp; @@ -1515,6 +1543,8 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda // use this function to update the values cached in the class using data received by the remote boards via the network callbacks // in embObjMotionControl it is updated only the timestamp of the encoders, thuus i dont used rxdata size_t joint = eoprot_ID2index(id32); + eOprotEntity_t ent = eoprot_ID2entity(id32); + eOprotTag_t tag = eoprot_ID2tag(id32); // rxdata = rxdata; @@ -1535,9 +1565,6 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda if(eomn_serv_diagn_mode_MC_AMOyarp == mcdiagnostics.config.mode) { - eOprotEntity_t ent = eoprot_ID2entity(id32); - eOprotTag_t tag = eoprot_ID2tag(id32); - char str[128] = "boh"; eoprot_ID2information(id32, str, sizeof(str)); @@ -1545,10 +1572,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 @@ -1565,6 +1592,62 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda } } + if((eoprot_entity_mc_motor == ent) && (eoprot_tag_mc_motor_status == tag)) + { + if(false == initialised()) + return true; + + uint8_t motor = eoprot_ID2index(id32); + if((_temperatureSensorsVector.at(motor)->getType() == motor_temperature_sensor_none)) + return true; + + eOmc_motor_status_t *mc_motor_status = 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); + + 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 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()) + { + 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 + { + _temperatureExceededLimitWatchdog.at(motor).clear(); + } + } + else //I get a NOT valid value + { + 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(); + } + else + { + _temperatureSensorErrorWatchdog.at(motor).increment(); + if( _temperatureSensorErrorWatchdog.at(motor).isExpired()) + { + yError()<< getBoardInfo() << "Motor" << motor << "failed to read temperature for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds"; + _temperatureSensorErrorWatchdog.at(motor).start(); + } + } + } + } + return true; } @@ -3270,7 +3353,27 @@ bool embObjMotionControl::getKinematicMJRaw(int j, double &rotres) return false; } -bool embObjMotionControl::getHasTempSensorsRaw(int j, int& ret) +bool embObjMotionControl::getTemperatureSensorTypeRaw(int j, std::string& ret) +{ + // refresh cached value when reading data from the EMS + ret = "NONE"; + if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_pt100) + { + ret = "PT100"; + } + else if (_temperatureSensorsVector.at(j)->getType() == motor_temperature_sensor_pt1000) + { + ret = "PT1000"; + } + else + { + ret = "NONE"; + } + + return true; +} + +bool embObjMotionControl::getHasTempSensorsRaw(int j, int& ret) { eOprotID32_t protoid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, j, eoprot_tag_mc_motor_config); uint16_t size; @@ -3456,7 +3559,12 @@ bool embObjMotionControl::getRemoteVariableRaw(std::string key, yarp::os::Bottle } else if (key == "hasTempSensor") { - Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { int tmp = 0; getHasTempSensorsRaw(i, tmp); r.addInt32(tmp); } + Bottle& r = val.addList(); for (int i = 0; i < _njoints; i++) { int tmp = 0; getHasTempSensorsRaw(i, tmp); r.addInt32(tmp); } + return true; + } + else if (key == "TemperatureSensorType") + { + Bottle& r = val.addList(); for (int i = 0; i<_njoints; i++) { std::string tmp = ""; getTemperatureSensorTypeRaw(i, tmp); r.addString(tmp); } return true; } else if (key == "hasRotorEncoder") @@ -3752,6 +3860,7 @@ bool embObjMotionControl::getRemoteVariablesListRaw(yarp::os::Bottle* listOfKeys listOfKeys->addString("gearbox_E2J"); listOfKeys->addString("hasHallSensor"); listOfKeys->addString("hasTempSensor"); + listOfKeys->addString("TemperatureSensorType"); listOfKeys->addString("hasRotorEncoder"); listOfKeys->addString("hasRotorEncoderIndex"); listOfKeys->addString("rotorIndexOffset"); @@ -4681,17 +4790,27 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) eOmc_motor_status_basic_t status; eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic); + // if (_temperatureSensorsVector.at(m) == nullptr || (_temperatureSensorsVector.at(motor)->getType() == motor_temperature_sensor_none)) + *val = NAN; + if (_temperatureSensorsVector.at(m)->getType() == motor_temperature_sensor_none) + return true; + + bool ret = res->getLocalValue(protid, &status); - if(ret) + if(!ret) { - *val = (double) status.mot_temperature; + yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue()"; + return ret; } - else + + 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() << "embObjMotionControl::getTemperatureRaw failed for" << getBoardInfo() << " motor " << m ; - *val = 0; + return false; } - + + *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature); + + return ret; } @@ -4707,17 +4826,19 @@ bool embObjMotionControl::getTemperaturesRaw(double *vals) bool embObjMotionControl::getTemperatureLimitRaw(int m, double *temp) { - eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit); - uint16_t size; - eOmeas_temperature_t temperaturelimit = {0}; - *temp = 0; - if(!askRemoteValue(protid, &temperaturelimit, size)) - { - yError() << "embObjMotionControl::getTemperatureLimitRaw() can't read temperature limits for" << getBoardInfo() << " motor " << m; - return false; - } + // eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit); + // uint16_t size; + // eOmeas_temperature_t temperaturelimit = {0}; + // *temp = 0; + // if(!askRemoteValue(protid, &temperaturelimit, size)) + // { + // yError() << "embObjMotionControl::getTemperatureLimitRaw() can't read temperature limits for" << getBoardInfo() << " motor " << m; + // return false; + // } + + // *temp = (double) temperaturelimit; - *temp = (double) temperaturelimit; + *temp= _temperatureLimits[m].warningTemperatureLimit; return true; } diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index edac70c9d4..5ce9989704 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -106,7 +106,41 @@ typedef struct bool pwmIsLimited; /** set to true if pwm is limited */ }behaviour_flags_t; -}}}; + +class Watchdog +{ + +private: + +bool _isStarted; +uint32_t _count; +uint32_t _threshold; // use 10000 as limit on the watchdog for the error on the temperature sensor receiving of the values - + // since the ETH callback timing is 2ms by default so using 10000 we can set a checking threshould of 5 second + // in which we can allow the tdb to not respond. If cannot receive response over 1s we trigger the error + +double _time; + +public: + +Watchdog(): _count(0), _isStarted(false), _threshold(10000), _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; +Watchdog& operator=(const Watchdog& other) = default; +Watchdog& operator=(Watchdog&& other) noexcept = default; + + +bool isStarted(){return _isStarted;} +void start() {_count = 0; _time = yarp::os::Time::now(); _isStarted = true;} +bool isExpired() {return (_count > _threshold);} +void increment() {++_count;} +void clear(){_isStarted=false;} +double getStartTime() {return _time;} +uint32_t getCount() {return _count; } + +}; +}}} namespace yarp { namespace dev { @@ -207,6 +241,8 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, double * _deadzone; std::vector _kalman_params; /** Kalman filter parameters */ + std::vector> _temperatureSensorsVector; + eomc::focBasedSpecificInfo_t * _foc_based_info; std::vector _jointEncs; @@ -215,6 +251,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, std::vector _rotorsLimits; /** contains limit about rotors such as position and pwm */ std::vector _jointsLimits; /** contains limit about joints such as position and velocity */ std::vector _currentLimits; + std::vector _temperatureLimits; eomc::couplingInfo_t _couplingInfo; /** contains coupling matrix */ std::vector _jsets; std::vector _joint2set; /** for each joint says the number of set it belongs to */ @@ -265,6 +302,8 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, #define MAX_POSITION_MOVE_INTERVAL 0.080 double *_last_position_move_time; /** time stamp for last received position move command*/ eOmc_impedance_t *_cacheImpedance; /* cache impedance value to split up the 2 sets */ + std::vector _temperatureSensorErrorWatchdog; /* counter used to filter error coming from tdb reading fromm 2FOC board*/ + std::vector _temperatureExceededLimitWatchdog; /* counter used to filter the print of the exeded limits*/ #ifdef NETWORK_PERFORMANCE_BENCHMARK @@ -490,6 +529,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, bool getJointEncoderTypeRaw(int j, int &type) ; bool getRotorEncoderTypeRaw(int j, int &type) ; bool getKinematicMJRaw(int j, double &rotres) ; + bool getTemperatureSensorTypeRaw(int j, std::string& ret) ; bool getHasTempSensorsRaw(int j, int& ret) ; bool getHasHallSensorRaw(int j, int& ret) ; bool getHasRotorEncoderRaw(int j, int& ret) ; @@ -513,6 +553,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, virtual bool getPWMLimitRaw(int j, double* val) override; virtual bool setPWMLimitRaw(int j, const double val) override; virtual bool getPowerSupplyVoltageRaw(int j, double* val) override; + /////////////// END AMPLIFIER INTERFACE // virtual analog sensor diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index b9d221bcc7..9cc7634421 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -1370,14 +1370,14 @@ bool Parser::parsePidUnitsType(Bottle &bPid, yarp::dev::PidFeedbackUnitsEnum &f return true; } -bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificInfo_t *foc_based_info, std::string groupName) +bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificInfo_t *foc_based_info, std::string groupName, std::vector>& temperatureSensorsVector) { - Bottle &focGroup=config.findGroup(groupName); - if (focGroup.isNull() ) - { + Bottle &focGroup=config.findGroup(groupName); + if (focGroup.isNull() ) + { yError() << "embObjMC BOARD " << _boardname << " detected that Group " << groupName << " is not found in configuration file"; return false; - } + } Bottle xtmp; unsigned int i; @@ -1391,14 +1391,56 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI for (i = 1; i < xtmp.size(); i++) foc_based_info[i - 1].hasHallSensor = xtmp.get(i).asInt32() != 0; } - if (!extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints)) + + if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints, false)) { - return false; - } + // 1. check if I have old config + if (extractGroup(focGroup, xtmp, "HasTempSensor", "HasTempSensor 0/1 ", _njoints, false)) + { + for (i = 1; i < xtmp.size(); i++) + { + if (xtmp.get(i).asInt32() != 0) + { + 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; + } + } + } + + // 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++) - foc_based_info[i - 1].hasTempSensor = xtmp.get(i).asInt32() != 0; + { + std::string s = xtmp.get(i).asString(); + if(s == "PT100") + { + foc_based_info[i - 1].hasTempSensor = 1; + temperatureSensorsVector.at(i-1) = std::make_unique(); + + } + else if (s == "PT1000") + { + + foc_based_info[i - 1].hasTempSensor = 1; + temperatureSensorsVector.at(i-1) = std::make_unique(); + } + else + { + 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; + temperatureSensorsVector.at(i-1) = std::make_unique(); + } + } } if (!extractGroup(focGroup, xtmp, "HasRotorEncoder", "HasRotorEncoder 0/1 ", _njoints)) { @@ -1694,6 +1736,57 @@ bool Parser::parseCurrentLimits(yarp::os::Searchable &config, std::vector &temperatureLimits) +{ + Bottle &limits = config.findGroup("LIMITS"); + if (limits.isNull()) + { + yError() << "embObjMC BOARD " << _boardname << " detected that Group LIMITS is not found in configuration file"; + return false; + } + + temperatureLimits.resize(_njoints); + unsigned int i; + Bottle xtmp; + + // hardware limit + if(!extractGroup(limits, xtmp, "hardwareTemperatureLimits", "a list of temperature limits", _njoints, false)) + { + yWarning("hardwareTemperatureLimits param not found in config file for board %s. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. Using default values.", _boardname.c_str()); + for (i = 0; i < (unsigned)_njoints; i++) + { + temperatureLimits[i].hardwareTemperatureLimit = 0; + temperatureLimits[i].warningTemperatureLimit = 0; + } + } + else + { + for (i = 1; i < xtmp.size(); i++) temperatureLimits[i - 1].hardwareTemperatureLimit = xtmp.get(i).asFloat64(); + if (!extractGroup(limits, xtmp, "warningTemperatureLimits", "a list of warning temperature limits", _njoints, false)) + { + // warning limit - parsing it only if hardwareTemperatureLimit available + yWarning("warningTemperatureLimits param not found in config file for board %s. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. Using default values.", _boardname.c_str()); + + for (i = 0; i < (unsigned)_njoints; i++) temperatureLimits[i].warningTemperatureLimit = 0; + } + else + { + for (i = 1; i < xtmp.size(); i++) temperatureLimits[i - 1].warningTemperatureLimit = xtmp.get(i).asFloat64(); + } + } + + //Now I verify that warning temperature limits is < 85% of hardwareTemperatureLimit. + for (i = 0; i < (unsigned)_njoints; i++) + { + if(temperatureLimits[i].warningTemperatureLimit > (0.85 * temperatureLimits[i].hardwareTemperatureLimit)) + { + yError() << "In " << _boardname << "joint " << i << ": inconsistent temperature limits. warningTemperatureLimit must be smaller than 85% of hardwareTemperatureLimit" ; + return false; + } + } + return true; +} + bool Parser::parseJointsLimits(yarp::os::Searchable &config, std::vector &jointsLimits) { Bottle &limits=config.findGroup("LIMITS"); diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index 0475fcc05b..e0c34a7e59 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -26,6 +26,7 @@ #include #include #include +#include // Yarp stuff #include @@ -56,6 +57,209 @@ namespace yarp { // PidAlgo_currentInnerLoop =2 //} PidAlgorithmType_t; +typedef enum +{ + motor_temperature_sensor_pt100 = 0, + motor_temperature_sensor_pt1000 = 1, + motor_temperature_sensor_none = 255 +} motor_temperatureSensorTypeEnum_t; + +class ITemperatureSensor +{ + +public: + + virtual double convertTempCelsiusToRaw(const double temperature) = 0; + + virtual double convertRawToTempCelsius(const double temperature) = 0; + + virtual motor_temperatureSensorTypeEnum_t getType() = 0; +}; + +class TemperatureSensorPT100 : public ITemperatureSensor +{ +private: + // resistors of the voltage divider bridge + int _r_1; + int _r_2; + int _r_3; + + + double _ptc_offset; // offset of the temperature sensor line + double _ptc_gradient; // slope/gradient of the temperature sensor line + double _pga_gain; // ADC gain set for the tdb (temperature detection board) + + int _vcc; // vcc that enters the voltage divider bridge + double _resolution_pga; // resolution of the internal pga of the tdb + double _resolution_tdb; // resolution used for the raw value for the output of the tdb + + double _half_bridge_resistor_coeff; + double _first_res_coeff; + double _second_res_coeff; + +public: + + TemperatureSensorPT100() + { + _r_1 = 4700; + _r_2 = 4700; + _r_3 = 100; + + _ptc_offset = 100.0; + _ptc_gradient = 0.3851; + _pga_gain = 2; + _vcc = 5; + _resolution_pga = 2.048; + _resolution_tdb = 32767; + + // define here and calculate when the class object is built to speed up calculations + // since the value does not change in the sensor type class object + _half_bridge_resistor_coeff = (double)_r_3 / (double)(_r_2 + _r_3); + _first_res_coeff = _r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3; + _second_res_coeff = _r_3*_r_1 - _r_2*_ptc_offset; + } + + TemperatureSensorPT100(const TemperatureSensorPT100& other) = default; // const copy constructor + TemperatureSensorPT100& operator=(const TemperatureSensorPT100& other) = default; // const copy assignment operator + TemperatureSensorPT100(TemperatureSensorPT100&& other) = default; // move constructor + TemperatureSensorPT100& operator=(TemperatureSensorPT100&& other) = default; // move assignment operator + + ~TemperatureSensorPT100() = default; // destructor + + virtual double convertTempCelsiusToRaw(const double temperature) override + { + double res = 0; + + double tmp = (( (_ptc_offset + _ptc_gradient * temperature) / ((double)_r_1 + (_ptc_offset + _ptc_gradient * temperature))) - _half_bridge_resistor_coeff) * (double)_vcc; + res = (_resolution_tdb + 1) * ((_pga_gain * tmp) / _resolution_pga); + + yDebug("Converted temperature limit to raw value:%f", res); + return res; + } + + virtual double convertRawToTempCelsius(const double temperature) override + { + double res = 0; + + double tmp = temperature * ((_resolution_pga) / (_pga_gain * _vcc * (_resolution_tdb + 1))); + double den = _ptc_gradient * (_r_2 - _r_2*tmp - _r_3*tmp); + + res = (tmp * (_first_res_coeff) / den) + ((_second_res_coeff) / den); + + return res; + } + + virtual motor_temperatureSensorTypeEnum_t getType() override + {return motor_temperature_sensor_pt100;} +}; + + +class TemperatureSensorPT1000 : public ITemperatureSensor +{ + +private: + // resistors of the voltage divider bridge + int _r_1; + int _r_2; + int _r_3; + + double _ptc_offset; // offset of the temperature sensor line + double _ptc_gradient; // slope/gradient of the temperature sensor line + double _pga_gain; // ADC gain set for the tdb (temperature detection board) + + int _vcc; // vcc that enters the voltage divider bridge + double _resolution_pga; // resolution of the internal pga of the tdb + double _resolution_tdb; // resolution used for the raw value for the output of the tdb + + // define here and calculate when the class object is built to speed up calculations + // since the value does not change in the sensor type class object + double _half_bridge_resistor_coeff; + double _first_res_coeff; + double _second_res_coeff; + +public: + + TemperatureSensorPT1000() + { + _r_1 = 4700; + _r_2 = 4700; + _r_3 = 1000; + + _ptc_offset = 1000; + _ptc_gradient = 3.851; + _pga_gain = 2; + _vcc = 5; + _resolution_pga = 2.048; + _resolution_tdb = 32767; + + _half_bridge_resistor_coeff = (double)_r_3 / (double)(_r_2 + _r_3); + + _first_res_coeff = _r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3; + _second_res_coeff = _r_3*_r_1 - _r_2*_ptc_offset; + } + + TemperatureSensorPT1000(const TemperatureSensorPT1000& other) = default; // const copy constructor + TemperatureSensorPT1000& operator=(const TemperatureSensorPT1000& other) = default; // const copy assignment operator + TemperatureSensorPT1000(TemperatureSensorPT1000&& other) = default; // move constructor + TemperatureSensorPT1000& operator=(TemperatureSensorPT1000&& other) = default; // move assignment operator + + ~TemperatureSensorPT1000() = default; // destructor + + virtual double convertTempCelsiusToRaw(const double temperature) override + { + double res = 0; + + double tmp = (( (_ptc_offset + _ptc_gradient * temperature) / ((double)_r_1 + (_ptc_offset + _ptc_gradient * temperature))) - _half_bridge_resistor_coeff) * (double)_vcc; + res = (_resolution_tdb + 1) * ((_pga_gain * tmp) / _resolution_pga); + + yDebug("Converted temperature limit to raw value:%f", res); + return res; + } + + virtual double convertRawToTempCelsius(const double temperature) override + { + double res = 0; + + double tmp = temperature * ((_resolution_pga) / (_pga_gain * _vcc * (_resolution_tdb + 1))); + double den = _ptc_gradient * (_r_2 - _r_2*tmp - _r_3*tmp); + + res = (tmp * (_first_res_coeff) / den) + ((_second_res_coeff) / den); + + return res; + } + + virtual motor_temperatureSensorTypeEnum_t getType() override + {return motor_temperature_sensor_pt1000;} +}; + + +class TemperatureSensorNONE : public ITemperatureSensor +{ + +public: + + TemperatureSensorNONE() + { + yInfo("Private varibales NOT DEFINED for class TemperatureSensorNONE"); + } + + ~TemperatureSensorNONE() = default; + + virtual double convertTempCelsiusToRaw(const double temperature) override + { + yInfo("convertTempCelsiusToRaw METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); + return 0; + } + + virtual double convertRawToTempCelsius(const double temperature) override + { + return 0; + } + + virtual motor_temperatureSensorTypeEnum_t getType() override + {return motor_temperature_sensor_none;} +}; + class Pid_Algorithm { @@ -201,8 +405,6 @@ class TrqPidInfo : public PidInfo int filterType; }; - - typedef struct { bool hasHallSensor; @@ -317,6 +519,13 @@ typedef struct float32_t P0; } kalmanFilterParams_t; + +typedef struct +{ + double hardwareTemperatureLimit; + double warningTemperatureLimit; +} temperatureLimits_t; // limits expressed as raw values after conversion is applied + //template class Parser @@ -420,11 +629,12 @@ class Parser ~Parser(); bool parsePids(yarp::os::Searchable &config, PidInfo *ppids/*, PidInfo *vpids*/, TrqPidInfo *tpids, PidInfo *cpids, PidInfo *spids, bool lowLevPidisMandatory); - bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName); + bool parseFocGroup(yarp::os::Searchable &config, focBasedSpecificInfo_t *foc_based_info, std::string groupName, std::vector>& temperatureSensorsVector); //bool parseCurrentPid(yarp::os::Searchable &config, PidInfo *cpids);//deprecated bool parseJointsetCfgGroup(yarp::os::Searchable &config, std::vector &jsets, std::vector &jointtoset); bool parseTimeoutsGroup(yarp::os::Searchable &config, std::vector &timeouts, int defaultVelocityTimeout); bool parseCurrentLimits(yarp::os::Searchable &config, std::vector &currLimits); + bool parseTemperatureLimits(yarp::os::Searchable &config, std::vector &temperatureLimits); bool parseJointsLimits(yarp::os::Searchable &config, std::vector &jointsLimits); bool parseRotorsLimits(yarp::os::Searchable &config, std::vector &rotorsLimits); bool parseCouplingInfo(yarp::os::Searchable &config, couplingInfo_t &couplingInfo);