Add method for getting temperature update from callback

src/libraries/icubmod/embObjLib/FeatureInterface.cpp

@@ -242,6 +242,38 @@ eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, vo
     return eobool_true;
 }
 
+eObool_t feat_manage_motor_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *rxdata)
+{
+    IethResource* mc = nullptr;
+
+    if (_interface2ethManager == nullptr)
+    {
+        return eobool_false;
+    }
+
+    mc = _interface2ethManager->getInterface(ipv4, id32);
+
+    if (nullptr == mc)
+    {
+        char ipinfo[20];
+        char nvinfo[128];
+        eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
+        eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
+        yDebug("feat_manage_motor_data() fails to get a handle of embObjMotionControl for IP = %s and NV = %s", ipinfo, nvinfo);
+        return eobool_false;
+    }
+
+    if(mc->initialised() == false)
+    {
+        return eobool_false;
+    }
+    else
+    {
+        mc->getMotorTemperature(id32, yarp::os::Time::now(), rxdata);
+    }
+
+    return eobool_true;
+}
 
 void* feat_MC_handler_get(eOipv4addr_t ipv4, eOprotID32_t id32)
 {

src/libraries/icubmod/embObjLib/FeatureInterface.h

@@ -51,6 +51,8 @@ eObool_t feat_manage_skin_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *array
 
 eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *data);
 
+eObool_t feat_manage_motor_data(eOipv4addr_t ipv4, eOprotID32_t id32, void* rxdata);
+
 void * feat_MC_handler_get(eOipv4addr_t ipv4, eOprotID32_t id32);
 
 double feat_yarp_time_now(void);

src/libraries/icubmod/embObjLib/protocolCallbacks/EoProtocolMC_fun_userdef.c

@@ -83,6 +83,7 @@ extern void eoprot_fun_UPDT_mc_joint_status_debug(const EOnv* nv, const eOropdes
 
 extern void eoprot_fun_UPDT_mc_motor_status_basic(const EOnv* nv, const eOropdescriptor_t* rd)
 {
+    feat_manage_motor_data(eo_nv_GetIP(nv), rd->id32);
 }
 
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -137,7 +137,7 @@ bool embObjMotionControl::alloc(int nj)
     _jointEncs.resize(nj);
     _motorEncs.resize(nj);
     _kalman_params.resize(nj);
-    _temperatureSensorsVector.resize(nj);
+    _temperatureSensorsVector.resize(nj, nullptr);
     _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
@@ -1006,6 +1006,14 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
             }
         }
     }
+    else
+    {
+        for (j = 0; j < _njoints; j++)
+        {
+            temperatureSensorsVector.at(j) = std::make_unique<eomc::TemperatureSensorNONE>();
+        }
+    }
+
 
 
     /////// [TIMEOUTS]
@@ -1560,26 +1568,6 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
     {   // do it only if we already have opened the device
         std::lock_guard<std::mutex> 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";
-                    }
-                }
-            }
-        }
-
     }
 
 
@@ -1633,6 +1621,31 @@ bool embObjMotionControl::getEntityName(uint32_t entityId, std::string &entityNa
 
 }
 
+bool embObjMotionControl::getMotorTemperature(eOprotID32_t id32, double timestamp, void *rxdata)
+{
+    // use this function to update the values cached in the class using data received by the remote boards via the network callbacks
+    // this function will specifically get the motor data (for now only the temperature)
+    size_t motor = eoprot_ID2index(id32);
+
+    if(true == initialised())
+    {
+        if(!getTemperaturesRaw(_temperatureValues))
+        {
+            yError() << "embObjMotionControl::getTemperaturesRaw failed for" << getBoardInfo() << "at motor" << motor;
+            return false;
+        }
+        else
+        {
+            if (_temperatureValues[motor] > _temperatureLimits[motor].warningTemperatureLimit)
+            {
+                yWarning() << getBoardInfo() << "Temperature limit for motor" << motor << " overcame! Processes not stopped but consider to decrese motor usage or reduce currents and PWMs to not risk motor damaging";
+            }
+        }
+    }
+
+    return true;
+}
+
 ///////////// PID INTERFACE
 bool embObjMotionControl::setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid)
 {
@@ -4761,19 +4774,19 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* 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))
+            //if ((_foc_based_info[m].temperatureSensorType != motor_temperature_sensor_none) && (serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc))
+            if(_temperatureSensorsVector.at(m) != nullptr)
             {
                 *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";
+                yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
                 return false;
             }
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -364,6 +364,7 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     virtual eth::iethresType_t type();
     virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata);
     virtual bool getEntityName(uint32_t entityId, std::string &entityName);
+    virtual bool getMotorTemperature(eOprotID32_t id32, double timestamp, void *rxdata);
 
     /////////   PID INTERFACE   /////////
     virtual bool setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) override;

src/libraries/icubmod/embObjMotionControl/eomcParser.h

@@ -262,7 +262,7 @@ class TemperatureSensorNONE : public ITemperatureSensor
 
     virtual double convertRawToTempCelsius(const double temperature) override
     {
-        yError("convertRawToTempCelsius METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE");
+        //yError("convertRawToTempCelsius METHOD, NOT IMPLEMENTED for class TemperatureSensorNONE");
         return 0;
     }
 };