Retrieve latest joint fault state from firmware for exposition towards client

CMakeLists.txt

@@ -15,7 +15,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 option(ICUB_COMPILE_BINDINGS "Compile optional language bindings"  FALSE)
 
 set(YCM_REQUIRED_VERSION 0.13.0)
-set(YARP_REQUIRED_VERSION 3.5.1)
+set(YARP_REQUIRED_VERSION 3.5.100)
 
 find_package(YCM ${YCM_REQUIRED_VERSION} REQUIRED)
 find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED)

conf/iCubFindDependencies.cmake

@@ -72,8 +72,8 @@ checkandset_dependency(Qt5)
 
 if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
   # icub-firmware-shared 4.0.7 was actually a wrong version exported by icub-firmware-shared <= 1.15
-  if(icub_firmware_shared_VERSION VERSION_GREATER 4 OR icub_firmware_shared_VERSION VERSION_LESS 1.22.1)
-    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected, but at least 1.22.1 is required")
+  if(icub_firmware_shared_VERSION VERSION_GREATER 4 OR icub_firmware_shared_VERSION VERSION_LESS 1.22.2)
+    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected, but at least 1.22.2 is required")
   endif()
 endif()
 

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp

@@ -202,6 +202,7 @@ embObjMotionControl::embObjMotionControl() :
     ImplementAxisInfo(this),
     ImplementPWMControl(this),
     ImplementCurrentControl(this),
+    ImplementJointFault(this),
     SAFETY_THRESHOLD(2.0),
     _rotorsLimits(0),
     _jointsLimits(0),
@@ -321,6 +322,7 @@ bool embObjMotionControl::initializeInterfaces(measureConvFactors &f)
     ImplementAxisInfo::initialize(_njoints, _axisMap);
     ImplementCurrentControl::initialize(_njoints, _axisMap, f.ampsToSensor);
     ImplementPWMControl::initialize(_njoints, _axisMap, f.dutycycleToPWM);
+    ImplementJointFault::initialize(_njoints, _axisMap);
 
     return true;
 
@@ -1245,7 +1247,7 @@ bool embObjMotionControl::init()
     {
         protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, n, eoprot_tag_mc_joint_status_core);
         id32v.push_back(protid);
-        protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, n, eoprot_tag_mc_motor_status_basic);
+        protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, n, eoprot_tag_mc_motor_status);
         id32v.push_back(protid);
     }
 
@@ -1435,6 +1437,7 @@ bool embObjMotionControl::close()
     ImplementAxisInfo::uninitialize();
     ImplementCurrentControl::uninitialize();
     ImplementPWMControl::uninitialize();
+    ImplementJointFault::uninitialize();
 
     if (_measureConverter)  {delete _measureConverter; _measureConverter=0;}
 
@@ -5256,4 +5259,39 @@ bool embObjMotionControl::getMotorEncTolerance(int axis, double *mEncTolerance_p
     return true;
 }
 
+bool embObjMotionControl::getLastJointFaultRaw(int j, int& fault, std::string& message)
+{
+    eOmc_motor_status_t status;
+
+    eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, 
+                                        eoprot_entity_mc_motor, j, 
+                                        eoprot_tag_mc_motor_status);
+
+    bool ret = res->getLocalValue(protid, &status);
+
+    message.clear();
+
+    // Set fault state to -2 to avoid ambiguity with eoerror_code_dummy
+    if (!ret)
+    {
+        fault = -2;
+        message = "Could not retrieve the fault state.";
+        return false;
+    }
+
+    // eoerror_code_dummy is defined in the firmware as 0xffffffffL and evaluates to -1
+    if (status.mc_fault_state == eoerror_code_dummy)
+    {
+        fault = eoerror_code_dummy;
+        message = "No fault detected.";
+
+        return true;
+    }
+
+    fault = eoerror_code2value(status.mc_fault_state);
+    message = eoerror_code2string(status.mc_fault_state);
+
+    return true;
+}
+
 // eof

src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h

@@ -42,10 +42,11 @@ using namespace std;
 
 #include <yarp/dev/IVirtualAnalogSensor.h>
 
-
+#include<yarp/dev/ImplementJointFault.h>
 
 
 #include "IethResource.h"
+#include"EoError.h"
 #include <ethManager.h>
 #include <abstractEthResource.h>
 
@@ -55,6 +56,7 @@ using namespace std;
 
 #include "mcEventDownsampler.h"
 
+
 #ifdef NETWORK_PERFORMANCE_BENCHMARK 
 #include <PeriodicEventsVerifier.h>
 #endif
@@ -179,10 +181,10 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     public ImplementPWMControl,
     public ICurrentControlRaw,
     public ImplementCurrentControl,
-    public eth::IethResource
+    public eth::IethResource,
+    public IJointFaultRaw,
+    public ImplementJointFault
     {
-
-
 private:
 
     eth::TheEthManager*        ethManager;
@@ -196,8 +198,6 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
 
     MCdiagnostics mcdiagnostics;
 
-
-
      /////configuartion info (read from xml files)
     int                                     _njoints;       /** Number of joints handled by this EMS */
     eomc::behaviour_flags_t                  behFlags;       /** Contains all flags that define the behaviour of this device */
@@ -612,6 +612,13 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     virtual bool setRefCurrentsRaw(const int n_joint, const int *joints, const double *t) override;
     virtual bool getRefCurrentsRaw(double *t) override;
     virtual bool getRefCurrentRaw(int j, double *t) override;
+
+    // Used in joint faults interface
+    // Teturns true if it was successful and writes the fault code in the fault parameter 
+    // with the associated string in message. If no fault is detected the fault parameters is set to -1.
+    // Returns false and fault is set to -2 if retrieval was unsuccessful.
+    virtual bool getLastJointFaultRaw(int j, int& fault, std::string& message) override;
+
 };
 
 #endif // include guard