diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 3e1e780d7..78a10265b 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -81,7 +81,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 47 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 48 // version @@ -89,13 +89,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2021 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 23 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 15 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 12 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 34 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 35 // build date // Info diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c index 32197b00f..bd6e4d7bd 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c +++ b/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; } diff --git a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 958b949a0..509f696ed 100644 --- a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -75,7 +75,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3 // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 31 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 32 // version // build date @@ -84,11 +84,11 @@ extern "C" { // month <1-12> #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 15 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 12 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 35 // build date // Info diff --git a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index c8842e426..13cec022e 100644 --- a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -87,7 +87,7 @@ extern "C" { // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 45 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 46 // version @@ -98,11 +98,11 @@ extern "C" { // month <1-12> #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 6 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 15 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 12 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 38 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 35 // build date diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c index 436d7ea79..03e105c1c 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c +++ b/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) diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c index 202f2c40e..4528b65f0 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c +++ b/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) diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index e44660e57..768ab449e 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/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 diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h index 1829fe108..a9762f221 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/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); ////////////////////////////////////////////////////////////////////////////