Add check of error value from tdb

robotology · Nov 8, 2023 · c29f73b · c29f73b
1 parent b15b141
commit c29f73b
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
@@ -4754,9 +4754,17 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val)
     if(ret)
     {
         *val = (double) status.mot_temperature ;
-        if (_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none)
+        if (*val != -5000) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated
         {
-            *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius(*val);
+            if (_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none)
+            {
+                *val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius(*val);
+            }
+        }
+        else
+        {
+            yError() << "Cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
+            return false;
         }
     }
     else

diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h
@@ -111,7 +111,7 @@ class TemperatureSensorPT100 : public ITemperatureSensor
         _resolution_tdb = 32767; 
 
         _half_bridge_resistor_coeff = (double)_r_3 / (double)(_r_2 + _r_3);
-        bool isConfigured = true;
+        isConfigured = true;
     }
 
     TemperatureSensorPT100(const TemperatureSensorPT100& other) = default; // const copy constructor