From b81acbc38603d60838a731acc6cf14062b6299e9 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Wed, 4 Oct 2023 18:02:27 +0200 Subject: [PATCH 1/7] Start work on temperature limits --- .../icubmod/embObjMotionControl/embObjMotionControl.h | 1 + src/libraries/icubmod/embObjMotionControl/eomcParser.h | 2 ++ src/libraries/icubmod/embObjMotionControl/measuresConverter.h | 3 +++ 3 files changed, 6 insertions(+) diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index edac70c9d4..5eb2875834 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -513,6 +513,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.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index 0475fcc05b..08a663fa88 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -440,6 +440,8 @@ class Parser bool parseImpedanceGroup(yarp::os::Searchable &config,std::vector &impedance); bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found); bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector &kalmanFilterParams); + + bool parseTemperatureLimits(); }; }}}; //close namespaces diff --git a/src/libraries/icubmod/embObjMotionControl/measuresConverter.h b/src/libraries/icubmod/embObjMotionControl/measuresConverter.h index a8247b922e..2d3b4330f3 100644 --- a/src/libraries/icubmod/embObjMotionControl/measuresConverter.h +++ b/src/libraries/icubmod/embObjMotionControl/measuresConverter.h @@ -53,6 +53,9 @@ class measureConvFactors double* bemf2raw; double* ktau2raw; + double* DegCel2Raw; + double* Raw2DegCel; + measureConvFactors(int numofjoints); ~measureConvFactors(); From 66503b464d5956b3e3046c94739054606b1994e9 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Thu, 5 Oct 2023 14:20:40 +0200 Subject: [PATCH 2/7] Add parser for temperature limits from hardware/motorControl/LIMIT section and update struct containing temp sensor type Update methods for parsing temperature limits, conv factors and sensor types --- .../CanBusMotionControl.cpp | 4 + .../embObjMotionControl.cpp | 31 +++++-- .../embObjMotionControl/embObjMotionControl.h | 3 + .../embObjMotionControl/eomcParser.cpp | 87 ++++++++++++++++--- .../icubmod/embObjMotionControl/eomcParser.h | 23 ++++- .../embObjMotionControl/measuresConverter.h | 3 - 6 files changed, 124 insertions(+), 27 deletions(-) diff --git a/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp b/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp index c32e5e0e24..755116fca2 100644 --- a/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp +++ b/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp @@ -231,6 +231,7 @@ class BCastBufferElement // msg 3b unsigned char _interactionmodeStatus; + //short _hardwareTemperatureLimit; double _update_e2; // msg 4 @@ -324,6 +325,7 @@ class BCastBufferElement _boardStatus=0; _controlmodeStatus=0; _interactionmodeStatus=0; + //_hardwareTemperatureLimit = 0; _position_error = 0; _torque_error = 0; _speed_joint = 0; @@ -3388,11 +3390,13 @@ void CanBusMotionControl::handleBroadcasts() case ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS: r._bcastRecvBuffer[j]._interactionmodeStatus=*((char *)(data)) & 0x0F; + //r._bcastRecvBuffer[j]._hardwareTemperatureLimit= *((short *)(data+1)); r._bcastRecvBuffer[j]._update_e2 = before; j++; if (j < r.getJoints()) { r._bcastRecvBuffer[j]._interactionmodeStatus=(*((char *)(data)) >> 4) & 0x0F; + //r._bcastRecvBuffer[j]._hardwareTemperatureLimit=(*((short*)(data+3))); r._bcastRecvBuffer[j]._update_e2 = before; } break; diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index f3c0d76659..a335105137 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -100,6 +100,7 @@ bool embObjMotionControl::alloc(int nj) _encodersStamp = allocAndCheck(nj); _gearbox_M2J = allocAndCheck(nj); _gearbox_E2J = allocAndCheck(nj); + _temperatureFactor_degcel2raw = allocAndCheck(nj); _deadzone = allocAndCheck(nj); _foc_based_info=allocAndCheck(nj); _trj_pids= new eomc::PidInfo[nj]; @@ -127,6 +128,7 @@ bool embObjMotionControl::alloc(int nj) _rotorsLimits.resize(nj); _jointsLimits.resize(nj); _currentLimits.resize(nj); + _temperatureLimits.resize(nj); _jsets.resize(nj); _joint2set.resize(nj); _timeouts.resize(nj); @@ -147,6 +149,7 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_encodersStamp); checkAndDestroy(_gearbox_M2J); checkAndDestroy(_gearbox_E2J); + checkAndDestroy(_temperatureFactor_degcel2raw); checkAndDestroy(_deadzone); checkAndDestroy(_impedance_limits); checkAndDestroy(checking_motiondone); @@ -208,6 +211,7 @@ embObjMotionControl::embObjMotionControl() : _rotorsLimits(0), _jointsLimits(0), _currentLimits(0), + _temperatureLimits(0), _jsets(0), _joint2set(0), _timeouts(0), @@ -219,6 +223,7 @@ embObjMotionControl::embObjMotionControl() : { _gearbox_M2J = 0; _gearbox_E2J = 0; + _temperatureFactor_degcel2raw = 0; _deadzone = 0; opened = 0; _trj_pids = NULL; @@ -789,8 +794,6 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) ///////// GENERAL MECHANICAL INFO - - { if(!_mcparser->parseAxisInfo(config, _axisMap, _axesInfo)) return false; @@ -814,7 +817,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 +965,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,7 +979,7 @@ 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, _temperatureFactor_degcel2raw)) return false; } @@ -1395,7 +1401,7 @@ bool embObjMotionControl::init() motor_cfg.rotEncTolerance = _motorEncs[logico].tolerance; motor_cfg.hasHallSensor = _foc_based_info[logico].hasHallSensor; motor_cfg.hasRotorEncoder = _foc_based_info[logico].hasRotorEncoder; - motor_cfg.hasTempSensor = _foc_based_info[logico].hasTempSensor; + motor_cfg.hasTempSensor = _foc_based_info[logico].hasTemperatureSensor; motor_cfg.hasRotorEncoderIndex = _foc_based_info[logico].hasRotorEncoderIndex; motor_cfg.hasSpeedEncoder = _foc_based_info[logico].hasSpeedEncoder; motor_cfg.verbose = _foc_based_info[logico].verbose; @@ -1403,6 +1409,7 @@ 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.temperatureLimit = (eOmeas_temperature_t)(_temperatureLimits[logico].hardwareTemperatureLimit * _temperatureFactor_degcel2raw[logico]); //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 )); @@ -3270,7 +3277,15 @@ bool embObjMotionControl::getKinematicMJRaw(int j, double &rotres) return false; } -bool embObjMotionControl::getHasTempSensorsRaw(int j, int& ret) +bool embObjMotionControl::getTemperatureSensorTypeRaw(int j, int& ret) +{ + // refresh cached value when reading data from the EMS + ret = (int)_foc_based_info[j].temperatureSensorType; + + 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; @@ -3454,9 +3469,9 @@ bool embObjMotionControl::getRemoteVariableRaw(std::string key, yarp::os::Bottle Bottle& r = val.addList(); for (int i = 0; i < _njoints; i++) { int tmp = 0; getHasHallSensorRaw(i, tmp); r.addInt32(tmp); } return true; } - else if (key == "hasTempSensor") + else if (key == "TemperatureSensorType") { - 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; getTemperatureSensorTypeRaw(i, tmp); r.addInt32(tmp); } return true; } else if (key == "hasRotorEncoder") diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index 5eb2875834..e1125f36eb 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -204,6 +204,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, servConfigMC_t serviceConfig; /** contains the needed data for configure motion control service, like i.e. board ports where joint are connected */ double * _gearbox_M2J; /** the gearbox ratio motor to joint */ double * _gearbox_E2J; /** the gearbox ratio encoder to joint */ + double * _temperatureFactor_degcel2raw; /** the conversion factor between degree in celsius and raw value based on type of temperature sensor available for the motor*/ double * _deadzone; std::vector _kalman_params; /** Kalman filter parameters */ @@ -215,6 +216,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 */ @@ -490,6 +492,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, int& ret) ; bool getHasTempSensorsRaw(int j, int& ret) ; bool getHasHallSensorRaw(int j, int& ret) ; bool getHasRotorEncoderRaw(int j, int& ret) ; diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index b9d221bcc7..f84fdf1b93 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -32,8 +32,8 @@ using namespace yarp::dev; using namespace yarp::os; using namespace yarp::dev::eomc; - - +static constexpr double _temperatureFactor_degcel2raw_pt100 = 1; +static constexpr double _temperatureFactor_degcel2raw_pt1000 = 1; yarp::dev::eomc::Parser::Parser(int numofjoints, string boardname) { @@ -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, double temperatureFactor_degcel2raw[]) { - 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 &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,37 @@ 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)) { return false; } 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].hasTemperatureSensor = 1; + foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_type_pt100; + temperatureFactor_degcel2raw[i - 1] = _temperatureFactor_degcel2raw_pt100; + } + + else if (s == "PT1000") + { + foc_based_info[i - 1].hasTemperatureSensor = 1; + foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt1000; + temperatureFactor_degcel2raw[i - 1] = _temperatureFactor_degcel2raw_pt1000; + } + else + { + foc_based_info[i - 1].hasTemperatureSensor = 0; + yError("Not supported TemperatureSensorType %s!", s.c_str()); + foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_none; + temperatureFactor_degcel2raw[i - 1] = 0; + return false; + } + } } if (!extractGroup(focGroup, xtmp, "HasRotorEncoder", "HasRotorEncoder 0/1 ", _njoints)) { @@ -1694,6 +1717,47 @@ 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. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. \n Using default values for warningTemperatureLimits too."); + + temperatureLimits[i - 1].hardwareTemperatureLimit = 0; //change with constexpr default value + temperatureLimits[i - 1].warningTemperatureLimit = 0; //change with constexpr default value + } + else 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. Please update robot configuration files or contact https://github.com/robotology/icub-support if needed. \n Using default value for warningTemperatureLimits only."); + + temperatureLimits[i - 1].warningTemperatureLimit = 0; //change with constexpr default value + temperatureLimits[i - 1].hardwareTemperatureLimit = xtmp.get(i).asFloat64(); + } + else + { + for (i = 1; i < xtmp.size(); i++) + { + temperatureLimits[i - 1].hardwareTemperatureLimit = xtmp.get(i).asFloat64(); + temperatureLimits[i - 1].warningTemperatureLimit = xtmp.get(i).asFloat64(); + } + } + + return true; +} + bool Parser::parseJointsLimits(yarp::os::Searchable &config, std::vector &jointsLimits) { Bottle &limits=config.findGroup("LIMITS"); @@ -2154,7 +2218,6 @@ bool Parser::parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J } } - return true; } diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index 08a663fa88..d55718d767 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -201,12 +201,18 @@ class TrqPidInfo : public PidInfo int filterType; }; - +typedef enum +{ + motor_temperature_sensor_type_pt100 = 0, + motor_temperature_sensor_pt1000 = 1, + motor_temperature_sensor_none = 255 +} motor_temperatureSensorTypeEnum_t; typedef struct { bool hasHallSensor; - bool hasTempSensor; + bool hasTemperatureSensor; + motor_temperatureSensorTypeEnum_t temperatureSensorType; // uses a typedef enum bool hasRotorEncoder; bool hasRotorEncoderIndex; int rotorIndexOffset; @@ -317,6 +323,13 @@ typedef struct float32_t P0; } kalmanFilterParams_t; + +typedef struct +{ + double hardwareTemperatureLimit; + double warningTemperatureLimit; +} temperatureLimits_t; + //template class Parser @@ -420,11 +433,14 @@ 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, double temperatureFactor_degcel2raw[]); //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); @@ -441,7 +457,6 @@ class Parser bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found); bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector &kalmanFilterParams); - bool parseTemperatureLimits(); }; }}}; //close namespaces diff --git a/src/libraries/icubmod/embObjMotionControl/measuresConverter.h b/src/libraries/icubmod/embObjMotionControl/measuresConverter.h index 2d3b4330f3..a8247b922e 100644 --- a/src/libraries/icubmod/embObjMotionControl/measuresConverter.h +++ b/src/libraries/icubmod/embObjMotionControl/measuresConverter.h @@ -53,9 +53,6 @@ class measureConvFactors double* bemf2raw; double* ktau2raw; - double* DegCel2Raw; - double* Raw2DegCel; - measureConvFactors(int numofjoints); ~measureConvFactors(); From 43f385d1a94d1579ee41794082a42d88bbb54297 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Wed, 11 Oct 2023 11:52:31 +0200 Subject: [PATCH 3/7] Define check on warning and hardware limits Keep hasTempSensor in conf parameters --- .../embObjMotionControl.cpp | 40 +++++++++++++++++-- .../embObjMotionControl/embObjMotionControl.h | 1 + .../embObjMotionControl/eomcParser.cpp | 16 +++++--- .../icubmod/embObjMotionControl/eomcParser.h | 2 +- 4 files changed, 49 insertions(+), 10 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index a335105137..c16774fa8c 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -74,7 +74,7 @@ static bool nv_not_found(void) - +int _printCounter = 0; @@ -102,6 +102,7 @@ bool embObjMotionControl::alloc(int nj) _gearbox_E2J = allocAndCheck(nj); _temperatureFactor_degcel2raw = 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]; @@ -151,6 +152,7 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_gearbox_E2J); checkAndDestroy(_temperatureFactor_degcel2raw); checkAndDestroy(_deadzone); + checkAndDestroy(_temperatureValues); checkAndDestroy(_impedance_limits); checkAndDestroy(checking_motiondone); checkAndDestroy(_ref_command_positions); @@ -225,6 +227,7 @@ embObjMotionControl::embObjMotionControl() : _gearbox_E2J = 0; _temperatureFactor_degcel2raw = 0; _deadzone = 0; + _temperatureValues = 0; opened = 0; _trj_pids = NULL; //_dir_pids = NULL; @@ -338,7 +341,7 @@ bool embObjMotionControl::initializeInterfaces(measureConvFactors &f) bool embObjMotionControl::open(yarp::os::Searchable &config) { // - first thing to do is verify if the eth manager is available. then i parse info about the eth board. - + _printCounter = 0; ethManager = eth::TheEthManager::instance(); if(NULL == ethManager) { @@ -1401,7 +1404,7 @@ bool embObjMotionControl::init() motor_cfg.rotEncTolerance = _motorEncs[logico].tolerance; motor_cfg.hasHallSensor = _foc_based_info[logico].hasHallSensor; motor_cfg.hasRotorEncoder = _foc_based_info[logico].hasRotorEncoder; - motor_cfg.hasTempSensor = _foc_based_info[logico].hasTemperatureSensor; + motor_cfg.hasTempSensor = _foc_based_info[logico].hasTempSensor; motor_cfg.hasRotorEncoderIndex = _foc_based_info[logico].hasRotorEncoderIndex; motor_cfg.hasSpeedEncoder = _foc_based_info[logico].hasSpeedEncoder; motor_cfg.verbose = _foc_based_info[logico].verbose; @@ -1572,6 +1575,23 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda } } + + if ((_printCounter % 1000) == 0) + { + _printCounter = 0; + if(!getTemperaturesRaw(_temperatureValues)) + { + yError("ERROR IN GETTING SOME TEMPERATURES"); + return false; + } + else + { + yDebug("GOT SOME TEMPERATURESSSSSS"); + } + } + ++_printCounter; + + return true; } @@ -4699,7 +4719,9 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) bool ret = res->getLocalValue(protid, &status); if(ret) { - *val = (double) status.mot_temperature; + *val = (double) status.mot_temperature ; + *val /= _temperatureFactor_degcel2raw[m]; + yDebug("Temperature updating to value: %lf", *val); } else { @@ -4707,6 +4729,16 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) *val = 0; } + if (val > _temperatureLimits[m].hardwareTemperatureLimit) + { + yError("Hardware temperature limit for motor:%i overcame! Stopping processess as safety procedure to not damage motor", m); + return false; + } + else if (val > _temperatureLimits[m].warningTemperatureLimit) + { + yWarning("Warning 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", m); + } + return ret; } diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index e1125f36eb..de56ba15dd 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -206,6 +206,7 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, double * _gearbox_E2J; /** the gearbox ratio encoder to joint */ double * _temperatureFactor_degcel2raw; /** the conversion factor between degree in celsius and raw value based on type of temperature sensor available for the motor*/ double * _deadzone; + double * _temperatureValues; std::vector _kalman_params; /** Kalman filter parameters */ eomc::focBasedSpecificInfo_t * _foc_based_info; diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index f84fdf1b93..1c655558f1 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -1391,6 +1391,15 @@ 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)) + { + return false; + } + else + { + for (i = 1; i < xtmp.size(); i++) + foc_based_info[i - 1].hasTempSensor = xtmp.get(i).asInt32() != 0; + } if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints)) { return false; @@ -1402,20 +1411,17 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI std::string s = xtmp.get(i).asString(); if(s == "PT100") { - foc_based_info[i - 1].hasTemperatureSensor = 1; foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_type_pt100; temperatureFactor_degcel2raw[i - 1] = _temperatureFactor_degcel2raw_pt100; } else if (s == "PT1000") { - foc_based_info[i - 1].hasTemperatureSensor = 1; foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt1000; temperatureFactor_degcel2raw[i - 1] = _temperatureFactor_degcel2raw_pt1000; } else { - foc_based_info[i - 1].hasTemperatureSensor = 0; yError("Not supported TemperatureSensorType %s!", s.c_str()); foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_none; temperatureFactor_degcel2raw[i - 1] = 0; @@ -1733,7 +1739,7 @@ bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector Date: Fri, 13 Oct 2023 17:32:59 +0200 Subject: [PATCH 4/7] Add classes to manage handling of temperature sensors configuration and conversions Move temperature sensor converter class to eomcParser header First working version of temperature reading feature --- .../CanBusMotionControl.cpp | 4 - .../embObjMotionControl.cpp | 172 ++++++++----- .../embObjMotionControl/embObjMotionControl.h | 6 +- .../embObjMotionControl/eomcParser.cpp | 108 ++++++--- .../icubmod/embObjMotionControl/eomcParser.h | 225 +++++++++++++++++- 5 files changed, 399 insertions(+), 116 deletions(-) diff --git a/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp b/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp index 755116fca2..c32e5e0e24 100644 --- a/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp +++ b/src/libraries/icubmod/canBusMotionControl/CanBusMotionControl.cpp @@ -231,7 +231,6 @@ class BCastBufferElement // msg 3b unsigned char _interactionmodeStatus; - //short _hardwareTemperatureLimit; double _update_e2; // msg 4 @@ -325,7 +324,6 @@ class BCastBufferElement _boardStatus=0; _controlmodeStatus=0; _interactionmodeStatus=0; - //_hardwareTemperatureLimit = 0; _position_error = 0; _torque_error = 0; _speed_joint = 0; @@ -3390,13 +3388,11 @@ void CanBusMotionControl::handleBroadcasts() case ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS: r._bcastRecvBuffer[j]._interactionmodeStatus=*((char *)(data)) & 0x0F; - //r._bcastRecvBuffer[j]._hardwareTemperatureLimit= *((short *)(data+1)); r._bcastRecvBuffer[j]._update_e2 = before; j++; if (j < r.getJoints()) { r._bcastRecvBuffer[j]._interactionmodeStatus=(*((char *)(data)) >> 4) & 0x0F; - //r._bcastRecvBuffer[j]._hardwareTemperatureLimit=(*((short*)(data+3))); r._bcastRecvBuffer[j]._update_e2 = before; } break; diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index c16774fa8c..136da19b51 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 @@ -74,7 +75,7 @@ static bool nv_not_found(void) -int _printCounter = 0; + @@ -100,10 +101,9 @@ bool embObjMotionControl::alloc(int nj) _encodersStamp = allocAndCheck(nj); _gearbox_M2J = allocAndCheck(nj); _gearbox_E2J = allocAndCheck(nj); - _temperatureFactor_degcel2raw = allocAndCheck(nj); _deadzone = allocAndCheck(nj); _temperatureValues = 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]; @@ -125,7 +125,6 @@ bool embObjMotionControl::alloc(int nj) _calibrated = allocAndCheck(nj); _cacheImpedance = allocAndCheck(nj); - _rotorsLimits.resize(nj); _jointsLimits.resize(nj); _currentLimits.resize(nj); @@ -138,8 +137,10 @@ bool embObjMotionControl::alloc(int nj) _jointEncs.resize(nj); _motorEncs.resize(nj); _kalman_params.resize(nj); - - //debug purpose + _temperatureSensorsVector.resize(nj); + _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 return true; } @@ -150,7 +151,6 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_encodersStamp); checkAndDestroy(_gearbox_M2J); checkAndDestroy(_gearbox_E2J); - checkAndDestroy(_temperatureFactor_degcel2raw); checkAndDestroy(_deadzone); checkAndDestroy(_temperatureValues); checkAndDestroy(_impedance_limits); @@ -166,7 +166,6 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_calibrated); checkAndDestroy(_foc_based_info); - if(_trj_pids) delete [] _trj_pids; @@ -221,13 +220,14 @@ embObjMotionControl::embObjMotionControl() : _axesInfo(0), _jointEncs(0), _motorEncs(0), - _kalman_params(0) + _kalman_params(0), + _temperatureSensorsVector(0), + _temperatureSensorErrorWatchdog(0) { _gearbox_M2J = 0; _gearbox_E2J = 0; - _temperatureFactor_degcel2raw = 0; _deadzone = 0; - _temperatureValues = 0; + _temperatureValues = NULL; opened = 0; _trj_pids = NULL; //_dir_pids = NULL; @@ -341,7 +341,7 @@ bool embObjMotionControl::initializeInterfaces(measureConvFactors &f) bool embObjMotionControl::open(yarp::os::Searchable &config) { // - first thing to do is verify if the eth manager is available. then i parse info about the eth board. - _printCounter = 0; + ethManager = eth::TheEthManager::instance(); if(NULL == ethManager) { @@ -797,6 +797,8 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) ///////// GENERAL MECHANICAL INFO + + { if(!_mcparser->parseAxisInfo(config, _axisMap, _axesInfo)) return false; @@ -982,8 +984,27 @@ 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, _temperatureFactor_degcel2raw)) + if(!_mcparser->parseFocGroup(config, _foc_based_info, groupName, _temperatureSensorsVector)) return false; + + 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))) + { + 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) + { + yWarning() << "embObjMC " << getBoardInfo() << "joint " << j << " has motor not provided with any available type of temperature sensor. If needed update the configurations file accordingly"; + } + else + { + _temperatureLimits[j].hardwareTemperatureLimit = (_temperatureSensorsVector[j])->convertTempCelsiusToRaw(_temperatureLimits[j].hardwareTemperatureLimit); + yDebug("Converted given Temp LIMIT value to:%f", _temperatureLimits[j].hardwareTemperatureLimit); + } + } } @@ -1412,8 +1433,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.temperatureLimit = (eOmeas_temperature_t)(_temperatureLimits[logico].hardwareTemperatureLimit * _temperatureFactor_degcel2raw[logico]); //passing raw value not in degree - motor_cfg.limitsofrotor.max = (eOmeas_position_t) S_32(_measureConverter->posA2E(_rotorsLimits[logico].posMax, fisico )); + motor_cfg.temperatureLimit = (eOmeas_temperature_t) S_16(_temperatureLimits[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; @@ -1540,6 +1561,26 @@ 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"; + } + } + } + } + } @@ -1574,24 +1615,7 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda mcdiagnostics.ports[joint]->write(); } } - - if ((_printCounter % 1000) == 0) - { - _printCounter = 0; - if(!getTemperaturesRaw(_temperatureValues)) - { - yError("ERROR IN GETTING SOME TEMPERATURES"); - return false; - } - else - { - yDebug("GOT SOME TEMPERATURESSSSSS"); - } - } - ++_printCounter; - - return true; } @@ -3297,11 +3321,23 @@ bool embObjMotionControl::getKinematicMJRaw(int j, double &rotres) return false; } -bool embObjMotionControl::getTemperatureSensorTypeRaw(int j, int& ret) +bool embObjMotionControl::getTemperatureSensorTypeRaw(int j, std::string& ret) { // refresh cached value when reading data from the EMS - ret = (int)_foc_based_info[j].temperatureSensorType; - + ret = "NONE"; + if (_foc_based_info[j].temperatureSensorType = motor_temperature_sensor_pt100) + { + ret = "PT100"; + } + else if (_foc_based_info[j].temperatureSensorType = motor_temperature_sensor_pt1000) + { + ret = "PT1000"; + } + else + { + ret = "NONE"; + } + return true; } @@ -3489,9 +3525,14 @@ bool embObjMotionControl::getRemoteVariableRaw(std::string key, yarp::os::Bottle Bottle& r = val.addList(); for (int i = 0; i < _njoints; i++) { int tmp = 0; getHasHallSensorRaw(i, tmp); r.addInt32(tmp); } return true; } + else if (key == "hasTempSensor") + { + 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++) { int tmp = 0; getTemperatureSensorTypeRaw(i, tmp); r.addInt32(tmp); } + 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") @@ -3787,6 +3828,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"); @@ -4717,26 +4759,32 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic); bool ret = res->getLocalValue(protid, &status); + *val = NAN; if(ret) { - *val = (double) status.mot_temperature ; - *val /= _temperatureFactor_degcel2raw[m]; - yDebug("Temperature updating to value: %lf", *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)) + { + *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature); + } + + } + else + { + --_temperatureSensorErrorWatchdog.at(m); + 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"; + return false; + } + + } } else { - yError() << "embObjMotionControl::getTemperatureRaw failed for" << getBoardInfo() << " motor " << m ; - *val = 0; - } - - if (val > _temperatureLimits[m].hardwareTemperatureLimit) - { - yError("Hardware temperature limit for motor:%i overcame! Stopping processess as safety procedure to not damage motor", m); - return false; - } - else if (val > _temperatureLimits[m].warningTemperatureLimit) - { - yWarning("Warning 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", m); + yError() << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue() for " << getBoardInfo() << " Setting mot_temperature" << m << "to" << *val; } return ret; @@ -4754,17 +4802,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 de56ba15dd..a3009b6a93 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -204,11 +204,12 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, servConfigMC_t serviceConfig; /** contains the needed data for configure motion control service, like i.e. board ports where joint are connected */ double * _gearbox_M2J; /** the gearbox ratio motor to joint */ double * _gearbox_E2J; /** the gearbox ratio encoder to joint */ - double * _temperatureFactor_degcel2raw; /** the conversion factor between degree in celsius and raw value based on type of temperature sensor available for the motor*/ double * _deadzone; double * _temperatureValues; std::vector _kalman_params; /** Kalman filter parameters */ + std::vector> _temperatureSensorsVector; + eomc::focBasedSpecificInfo_t * _foc_based_info; std::vector _jointEncs; @@ -268,6 +269,7 @@ 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*/ #ifdef NETWORK_PERFORMANCE_BENCHMARK @@ -493,7 +495,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, int& ret) ; + 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) ; diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 1c655558f1..3723b052bb 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -32,8 +32,8 @@ using namespace yarp::dev; using namespace yarp::os; using namespace yarp::dev::eomc; -static constexpr double _temperatureFactor_degcel2raw_pt100 = 1; -static constexpr double _temperatureFactor_degcel2raw_pt1000 = 1; + + yarp::dev::eomc::Parser::Parser(int numofjoints, string boardname) { @@ -1370,13 +1370,13 @@ 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, double temperatureFactor_degcel2raw[]) +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() ) { - yError() << "embObjMC BOARD " << _boardname << " detected that Group " << groupName << " is not found in configuration file"; - return false; + yError() << "embObjMC BOARD " << _boardname << " detected that Group " << groupName << " is not found in configuration file"; + return false; } Bottle xtmp; @@ -1391,18 +1391,38 @@ 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)) - { - return false; - } - else - { - for (i = 1; i < xtmp.size(); i++) - foc_based_info[i - 1].hasTempSensor = xtmp.get(i).asInt32() != 0; - } - if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints)) + + if (!extractGroup(focGroup, xtmp, "TemperatureSensorType", "TemperatureSensorType PT100/PT1000/NONE ", _njoints, false)) { - return 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; + } + + 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 + { + 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) + { + 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(); + } + } + } } else { @@ -1411,21 +1431,24 @@ bool Parser::parseFocGroup(yarp::os::Searchable &config, eomc::focBasedSpecificI std::string s = xtmp.get(i).asString(); if(s == "PT100") { - foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_type_pt100; - temperatureFactor_degcel2raw[i - 1] = _temperatureFactor_degcel2raw_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(); + } else if (s == "PT1000") { + + foc_based_info[i - 1].hasTempSensor = 1; foc_based_info[i - 1].temperatureSensorType = motor_temperature_sensor_pt1000; - temperatureFactor_degcel2raw[i - 1] = _temperatureFactor_degcel2raw_pt1000; + temperatureSensorsVector.at(i-1) = std::make_unique(); } else { - yError("Not supported TemperatureSensorType %s!", s.c_str()); + 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; - temperatureFactor_degcel2raw[i - 1] = 0; - return false; + temperatureSensorsVector.at(i-1) = std::make_unique(); } } } @@ -1739,28 +1762,38 @@ bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector (0.85 * temperatureLimits[i].hardwareTemperatureLimit)) { - temperatureLimits[i - 1].hardwareTemperatureLimit = xtmp.get(i).asFloat64(); - temperatureLimits[i - 1].warningTemperatureLimit = xtmp.get(i).asFloat64(); + yError() << "In " << _boardname << "joint " << i << ": inconsistent temperature limits. warningTemperatureLimit must be smaller than 85% of hardwareTemperatureLimit" ; + return false; } } - return true; } @@ -2224,6 +2257,7 @@ bool Parser::parseGearboxValues(yarp::os::Searchable &config, double gearbox_M2J } } + return true; } diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index bc9b5463b6..d107972de9 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,216 @@ 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; +}; + +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; + + bool isConfigured = false; + +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; + + _half_bridge_resistor_coeff = (double)_r_3 / (double)(_r_2 + _r_3); + isConfigured = true; + } + + 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; + if (!isConfigured) + { + yError("Cannot proceed, class paramters not confgured"); + return res; + } + + 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; + if (!isConfigured) + { + yError("Cannot proceed, class paramters not confgured"); + return res; + } + + 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 * (_r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3) / den) + ((_r_3*_r_1 - _r_2*_ptc_offset) / den); + + + //yDebug("Converted temperature to Celsius degree value:%f", res); + return res; + } + +}; + + +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 + + double _half_bridge_resistor_coeff; + + bool isConfigured = false; + +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); + + isConfigured = true; + } + + 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; + if (!isConfigured) + { + yError("Cannot proceed, class paramters not confgured"); + return res; + } + + 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; + if (!isConfigured) + { + yError("Cannot proceed, class paramters not confgured"); + return res; + } + + 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 * (_r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3) / den) + ((_r_3*_r_1 - _r_2*_ptc_offset) / den); + + //yDebug("Converted temperature to Celsius degree value:%f", res); + + return res; + } +}; + + +class TemperatureSensorNONE : public ITemperatureSensor +{ + +public: + + TemperatureSensorNONE() + { + yError("Private varibales NOT DEFINED for class TemperatureSensorNONE"); + } + + ~TemperatureSensorNONE() = default; + + virtual double convertTempCelsiusToRaw(const double temperature) override + { + yError("convertTempCelsiusToRaw METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); + return 0; + } + + virtual double convertRawToTempCelsius(const double temperature) override + { + yError("convertRawToTempCelsius METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); + return 0; + } +}; + class Pid_Algorithm { @@ -201,13 +412,6 @@ class TrqPidInfo : public PidInfo int filterType; }; -typedef enum -{ - motor_temperature_sensor_type_pt100 = 0, - motor_temperature_sensor_pt1000 = 1, - motor_temperature_sensor_none = 255 -} motor_temperatureSensorTypeEnum_t; - typedef struct { bool hasHallSensor; @@ -328,7 +532,7 @@ typedef struct { double hardwareTemperatureLimit; double warningTemperatureLimit; -} temperatureLimits_t; +} temperatureLimits_t; // limits expressed as raw values after conversion is applied //template @@ -433,14 +637,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, double temperatureFactor_degcel2raw[]); + 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); @@ -456,7 +658,6 @@ class Parser bool parseImpedanceGroup(yarp::os::Searchable &config,std::vector &impedance); bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found); bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector &kalmanFilterParams); - }; }}}; //close namespaces From 025ef6819f74fc77d06f5ba7e4105aa0865c0967 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Fri, 10 Nov 2023 18:08:56 +0100 Subject: [PATCH 5/7] Add method for getting temperature update from callback --- .../icubmod/embObjLib/FeatureInterface.cpp | 1 - .../icubmod/embObjLib/hostTransceiver.cpp | 7 + .../EoProtocolMC_fun_userdef.c | 3 +- .../embObjMotionControl.cpp | 178 +++++++++++------- .../embObjMotionControl/embObjMotionControl.h | 40 +++- .../embObjMotionControl/eomcParser.cpp | 44 ++--- .../icubmod/embObjMotionControl/eomcParser.h | 17 +- 7 files changed, 183 insertions(+), 107 deletions(-) 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 136da19b51..2cf3523bca 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -73,10 +73,7 @@ static bool nv_not_found(void) return false; } - - - - +static constexpr double const temperatureErrorValue_s = -5000; @@ -102,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]; @@ -138,10 +134,9 @@ bool embObjMotionControl::alloc(int nj) _motorEncs.resize(nj); _kalman_params.resize(nj); _temperatureSensorsVector.resize(nj); - _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 - + _temperatureExceededLimitWatchdog.resize(nj); + _temperatureSensorErrorWatchdog.resize(nj); + return true; } @@ -152,7 +147,6 @@ bool embObjMotionControl::dealloc() checkAndDestroy(_gearbox_M2J); checkAndDestroy(_gearbox_E2J); checkAndDestroy(_deadzone); - checkAndDestroy(_temperatureValues); checkAndDestroy(_impedance_limits); checkAndDestroy(checking_motiondone); checkAndDestroy(_ref_command_positions); @@ -222,12 +216,12 @@ embObjMotionControl::embObjMotionControl() : _motorEncs(0), _kalman_params(0), _temperatureSensorsVector(0), + _temperatureExceededLimitWatchdog(0), _temperatureSensorErrorWatchdog(0) { _gearbox_M2J = 0; _gearbox_E2J = 0; _deadzone = 0; - _temperatureValues = NULL; opened = 0; _trj_pids = NULL; //_dir_pids = NULL; @@ -989,23 +983,26 @@ 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"; - } - else - { - _temperatureLimits[j].hardwareTemperatureLimit = (_temperatureSensorsVector[j])->convertTempCelsiusToRaw(_temperatureLimits[j].hardwareTemperatureLimit); - yDebug("Converted given Temp LIMIT value to:%f", _temperatureLimits[j].hardwareTemperatureLimit); + 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] @@ -1433,7 +1430,7 @@ 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.temperatureLimit = (eOmeas_temperature_t) S_16(_temperatureLimits[logico].hardwareTemperatureLimit); //passing raw value not in degree + 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 )); @@ -1546,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; @@ -1561,34 +1560,11 @@ 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"; - } - } - } - } - } 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)); @@ -1596,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 @@ -1615,7 +1591,74 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda mcdiagnostics.ports[joint]->write(); } } - + + 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_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); + + 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; } @@ -3325,11 +3368,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"; } @@ -4758,35 +4801,28 @@ 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); - bool ret = res->getLocalValue(protid, &status); + // if (_temperatureSensorsVector.at(m) == nullptr || (_temperatureSensorsVector.at(motor)->getType() == motor_temperature_sensor_none)) *val = NAN; - if(ret) + if (_temperatureSensorsVector.at(m)->getType() == motor_temperature_sensor_none) + return true; + + + bool ret = res->getLocalValue(protid, &status); + if(!ret) { - 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)) - { - *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature); - } - - } - else - { - --_temperatureSensorErrorWatchdog.at(m); - 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"; - return false; - } - - } + 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 to complete getLocalValue() for " << getBoardInfo() << " Setting mot_temperature" << m << "to" << *val; + 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; } + *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature); + + return ret; } diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index a3009b6a93..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 { @@ -205,7 +239,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; @@ -269,7 +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 _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 diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 3723b052bb..9cc7634421 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -1394,22 +1394,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; - } - - 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) @@ -1417,13 +1404,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++) @@ -1432,7 +1424,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(); } @@ -1440,14 +1431,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(); } } @@ -1765,8 +1755,8 @@ bool Parser::parseTemperatureLimits(yarp::os::Searchable &config, std::vector Date: Thu, 23 Nov 2023 13:54:18 +0100 Subject: [PATCH 6/7] Update icub_firmware_shared_VERSION requirement to higher than 1.37.2 --- conf/iCubFindDependencies.cmake | 4 ++-- .../embObjMotionControl.cpp | 18 +++--------------- 2 files changed, 5 insertions(+), 17 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/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 2cf3523bca..5bee762ce6 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -1601,20 +1601,11 @@ 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 = {}; - id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, eoprot_tag_mc_motor_status_basic); + eOmc_motor_status_t *mc_motor_status = reinterpret_cast(rxdata); - if (!res->getLocalValue(id32, &mc_motor_status_basic)) + if((double)mc_motor_status->basic.mot_temperature != temperatureErrorValue_s) //I get a valid value { - 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) { @@ -1657,8 +1648,6 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda } } - - return true; } @@ -4816,7 +4805,6 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) 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" << *val << "from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable"; return false; } From 81e6cad71b963b0efeffcede731f093a312162df Mon Sep 17 00:00:00 2001 From: Jacopo Date: Fri, 24 Nov 2023 15:49:35 +0100 Subject: [PATCH 7/7] Update ITemperatureSensor interface with requested changes --- .../icubmod/embObjMotionControl/eomcParser.h | 51 +++++++------------ 1 file changed, 17 insertions(+), 34 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index afa14fb280..e0c34a7e59 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -94,8 +94,8 @@ class TemperatureSensorPT100 : public ITemperatureSensor double _resolution_tdb; // resolution used for the raw value for the output of the tdb double _half_bridge_resistor_coeff; - - bool isConfigured = false; + double _first_res_coeff; + double _second_res_coeff; public: @@ -112,8 +112,11 @@ class TemperatureSensorPT100 : public ITemperatureSensor _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); - isConfigured = true; + _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 @@ -126,11 +129,6 @@ class TemperatureSensorPT100 : public ITemperatureSensor virtual double convertTempCelsiusToRaw(const double temperature) override { double res = 0; - if (!isConfigured) - { - yError("Cannot proceed, class paramters not confgured"); - return res; - } 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); @@ -142,18 +140,12 @@ class TemperatureSensorPT100 : public ITemperatureSensor virtual double convertRawToTempCelsius(const double temperature) override { double res = 0; - if (!isConfigured) - { - yError("Cannot proceed, class paramters not confgured"); - return res; - } 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 * (_r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3) / den) + ((_r_3*_r_1 - _r_2*_ptc_offset) / den); - + + res = (tmp * (_first_res_coeff) / den) + ((_second_res_coeff) / den); - //yDebug("Converted temperature to Celsius degree value:%f", res); return res; } @@ -171,7 +163,7 @@ class TemperatureSensorPT1000 : public ITemperatureSensor int _r_2; int _r_3; - double _ptc_offset; // offset of the temperature sensor line + 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) @@ -179,10 +171,12 @@ class TemperatureSensorPT1000 : public ITemperatureSensor 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; - bool isConfigured = false; - public: TemperatureSensorPT1000() @@ -200,7 +194,8 @@ class TemperatureSensorPT1000 : public ITemperatureSensor _half_bridge_resistor_coeff = (double)_r_3 / (double)(_r_2 + _r_3); - isConfigured = true; + _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 @@ -213,11 +208,6 @@ class TemperatureSensorPT1000 : public ITemperatureSensor virtual double convertTempCelsiusToRaw(const double temperature) override { double res = 0; - if (!isConfigured) - { - yError("Cannot proceed, class paramters not confgured"); - return res; - } 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); @@ -229,17 +219,11 @@ class TemperatureSensorPT1000 : public ITemperatureSensor virtual double convertRawToTempCelsius(const double temperature) override { double res = 0; - if (!isConfigured) - { - yError("Cannot proceed, class paramters not confgured"); - return res; - } 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 * (_r_1*_r_2 + _r_1*_r_3 + _ptc_offset*_r_2 + _ptc_offset*_r_3) / den) + ((_r_3*_r_1 - _r_2*_ptc_offset) / den); - - //yDebug("Converted temperature to Celsius degree value:%f", res); + + res = (tmp * (_first_res_coeff) / den) + ((_second_res_coeff) / den); return res; } @@ -269,7 +253,6 @@ class TemperatureSensorNONE : public ITemperatureSensor virtual double convertRawToTempCelsius(const double temperature) override { - //yError("convertRawToTempCelsius METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE"); return 0; }