Update filtering of spikes in temp reading (#961)

Author: MSECode

src/libraries/icubmod/embObjLib/abstractEthResource.h

@@ -53,6 +53,7 @@ namespace eth {
             string              boardtypeString;
             string              boardnameString;
             string              firmwareString;
+            uint8_t             txROPratedivider;
         };
 
         //AbstractEthResource();

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);
 

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;

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -8,19 +8,25 @@
 */
 
 //#include <yarp/dev/CanBusInterface.h>
-#include <yarp/os/Bottle.h>
-#include <yarp/os/Time.h>
+// system std include
 #include <string.h>
 #include <iostream>
 #include <cmath>
 
+// yarp include
+#include <yarp/os/Bottle.h>
+#include <yarp/os/Time.h>
 #include "embObjMotionControl.h"
 #include <ethManager.h>
 #include <FeatureInterface.h>
 #include <yarp/conf/environment.h>
 
 #include <yarp/os/LogStream.h>
 
+#include <yarp/os/NetType.h>
+#include <yarp/dev/ControlBoardHelper.h>
+
+// local include
 #include "EoCommon.h"
 #include "EOarray.h"
 #include "EoProtocol.h"
@@ -29,10 +35,6 @@
 #include "EoProtocolAS.h"
 #include "motionControlDefaultValues.h"
 
-#include <yarp/os/NetType.h>
-#include <yarp/dev/ControlBoardHelper.h>
-
-
 #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<eOmc_motor_status_t*>(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);
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -30,8 +30,10 @@
 
 using namespace std;
 
+// system std include
 #include <string>
 #include <mutex>
+#include <math.h>
 //  Yarp stuff
 #include <yarp/os/Bottle.h>
 #include <yarp/os/BufferedPort.h>
@@ -44,11 +46,12 @@ using namespace std;
 
 #include<yarp/dev/ImplementJointFault.h>
 
+#include <abstractEthResource.h>
 
+// local include
 #include "IethResource.h"
 #include"EoError.h"
 #include <ethManager.h>
-#include <abstractEthResource.h>
 
 #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<double> _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<std::unique_ptr<eomc::ITemperatureSensor>> _temperatureSensorsVector;  
 
-    eomc::focBasedSpecificInfo_t *            _foc_based_info;
+    eomc::focBasedSpecificInfo_t *           _foc_based_info;
 
-    std::vector<eomc::encoder_t>            _jointEncs;
-    std::vector<eomc::encoder_t>            _motorEncs;
+    std::vector<eomc::encoder_t>             _jointEncs;
+    std::vector<eomc::encoder_t>             _motorEncs;
 
-    std::vector<eomc::rotorLimits_t>        _rotorsLimits; /** contains limit about rotors such as position and pwm */
-    std::vector<eomc::jointLimits_t>        _jointsLimits; /** contains limit about joints such as position and velocity */
-    std::vector<eomc::motorCurrentLimits_t> _currentLimits;
-    std::vector<eomc::temperatureLimits_t>  _temperatureLimits;
-    eomc::couplingInfo_t                    _couplingInfo; /** contains coupling matrix */
-    std::vector<eomc::JointsSet>            _jsets;
-    std::vector<int>                        _joint2set;   /** for each joint says the number of  set it belongs to */
-    std::vector<eomc::timeouts_t>           _timeouts;
+    std::vector<eomc::rotorLimits_t>         _rotorsLimits; /** contains limit about rotors such as position and pwm */
+    std::vector<eomc::jointLimits_t>         _jointsLimits; /** contains limit about joints such as position and velocity */
+    std::vector<eomc::motorCurrentLimits_t>  _currentLimits;
+    std::vector<eomc::temperatureLimits_t>   _temperatureLimits;
+    eomc::couplingInfo_t                     _couplingInfo; /** contains coupling matrix */
+    std::vector<eomc::JointsSet>             _jsets;
+    std::vector<int>                         _joint2set;   /** for each joint says the number of  set it belongs to */
+    std::vector<eomc::timeouts_t>            _timeouts;
 
     std::vector<eomc::impedanceParameters_t> _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<yarp::dev::eomc::Watchdog>    _temperatureSensorErrorWatchdog;  /* counter used to filter error coming from tdb reading fromm 2FOC board*/
     std::vector<yarp::dev::eomc::Watchdog>    _temperatureExceededLimitWatchdog;  /* counter used to filter the print of the exeded limits*/
-    
+    std::vector<yarp::dev::eomc::TemperatureFilter> _temperatureSpikesFilter;
 
 #ifdef NETWORK_PERFORMANCE_BENCHMARK 
     Tools:Emb_RensponseTimingVerifier m_responseTimingVerifier;