Retrieve latest joint fault state from firmware for exposition towards client

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,29 @@ 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();
+
+    if (!ret)
+    {
+        fault = -1;
+        message = "Could not retrieve the fault state.";
+        return false;
+    }
+
+    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,10 @@ 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
+    virtual bool getLastJointFaultRaw(int j, int& fault, std::string& message) override;
+
 };
 
 #endif // include guard