Add latest fault state to motor status data structure (#224)

* Add assignment of fault state bitmask to motor status struct

* Add lines to send error on embobj

* Pass error code to upstream

* Renamed fault variable in accordance to firmware shared and removed commented out functions

* Check motor status pointer against potential dereferencing

* Added missing dereferencing check

* Add function to set fault state to dummy before checking for new faults for each joint

* Add reset to dummy of mc fault state on activation and deactivation

* Remove inclusion of unneeded library

* Check if fault is active before get

* Updated defines regarding build date and version for all boards

emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h

@@ -81,21 +81,21 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          (VERSION_MAJOR_OFFSET+3)
 //  <o> minor           <0-255> 
 //  <o> minor           <0-255>
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          47
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          48
 
 //  </h>version
 
 //  <h> build date
 //  <o> year            <2010-2030>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR         2021
 //  <o> month           <1-12>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        11
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        12
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          23
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          15
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         16
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         12
 //  <o> minute          <0-59>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          34
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          35
 //  </h>build date
 // </h>Info 
 

emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c

@@ -813,7 +813,19 @@ extern eOresult_t eo_motioncontrol_Deactivate(EOtheMotionController *p)
         eo_vector_Clear(p->sharedcan.entitydescriptor);
         eo_vector_Clear(p->sharedcan.boardproperties);   
         eo_array_Reset(p->sharedcan.discoverytargets);                
-    }      
+    }
+
+    // Reset the communicated fault state to dummy for each motor
+    uint8_t n_motors = eo_entities_NumOfMotors(eo_entities_GetHandle());
+
+    for(uint8_t mId = 0; mId < n_motors; mId++)
+    {
+       eOmc_motor_status_t *mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), mId);
+       if (NULL != mstatus)
+       {
+           mstatus->mc_fault_state = eoerror_code_dummy;
+       }
+    }
 
     p->numofjomos = 0;
     eo_entities_SetNumOfJoints(eo_entities_GetHandle(), 0);
@@ -1665,6 +1677,9 @@ extern void eoprot_fun_INIT_mc_motor_status(const EOnv* nv)
 {
     eOmc_motor_status_t *sta = (eOmc_motor_status_t*)eo_nv_RAM(nv);
     memmove(sta, &s_motor_default_value.status, sizeof(eOmc_motor_status_t));
+
+    // Initialize the fault state to dummy since code zero is assigned to unspecified system error
+    sta->mc_fault_state = eoerror_code_dummy;
 }
 
 

emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h

@@ -75,7 +75,7 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          3
 //  <o> minor           <0-255> 
 //  <o> minor           <0-255>
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          31
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          32
 //  </h>version
 
 //  <h> build date
@@ -84,11 +84,11 @@ extern "C" {
 //  <o> month           <1-12>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        12
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          13
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          15
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         16
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         12
 //  <o> minute          <0-59>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          11
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          35
 //  </h>build date
 
 // </h>Info 

emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h

@@ -87,7 +87,7 @@ extern "C" {
 //  <o> minor           <0-255> 
 
 
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          45
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          46
 
 
 //  </h>version
@@ -98,11 +98,11 @@ extern "C" {
 //  <o> month           <1-12>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        12
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          6
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          15
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         11
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         12
 //  <o> minute          <0-59>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          38
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          35
 
 //  </h>build date
 

emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c

@@ -21,6 +21,8 @@
 #include "EoError.h"
 #include "EOtheErrorManager.h"
 
+#include "EOtheEntities.h"
+
 #include "EOMtheEMSrunner.h"
 
 #include "EOemsControllerCfg.h"
@@ -687,6 +689,13 @@ static void AbsEncoder_send_error(uint8_t id, eOerror_value_MC_t err_id, uint64_
     descriptor.sourceaddress = 0;
     descriptor.code = eoerror_code_get(eoerror_category_MotionControl, err_id);
     eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &descriptor);
+
+    eOmc_motor_status_t *mstatus = NULL;
+    mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), id);
+    if (NULL != mstatus)
+    {
+        mstatus->mc_fault_state = descriptor.code;
+    }
 }
 
 BOOL AbsEncoder_is_in_fault(AbsEncoder* o)

emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c

@@ -25,6 +25,8 @@
 #include "EOtheErrorManager.h"
 #include "EoError.h"
 
+#include "EOtheEntities.h"
+
 static void Joint_set_inner_control_flags(Joint* o);
 
 Joint* Joint_new(uint8_t n)
