Update Generic error parsing

robotology · Feb 20, 2024 · 64f43fe · 64f43fe
1 parent 8301198
commit 64f43fe
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 65 deletions.
diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp
@@ -141,65 +141,6 @@ void InfoFormatter::getSeverityOfError(EmbeddedInfo &info)
     };
 }
 
-void InfoFormatter::motorStatusBitsToString(uint32_t motorstatus, std::string statusstring)
-{
-    statusstring.clear();
-    static const std::array<std::string, 32> s_motor_fault_status =
-    {
-        //B0 L
-        {"ExternalFaultAsserted"},
-        {"UnderVoltageFailure"},
-        {"OverVoltageFailure"},
-        {"OverCurrentFailure"},
-        //B0 H
-        {"DHESInvalidValue"},
-        {"AS5045CSumError"},
-        {"DHESInvalidSequence"},
-        {"CANInvalidProtocol"},
-        //B1 L
-        {"CAN_BufferOverRun"},
-        {"SetpointExpired"},
-        {"CAN_TXIsPasv"},
-        {"CAN_RXIsPasv"},
-        //B1 H
-        {"CAN_IsWarnTX"},
-        {"CAN_IsWarnRX"},
-        {"OverHeatingFailure"},
-        {""},
-        //B2 L
-        {"ADCCalFailure"},
-        {"I2TFailure"},
-        {"EMUROMFault"},
-        {"EMUROMCRCFault"},
-        //B2 H
-        {"EncoderFault"},
-        {"FirmwareSPITimingError"},
-        {"AS5045CalcError"},
-        {"FirmwarePWMFatalError"},
-        //B3 L
-        {"CAN_TXWasPasv"},
-        {"CAN_RXWasPasv"},
-        {"CAN_RTRFlagActive"},
-        {"CAN_WasWarn"},
-        //B3 H
-        {"CAN_DLCError"},
-        {"SiliconRevisionFault"},
-        {"PositionLimitUpper"},
-        {"PositionLimitLower"}
-    };
-
-    std::stringstream mfaultss = {};
-    std::string tmps = "";
-
-    for (uint8_t i = 0; i < s_motor_fault_status.size(); i++)
-    {
-        // check bit by bit and add to ss the faulted bits
-        tmps = embot::core::binary::bit::check(motorstatus, i) ? s_motor_fault_status.at(i) : "";
-        mfaultss << " " << tmps;
-    }
-    statusstring = mfaultss.str();
-}
-
 /**************************************************************************************************************************/
 /******************************************        EntityNameProvider       ***************************************************/
 /**************************************************************************************************************************/

diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp
@@ -10,6 +10,7 @@
 #include "diagnosticLowLevelFormatter_hid.h"
 #include "diagnosticLowLevelFormatter.h"
 #include "EoBoards.h"
+#include "embot_core_binary.h"
 
 
 using namespace Diagnostic::LowLevel;
@@ -532,6 +533,65 @@ void ConfigParser::parseInfo()
 /******************************************   MotionControlParser   ***************************************************/
 /**************************************************************************************************************************/
 
