diff --git a/src/libraries/icubmod/embObjLib/IethResource.cpp b/src/libraries/icubmod/embObjLib/IethResource.cpp index b3d4d48d10..24e148d4f1 100644 --- a/src/libraries/icubmod/embObjLib/IethResource.cpp +++ b/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)---------------------------------------------------------------------------- diff --git a/src/libraries/icubmod/embObjLib/IethResource.h b/src/libraries/icubmod/embObjLib/IethResource.h index 1f33a1a241..763fec4b3b 100644 --- a/src/libraries/icubmod/embObjLib/IethResource.h +++ b/src/libraries/icubmod/embObjLib/IethResource.h @@ -91,7 +91,7 @@ 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 @@ -99,6 +99,16 @@ namespace eth { 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]; }; diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp index 0521d644b3..9ad22ddc3d 100644 --- a/src/libraries/icubmod/embObjLib/diagnosticInfoFormatter.cpp +++ b/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)); } \ No newline at end of file diff --git a/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp b/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp index 871b88c8ad..956dabcc30 100644 --- a/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp +++ b/src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp @@ -12,6 +12,7 @@ #include "embot_core_binary.h" #include +#include 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; diff --git a/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h b/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h index 03489f6197..a7cdb05512 100644 --- a/src/libraries/icubmod/embObjLib/diagnosticLowLevelFormatter_hid.h +++ b/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); }; diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 84a8e90c7a..18a1a2491b 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/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) { diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index 02ce831d2e..c890d7e495 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/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;