@@ -408,6 +410,13 @@ BOOL Joint_check_faults(Joint* o)
             descriptor.sourceaddress = 0;
             descriptor.code = eoerror_code_get(eoerror_category_MotionControl, eoerror_value_MC_axis_torque_sens);
             eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &descriptor);
+
+            eOmc_motor_status_t *mstatus = NULL;
+            mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), o->ID);
+            if (NULL != mstatus)
+            {
+                mstatus->mc_fault_state = descriptor.code;
+            }
         }
 
         if (o->fault_state.bits.hard_limit_reached && !o->fault_state_prec.bits.hard_limit_reached)
@@ -419,6 +428,13 @@ BOOL Joint_check_faults(Joint* o)
             descriptor.sourceaddress = 0;
             descriptor.code = eoerror_code_get(eoerror_category_MotionControl, eoerror_value_MC_joint_hard_limit);
             eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &descriptor);
+
+            eOmc_motor_status_t *mstatus = NULL;
+            mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), o->ID);
+            if (NULL != mstatus)
+            {
+                mstatus->mc_fault_state = descriptor.code;
+            }
         }
 
         o->fault_state_prec.bitmask = o->fault_state.bitmask;
@@ -916,7 +932,7 @@ void Joint_get_state(Joint* o, eOmc_joint_status_t* joint_state)
     joint_state->core.measures.meas_position         = o->pos_fbk;           
     joint_state->core.measures.meas_velocity         = o->vel_fbk;        
     joint_state->core.measures.meas_acceleration     = o->acc_fbk;      
-    joint_state->core.measures.meas_torque           = o->trq_fbk;
+    joint_state->core.measures.meas_torque           = o->trq_fbk;    
 }
 
 BOOL Joint_get_pid_state(Joint* o, eOmc_joint_status_ofpid_t* pid_state)

emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c

@@ -30,6 +30,7 @@
 
 #include "EOtheErrorManager.h"
 #include "EoError.h"
+#include "EOtheEntities.h"
 
 #if defined(EOTHESERVICES_customize_handV3_7joints)
 
@@ -701,8 +702,16 @@ static void Motor_send_error(uint8_t id, eOerror_value_MC_t err_id, uint64_t mas
     descriptor.sourceaddress = 0;
     descriptor.code = eoerror_code_get(eoerror_category_MotionControl, err_id);
     eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &descriptor);    
+
+    eOmc_motor_status_t *mstatus = NULL;
+    mstatus = eo_entities_GetMotorStatus(eo_entities_GetHandle(), id);
+    if (NULL != mstatus)
+    {
+        mstatus->mc_fault_state = descriptor.code;
+    }
 }
 
+
 static void Motor_send_diagnostic(eOerrmanErrorType_t errortype, uint8_t id, eOerror_value_MC_t err_id, uint64_t mask)
 {
     static eOerrmanDescriptor_t descriptor = {0}; 
@@ -1219,7 +1228,10 @@ void Motor_get_state(Motor* o, eOmc_motor_status_t* motor_status)
     motor_status->basic.mot_current  = o->Iqq_fbk; //o->Iqq_peak_fbk;    
     motor_status->basic.mot_pwm      = o->pwm_fbk;
 
-
+    if (Motor_is_motor_joint_fault_over(o))
+    {
+        motor_status->mc_fault_state = eoerror_code_dummy;
+    }
 }
 
 void Motor_update_odometry_fbk_can(Motor* o, CanOdometry2FocMsg* can_msg) //
@@ -1379,6 +1391,22 @@ void Motor_reset(Motor *o)
 }
 
 
+BOOL Motor_is_motor_joint_fault_over(Motor* o)
+{
+    BOOL ret = TRUE;
+
+    ret &= !Motor_is_in_fault(o);
+
+    eOmc_joint_status_t* jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), o->ID);    
+
+    if (NULL != jstatus)
+    {
+        ret &= (jstatus->core.modes.controlmodestatus != eomc_ctrlmval_hwFault);
+    }
+
+    return ret;
+}
+
 #if defined(EOTHESERVICES_customize_handV3_7joints)
 
 // - here is the new code for the pmc board

emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h

@@ -295,6 +295,8 @@ extern BOOL Motor_is_external_fault(Motor* o);
 extern BOOL Motor_is_in_fault(Motor* o);
 extern BOOL Motor_is_running(Motor* o);
 
+extern BOOL Motor_is_motor_joint_fault_over(Motor* o);
+
 //BOOL Motor_clear_ext_fault(Motor *o);
 
 ////////////////////////////////////////////////////////////////////////////