From 3765a955219d10c71d88f74ecdbdc4d28526d6da Mon Sep 17 00:00:00 2001 From: Valentina Date: Mon, 15 Jul 2024 10:28:52 +0000 Subject: [PATCH] Improved the diagnostic of quadrature encoder --- .../icubmod/embObjLib/diagnosticInfoParsers.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp index 1ffe4e2688..871b88c8ad 100644 --- 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;