+// private class functions
+void MotionControlParser::motorStatusBitsToString(uint32_t motorstatus, std::string &statusstring)
+{
+    statusstring.clear();
+    static const std::array<std::string, 32> s_motor_fault_status =
+    {
+        //B0 L
+        "ExternalFaultAsserted",
+        "UnderVoltageFailure",
+        "OverVoltageFailure",
+        "OverCurrentFailure",
+        //B0 H
+        "DHESInvalidValue",
+        "AS5045CSumError",
+        "DHESInvalidSequence",
+        "CANInvalidProtocol",
+        //B1 L
+        "CAN_BufferOverRun",
+        "SetpointExpired",
+        "CAN_TXIsPasv",
+        "CAN_RXIsPasv",
+        //B1 H
+        "CAN_IsWarnTX",
+        "CAN_IsWarnRX",
+        "OverHeatingFailure",
+        "",
+        //B2 L
+        "ADCCalFailure",
+        "I2TFailure",
+        "EMUROMFault",
+        "EMUROMCRCFault",
+        //B2 H
+        "EncoderFault",
+        "FirmwareSPITimingError",
+        "AS5045CalcError",
+        "FirmwarePWMFatalError",
+        //B3 L
+        "CAN_TXWasPasv",
+        "CAN_RXWasPasv",
+        "CAN_RTRFlagActive",
+        "CAN_WasWarn",
+        //B3 H
+        "CAN_DLCError",
+        "SiliconRevisionFault",
+        "PositionLimitUpper",
+        "PositionLimitLower"
+    };
+
+    std::stringstream mfaultss = {};
+
+    for (uint8_t i = 0; i < s_motor_fault_status.size(); i++)
+    {
+        // check bit by bit and add to ss the faulted bits
+        if(embot::core::binary::bit::check(motorstatus, i))
+            mfaultss << " " << s_motor_fault_status.at(i);
+    }
+    statusstring = mfaultss.str();
+}
+
 MotionControlParser::MotionControlParser(AuxEmbeddedInfo &dnginfo, EntityNameProvider &entityNameProvider):DefaultParser(dnginfo, entityNameProvider){;}
 
 void MotionControlParser::parseInfo()
@@ -596,13 +656,13 @@ void MotionControlParser::parseInfo()
 
         case eoerror_value_MC_generic_error: //TBD Check print
         {
-            std::string motorStatusString = "";
+            std::string motorStatusString;
             uint16_t joint_num = m_dnginfo.param16;
             uint32_t motor_status = m_dnginfo.param64 & 0xffffffff;
             m_entityNameProvider.getAxisName(joint_num, m_dnginfo.baseInfo.axisName);
             motorStatusBitsToString(motor_status, motorStatusString);
-            snprintf(str, sizeof(str), " %s (Joint=%s (NIB=%d) (Error is %s)", 
-                                        m_dnginfo.baseMessage.c_str(), m_dnginfo.baseInfo.axisName.c_str(), joint_num, motorStatusString
+            snprintf(str, sizeof(str), " %s (Joint=%s (NIB=%d) (Errors:%s)", 
+                                        m_dnginfo.baseMessage.c_str(), m_dnginfo.baseInfo.axisName.c_str(), joint_num, motorStatusString.c_str()
                                         );
             m_dnginfo.baseInfo.finalMessage.append(str);
         } break;
@@ -1405,7 +1465,7 @@ void InertialSensorParser::parseInfo()
         case eoerror_value_IS_arrayofinertialdataoverflow:
         {
 
-            uint8_t frame_id = m_dnginfo.param16 & 0x00ff;
+            uint8_t frame_id = m_dnginfo.param16 & 0x0fff;
             uint8_t frame_size = (m_dnginfo.param16 & 0xf000) >> 12;
             uint64_t frame_data = m_dnginfo.param64;
 

diff --git a/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter.h b/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter.h
@@ -49,7 +49,6 @@ class Diagnostic::LowLevel::InfoFormatter
     void getSourceOfMessage(Diagnostic::EmbeddedInfo &info);
     void ipv4ToString(Diagnostic::EmbeddedInfo &info);
     void getSeverityOfError(Diagnostic::EmbeddedInfo &info);
-    void motorStatusBitsToString(uint32_t motorstatus, std::string &statusstring);
 };
 
 #endif //__diagnosticLowLevelFormatter_h__
diff --git a/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h b/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h
@@ -116,7 +116,8 @@ class Diagnostic::LowLevel::MotionControlParser : public Diagnostic::LowLevel::D
 
     void parseInfo();
 
-
+private:
+void motorStatusBitsToString(uint32_t motorstatus, std::string &statusstring);
 };
 
 class Diagnostic::LowLevel::SkinParser : public Diagnostic::LowLevel::DefaultParser