Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add latest fault state to motor status data structure #224

Merged
merged 11 commits into from
Dec 15, 2021
Merged

Add latest fault state to motor status data structure #224

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
46aedcc
Add assignment of fault state bitmask to motor status struct
mfussi66 Dec 2, 2021
2fe4e81
Add lines to send error on embobj
mfussi66 Dec 6, 2021
accd445
Pass error code to upstream
mfussi66 Dec 6, 2021
2873035
Renamed fault variable in accordance to firmware shared and removed c…
mfussi66 Dec 7, 2021
fd94ba0
Check motor status pointer against potential dereferencing
mfussi66 Dec 9, 2021
fea4149
Added missing dereferencing check
mfussi66 Dec 9, 2021
de42a0a
Add function to set fault state to dummy before checking for new faul…
mfussi66 Dec 13, 2021
4d9c628
Add reset to dummy of mc fault state on activation and deactivation
mfussi66 Dec 13, 2021
15d0aab
Remove inclusion of unneeded library
mfussi66 Dec 13, 2021
192c0a9
Check if fault is active before get
mfussi66 Dec 14, 2021
aa839d9
Updated defines regarding build date and version for all boards
mfussi66 Dec 15, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
17 changes: 16 additions & 1 deletion emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;
}


Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ extern "C" {
// <o> minor <0-255>


#define EOMTHEEMSAPPLCFG_VERSION_MINOR 45
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 46


// </h>version
Expand All @@ -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

Expand Down
9 changes: 9 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@
#include "EoError.h"
#include "EOtheErrorManager.h"

#include "EOtheEntities.h"

#include "EOMtheEMSrunner.h"

#include "EOemsControllerCfg.h"
Expand Down Expand Up @@ -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)
Expand Down
18 changes: 17 additions & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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;
Expand Down Expand Up @@ -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)
Expand Down
30 changes: 29 additions & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

#include "EOtheErrorManager.h"
#include "EoError.h"
#include "EOtheEntities.h"

#if defined(EOTHESERVICES_customize_handV3_7joints)

Expand Down Expand Up @@ -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};
Expand Down Expand Up @@ -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) //
Expand Down Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

////////////////////////////////////////////////////////////////////////////
Expand Down