Improved the diagnostic of quadrature encoder (#972)

robotology · Jul 20, 2024 · 5cb39b0 · 5cb39b0
1 parent 1e1ce96
commit 5cb39b0
Showing 1 changed file with 15 additions and 1 deletion.
diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp
@@ -646,7 +646,7 @@ void MotionControlParser::parseInfo()
         } break;
 
 
-        case eoerror_value_MC_motor_qencoder_dirty:
+        //case eoerror_value_MC_motor_qencoder_dirty:
         case eoerror_value_MC_motor_qencoder_phase: //TBD: check encoder raw value
         {
             uint16_t joint_num = m_dnginfo.param16;
@@ -659,6 +659,20 @@ void MotionControlParser::parseInfo()
             m_dnginfo.baseInfo.finalMessage.append(str);
         } break;
 
+        case eoerror_value_MC_motor_qencoder_dirty: 
+        {
+            uint16_t joint_num = m_dnginfo.param16;
+            uint16_t dirty_error = m_dnginfo.param64 & 0xffff;
+            uint16_t index_error = (m_dnginfo.param64 & 0xffff0000)>>16;
+            uint16_t phase_error = (m_dnginfo.param64 & 0xffff00000000)>>32;
+            m_entityNameProvider.getAxisName(joint_num, m_dnginfo.baseInfo.axisName);
+
+            snprintf(str, sizeof(str), " %s (Joint=%s (NIB=%d), index=%d, dirty=%d, phase=%d)",
+                                        m_dnginfo.baseMessage.c_str(), m_dnginfo.baseInfo.axisName.c_str(), joint_num, index_error, dirty_error, phase_error
+                                        );
+            m_dnginfo.baseInfo.finalMessage.append(str);
+        } break;
+
         case eoerror_value_MC_generic_error: //TBD Check print
         {
             eOmc_motorFaultState_t motor_status;