Update diagnostic to make it more general

Make for abs encoders (at joint) are more general
Moroever, now we show the abs encoder type too in the diagnostic message

src/libraries/icubmod/embObjLib/IethResource.cpp

@@ -75,6 +75,12 @@ bool IethResource::getEntityName(uint32_t entityId, std::string &entityName)
     return true;
 }
 
+bool IethResource::getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName)
+{
+    encoderTypeName.clear();
+    return true;
+}
+
 
 // - end-of-file (leave a blank line after)----------------------------------------------------------------------------
 

src/libraries/icubmod/embObjLib/IethResource.h

@@ -91,14 +91,24 @@ namespace eth {
 
     public:
             const char * stringOfType();
-            /* the entityName there will be the name of the entity, or it can be an empty string if the enity han't name
+            /* the entityName there will be the name of the entity, or it can be an empty string if the enity hasn't name
             The entity could be:
               - axis in case the ethResource type is iethres_motioncontrol
               - sensor id in case of, skin, mais and strain 
               The defiult value is an empty string 
               return false in case of error, like entityId is not present. Aniway the entity name is initialize to empty string.*/
             virtual bool getEntityName(uint32_t entityId, std::string &entityName);
 
+            /* the encoderTypeName will be the name of the type of the encoder mounted at the joint or at the motor, 
+            or it can be the string "ERROR" is a corresponding name is not found
+            The entity, i.e. the jomo, can be:
+              - motor
+              - joint
+            The default value is the empty string
+            It returns false in case of error and the encoderTypeName string is filled with "ERROR"
+            */
+            virtual bool getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName);
+
     private:
             static const char * names[iethresType_numberof+1];
     };

src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp

@@ -157,4 +157,15 @@ bool EntityNameProvider::getAxisName(uint32_t entityId, std::string &axisName)
     }
 
     return (m_MC_ethRes->getEntityName(entityId, axisName));
+}
+
+bool EntityNameProvider::getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName)
+{
+    if (m_MC_ethRes == nullptr)  
+    {
+        encoderTypeName = "N/A";
+        return false;
+    }
+
+    return (m_MC_ethRes->getEncoderTypeName(jomoId, pos, encoderTypeName));
 }

src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp

@@ -12,6 +12,7 @@
 #include "embot_core_binary.h"
 
 #include <yarp/os/Time.h>
+#include <algorithm>
 
 
 using namespace Diagnostic::LowLevel;
@@ -631,16 +632,19 @@ void MotionControlParser::parseInfo()
             m_dnginfo.baseInfo.finalMessage.append(str);
         } break;
 
-        case eoerror_value_MC_aea_abs_enc_invalid:
-        case eoerror_value_MC_aea_abs_enc_spikes:
-        case eoerror_value_MC_aea_abs_enc_timeout:
+        case eoerror_value_MC_abs_enc_invalid:
+        case eoerror_value_MC_abs_enc_spikes:
+        case eoerror_value_MC_abs_enc_timeout:
         {
+            std::string encoderTypeName = {};
             uint8_t joint_num = m_dnginfo.param16 & 0x00ff;
             uint8_t enc_port = (m_dnginfo.param16 & 0xff00)>>8;
             m_entityNameProvider.getAxisName(joint_num, m_dnginfo.baseInfo.axisName);
+            m_entityNameProvider.getEncoderTypeName(joint_num, eomc_pos_atjoint, encoderTypeName);
+            std::transform(encoderTypeName.begin(), encoderTypeName.end(), encoderTypeName.begin(), ::toupper);
 
-            snprintf(str, sizeof(str), " %s (Joint=%s (NIB=%d), encoderPort=%d)",
-                                        m_dnginfo.baseMessage.c_str(), m_dnginfo.baseInfo.axisName.c_str(), joint_num, enc_port
+            snprintf(str, sizeof(str), " %s (Joint=%s (NIB=%d), encoderPort=%d, encoderType=%s)",
+                                        m_dnginfo.baseMessage.c_str(), m_dnginfo.baseInfo.axisName.c_str(), joint_num, enc_port, encoderTypeName.c_str()
                                         );
             m_dnginfo.baseInfo.finalMessage.append(str);
         } break;

src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h

@@ -73,6 +73,7 @@ class Diagnostic::LowLevel::EntityNameProvider
     eth::IethResource* m_MC_ethRes;
 public:
     bool getAxisName(uint32_t entityId, std::string &axisName);
+    bool getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName);
 };
 
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -1721,7 +1721,6 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
             _temperatureExceededLimitWatchdog.at(motor).clear();
         }
     }
-
     return true;
 }
 
@@ -1740,6 +1739,38 @@ bool embObjMotionControl::getEntityName(uint32_t entityId, std::string &entityNa
 
 }
 
+
+bool embObjMotionControl::getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName)
+{
+    encoderTypeName.clear();
+
+    if ((jomoId >= 0) && (jomoId < _njoints))
+    {
+        switch (pos)
+        {
+        case eomc_pos_atjoint:
+            encoderTypeName = eomc_encoder2string(_jointEncs[jomoId].type, eobool_true);
+            break;
+        case eomc_pos_atmotor:
+            encoderTypeName = eomc_encoder2string(_motorEncs[jomoId].type, eobool_true);
+            break;
+        case eomc_pos_unknown:
+            encoderTypeName = "UNKNOWN";
+            break;
+        case eomc_pos_none:
+        default:
+            encoderTypeName = "NONE";
+            break;
+        }
+        return true;
+    }
+    else
+    {
+        encoderTypeName = "ERROR";
+        return false;
+    }
+}
+
 ///////////// PID INTERFACE
 bool embObjMotionControl::setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid)
 {

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -447,6 +447,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 getEncoderTypeName(uint32_t jomoId, eOmc_position_t pos, std::string &encoderTypeName);
 
     /////////   PID INTERFACE   /////////
     virtual bool setPidRaw(const PidControlTypeEnum& pidtype, int j, const Pid &pid) override;