From e71d170b200174e49f615ba07f670e6154fa249d Mon Sep 17 00:00:00 2001 From: Jacopo Date: Thu, 9 May 2024 16:00:42 +0200 Subject: [PATCH] Update filtering of spikes in temp reading Spikes in temperature readings are now filtered when checking for overcoming warning/hw temperature limits spikes are not considered Median filter is added for initialization of temperature so that we are not risking to save invalid or null temperatures Overcoming of warning and hw limits is still correctly checked if not a spike Counter on the watchdog thresholds is now divided by the eth transmission rate We set a limit for the watchdog when temperature is higher than the warn/hw limit for 60 seconds continuously --- .../icubmod/embObjLib/abstractEthResource.h | 1 + .../icubmod/embObjLib/diagnosticInfo.cpp | 3 + .../icubmod/embObjLib/ethResource.cpp | 1 + .../embObjMotionControl.cpp | 118 +++++++++++------- .../embObjMotionControl/embObjMotionControl.h | 87 +++++++++---- 5 files changed, 141 insertions(+), 69 deletions(-) diff --git a/src/libraries/icubmod/embObjLib/abstractEthResource.h b/src/libraries/icubmod/embObjLib/abstractEthResource.h index 241afc581b..8db98704af 100755 --- a/src/libraries/icubmod/embObjLib/abstractEthResource.h +++ b/src/libraries/icubmod/embObjLib/abstractEthResource.h @@ -53,6 +53,7 @@ namespace eth { string boardtypeString; string boardnameString; string firmwareString; + uint8_t txROPratedivider; }; //AbstractEthResource(); diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfo.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfo.cpp index 3e14736172..e369b09cb2 100644 --- a/src/libraries/icubmod/embObjLib/diagnosticInfo.cpp +++ b/src/libraries/icubmod/embObjLib/diagnosticInfo.cpp @@ -34,6 +34,9 @@ void TimeOfInfo::toString(std::string &str_toi) void EmbeddedInfo::printMessage() { + if (finalMessage.size() == 0) + return; + std::string str_toi; timeOfInfo.toString(str_toi); diff --git a/src/libraries/icubmod/embObjLib/ethResource.cpp b/src/libraries/icubmod/embObjLib/ethResource.cpp index dbadeebd70..ce768a8cc4 100755 --- a/src/libraries/icubmod/embObjLib/ethResource.cpp +++ b/src/libraries/icubmod/embObjLib/ethResource.cpp @@ -129,6 +129,7 @@ bool EthResource::open2(eOipv4addr_t remIP, yarp::os::Searchable &cfgtotal) properties.ipv4addressingString = brddata.properties.ipv4addressingstring; properties.boardtypeString = brddata.properties.typestring; properties.boardnameString = brddata.settings.name; + properties.txROPratedivider = txconfig.txratedivider; eth::EthMonitorPresence::Config mpConfig; diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 53272de87b..0e4544b826 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -8,12 +8,14 @@ */ //#include -#include -#include +// system std include #include #include #include +// yarp include +#include +#include #include "embObjMotionControl.h" #include #include @@ -21,6 +23,10 @@ #include +#include +#include + +// local include #include "EoCommon.h" #include "EOarray.h" #include "EoProtocol.h" @@ -29,10 +35,6 @@ #include "EoProtocolAS.h" #include "motionControlDefaultValues.h" -#include -#include - - #include "eomcUtils.h" using namespace yarp::dev; @@ -50,9 +52,6 @@ using namespace yarp::dev::eomc; #define PARSER_MOTION_CONTROL_VERSION 6 - - - static inline bool NOT_YET_IMPLEMENTED(const char *txt) { yError() << txt << " is not yet implemented for embObjMotionControl"; @@ -73,10 +72,7 @@ static bool nv_not_found(void) return false; } -static constexpr double const temperatureErrorValue_s = -5000; - - - +//static constexpr double const temperatureErrorValue_s = -5000; std::string embObjMotionControl::getBoardInfo(void) { @@ -136,6 +132,15 @@ bool embObjMotionControl::alloc(int nj) _temperatureSensorsVector.resize(nj); _temperatureExceededLimitWatchdog.resize(nj); _temperatureSensorErrorWatchdog.resize(nj); + _temperatureSpikesFilter.resize(nj); + + // update threshold for watchdog parametrized on the ROP transmission rate (by default is 2ms) + uint8_t txrate = res->getProperties().txROPratedivider; + for(int i = 0; i < nj; ++i) + { + _temperatureExceededLimitWatchdog.at(i).setThreshold(txrate); + _temperatureSensorErrorWatchdog.at(i).setThreshold(txrate); + } return true; } @@ -217,7 +222,8 @@ embObjMotionControl::embObjMotionControl() : _kalman_params(0), _temperatureSensorsVector(0), _temperatureExceededLimitWatchdog(0), - _temperatureSensorErrorWatchdog(0) + _temperatureSensorErrorWatchdog(0), + _temperatureSpikesFilter(0) { _gearbox_M2J = 0; _gearbox_E2J = 0; @@ -1621,49 +1627,71 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda 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 + if((double)mc_motor_status->basic.mot_temperature < 0 ) //I get a invalid value { - double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((double)mc_motor_status->basic.mot_temperature); - - if (tmp > _temperatureLimits[motor].warningTemperatureLimit) + if(! _temperatureSensorErrorWatchdog.at(motor).isStarted()) { - 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(); - } + 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 { - _temperatureExceededLimitWatchdog.at(motor).clear(); + _temperatureSensorErrorWatchdog.at(motor).increment(); + if( _temperatureSensorErrorWatchdog.at(motor).isExpired()) + { + yError()<< getBoardInfo() << "Motor" << motor << "failed to read" << _temperatureSensorErrorWatchdog.at(motor).getCount() << "temperature readings for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds"; + _temperatureSensorErrorWatchdog.at(motor).start(); + } } + return true; } - else //I get a NOT valid value + + //if I'm here I have a valid value + double delta_tmp = 0; + double tmp = _temperatureSensorsVector.at(motor)->convertRawToTempCelsius((double)mc_motor_status->basic.mot_temperature); + + // check if this is a spike or not + // evaluate difference between current and previous temperature + if(!_temperatureSpikesFilter.at(motor).isStarted()) //Pre-set of the filter buffer is ready { - if(! _temperatureSensorErrorWatchdog.at(motor).isStarted()) + _temperatureSpikesFilter.at(motor).start(tmp); + return true; + } + + // when i'm here the filter is ready. + delta_tmp = std::abs(tmp - _temperatureSpikesFilter.at(motor).getPrevTemperature()); + + //1. check if I have a good value (not a spike) + if(delta_tmp > _temperatureSpikesFilter.at(motor).getTemperatureThreshold()) + { + //it is a spike + return true; + } + // this is a not spike --> can update prev temperature + _temperatureSpikesFilter.at(motor).updatePrevTemperature(tmp); + + //2. tmp is good and check the limits + if(tmp > _temperatureLimits[motor].warningTemperatureLimit) + { + if(! _temperatureExceededLimitWatchdog.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(); + 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 { - _temperatureSensorErrorWatchdog.at(motor).increment(); - if( _temperatureSensorErrorWatchdog.at(motor).isExpired()) + if(_temperatureExceededLimitWatchdog.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(); + 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(); + } } return true; @@ -4821,10 +4849,10 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val) return ret; } - if (((double)status.mot_temperature) == temperatureErrorValue_s) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated - { - return false; - } + // 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 + // { + // return false; + // } *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature); diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index 5ce9989704..3b742ad20d 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -30,8 +30,10 @@ using namespace std; +// system std include #include #include +#include // Yarp stuff #include #include @@ -44,11 +46,12 @@ using namespace std; #include +#include +// local include #include "IethResource.h" #include"EoError.h" #include -#include #include "serviceParser.h" #include "eomcParser.h" @@ -75,8 +78,6 @@ using namespace std; #define EMBOBJMC_DONT_USE_MAIS - - // // helper structures // @@ -114,16 +115,14 @@ class Watchdog bool _isStarted; uint32_t _count; +double _time; 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(): _count(0), _isStarted(false), _threshold(60000), _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; @@ -138,8 +137,49 @@ void increment() {++_count;} void clear(){_isStarted=false;} double getStartTime() {return _time;} uint32_t getCount() {return _count; } +void setThreshold(uint8_t txrateOfRegularROPs){_threshold = _threshold / txrateOfRegularROPs;} +uint32_t getThreshold(){return _threshold;} + +}; +class TemperatureFilter +{ +private: + uint32_t _threshold; // threshold for the delta between current and previous temperature --> set to 20 Celsius deg by default --> over 20 deg delta spike + double _motorTempPrev; // motor temperature at previous instant for checking positive temperature spikes + bool _isStarted; + int32_t _initCounter; + std::vector _initTempBuffer; +public: + TemperatureFilter(): _threshold(20), _isStarted(false), _initCounter(50), _initTempBuffer(0), _motorTempPrev(0){;} + TemperatureFilter(uint32_t threshold, int32_t initCounter): _threshold(threshold), _isStarted(false), _initCounter(initCounter), _initTempBuffer(0), _motorTempPrev(0){;} + ~TemperatureFilter() = default; + TemperatureFilter(const TemperatureFilter& other) = default; + TemperatureFilter(TemperatureFilter&& other) noexcept = default; + TemperatureFilter& operator=(const TemperatureFilter& other) = default; + TemperatureFilter& operator=(TemperatureFilter&& other) noexcept = default; + + bool isStarted(){return _isStarted;} + uint32_t getTemperatureThreshold() {return _threshold; } + double getPrevTemperature(){return _motorTempPrev;} + void updatePrevTemperature(double temperature){_motorTempPrev = temperature;} + void start(double temperature) + { + if(_initCounter < 0) + { + int median_pos = std::ceil(_initTempBuffer.size() / 2) -1; + _motorTempPrev = _initTempBuffer.at(median_pos); + _isStarted = true; + } + else + { + _initTempBuffer.push_back(temperature); + --_initCounter; + } + + } }; + }}} namespace yarp { @@ -220,7 +260,6 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, public ImplementJointFault { private: - eth::TheEthManager* ethManager; eth::AbstractEthResource* res; ServiceParser* parser; @@ -243,26 +282,26 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, std::vector> _temperatureSensorsVector; - eomc::focBasedSpecificInfo_t * _foc_based_info; + eomc::focBasedSpecificInfo_t * _foc_based_info; - std::vector _jointEncs; - std::vector _motorEncs; + std::vector _jointEncs; + std::vector _motorEncs; - 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 */ - std::vector _timeouts; + 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 */ + std::vector _timeouts; std::vector _impedance_params; /** impedance parameters */ // TODO doubled!!! optimize using just one of the 2!!! - eomc::impedanceLimits_t * _impedance_limits; /** impedancel imits */ + eomc::impedanceLimits_t * _impedance_limits; /** impedancel imits */ eomc::PidInfo * _trj_pids; - //eomc::PidInfo * _dir_pids; + //eomc::PidInfo * _dir_pids; eomc::TrqPidInfo * _trq_pids; eomc::PidInfo * _cur_pids; eomc::PidInfo * _spd_pids; @@ -298,13 +337,13 @@ class yarp::dev::embObjMotionControl: public DeviceDriver, double *_ref_positions; // used for direct position control. double *_ref_accs; // for velocity control, in position min jerk eq is used. double *_encodersStamp; /** keep information about acquisition time for encoders read */ - bool *checking_motiondone; /* flag telling if I'm already waiting for motion done */ + bool *checking_motiondone; /* flag telling if I'm already waiting for motion done */ #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*/ - + std::vector _temperatureSpikesFilter; #ifdef NETWORK_PERFORMANCE_BENCHMARK Tools:Emb_RensponseTimingVerifier m_responseTimingVerifier;