From da9402d44c7b2bd3b057f210b2b02d6f05a387d4 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Wed, 15 Jan 2025 10:20:35 +0100 Subject: [PATCH] Start work on update of AMO encoder diagnostic Update AMO diagnostic and fix errors for overheating and i2c comm failure Update error sending for amo Update versions for ems, mc2plus, mc4plus, amc, amc2c --- .../v2/cfg/theApplication_config.h | 4 +- .../embot_app_board_amc2c_info.cpp | 4 +- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 8 +- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 10 +- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 10 +- .../embobj/plus/board/EOappEncodersReader.c | 165 ++++++++++++++++-- .../plus/board/EOappEncodersReader_hid.h | 12 ++ .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 1 - emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c | 15 ++ 9 files changed, 192 insertions(+), 37 deletions(-) diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h index 962fcda21d..de8a50369f 100644 --- a/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h +++ b/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h @@ -38,8 +38,8 @@ namespace embot { namespace app { namespace eth { .property = { Process::eApplication, - {2, 16}, - {2025, Month::Jan, Day::thirty, 11, 15} + {2, 17}, + {2025, Month::Feb, Day::seven, 18, 00} }, .OStick = 1000*embot::core::time1microsec, .OSstacksizeinit = 10*1024, diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp index 6f107a48ad..753f1f9f73 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp @@ -39,9 +39,9 @@ namespace embot::app::board::amc2c::info { { embot::app::boards::Board::amc2c, {embot::app::msg::BUS::icc1, address}, - {3, 2, 0, 0}, // application version + {3, 3, 0, 0}, // application version {2, 0}, // protocol version - {2025, embot::app::eth::Month::Jan, embot::app::eth::Day::thirty, 11, 15} + {2025, embot::app::eth::Month::Feb, embot::app::eth::Day::seven, 18, 00} }; constexpr embot::app::msg::Location icclocation {signature.location}; 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 9fb36801b9..952df94cae 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,18 +81,18 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 99 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 100 // version // build date // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2025 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 1 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 2 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 22 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 7 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 0 // build date 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 34c07eca23..7f5eb42d69 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 78 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 79 // version @@ -83,13 +83,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2025 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 01 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 02 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 22 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 7 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 21 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00 // 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 57e9d14003..2c3c1a1bf0 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 @@ -84,7 +84,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 99 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 100 // version @@ -92,13 +92,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2025 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 01 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 02 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 22 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 7 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 22 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00 // build date diff --git a/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c b/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c index c569079a75..f54e96c263 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c @@ -42,6 +42,7 @@ #include "hal_spiencoder.h" #include "hal_quadencoder.h" #include "hal_adc.h" +#include "embot_core_binary.h" #include "EOtheEntities.h" #include "EOconstarray.h" @@ -131,6 +132,7 @@ static eObool_t s_eo_appEncReader_IsValidValue_AEA3(uint32_t *valueraw, eOencode static eObool_t s_eo_appEncReader_IsValidValue_AKSIM2(uint8_t jomo, hal_spiencoder_diagnostic_t* diag, eOencoderreader_errortype_t *error); static eObool_t s_eo_appEncReader_IsValidValue_SPICHAIN2(uint32_t *valueraw, eOencoderreader_errortype_t *error); static eObool_t s_eo_appEncReader_IsValidValue_SPICHAIN3(uint32_t *valueraw, eOencoderreader_errortype_t *error); +static eObool_t s_eo_appEncReader_IsValidValue_AMO(uint8_t jomo, hal_spiencoder_diagnostic_t* diag, eOencoderreader_errortype_t *error); static void s_eo_appEncReader_deconfigure_NONSPI_encoders(EOappEncReader *p); static void s_eo_appEncReader_configure_NONSPI_encoders(EOappEncReader *p); @@ -211,7 +213,7 @@ static EOappEncReader s_eo_theappencreader = EO_INIT(.regs) { 0, 0 }, EO_INIT(.cnts) { 0, 0}, EO_INIT(.one) { 0, 0, 0, 0, 0 }, - EO_INIT(.two) { 0, 0, 0, 0, 0 } + EO_INIT(.two) { 0, 0, 0, 0, 0 } }, EO_INIT(.aksim2DiagnerrorCounters ) { @@ -221,6 +223,15 @@ static EOappEncReader s_eo_theappencreader = EO_INIT(.encoder_error_hal_counter ) {0, 0, 0, 0}, EO_INIT(.encoder_error_total_timer_counter ) {0, 0, 0, 0} }, + EO_INIT(.amoDiagnerrorCounters ) + { + EO_INIT(.status0_poorlevel_clipping_master_counter ) {0, 0, 0, 0}, + EO_INIT(.status0_poorlevel_clipping_nonius_counter ) {0, 0, 0, 0}, + EO_INIT(.status1_CRC_counter ) {0, 0, 0, 0}, + EO_INIT(.status1_i2C_communication_counter ) {0, 0, 0, 0}, + EO_INIT(.status1_period_consistency_warning_counter ) {0, 0, 0, 0}, + EO_INIT(.status1_eccessive_signal_freq_counter ) {0, 0, 0, 0} + }, EO_INIT(.genericEncoderRawData) { EO_INIT(.val) { 0, 0, 0 }, @@ -777,6 +788,8 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen case eomc_enc_amo: { + // TODO: move or update --> japo + static uint16_t amodiagnostic_counter[2] = {}; // hal_spiencoder_errors_flags flags = {0}; #if defined(FAKE_AMO) static int32_t cnt = 0; @@ -790,22 +803,32 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen //spiRawValue = (spiRawValue>>4) & 0xFFFF; marco.accame on 07jun17: why is there this comment? shall we remove it? // GOOD VALUE - - eOmc_joint_t *joint = (eOmc_joint_t*) eoprot_entity_ramof_get(eoprot_board_localboard, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, jomo); - - int16_t sectors = (int16_t)(joint->config.gearbox_E2J + 0.5f); - - aux_rawvalue = rawValue; - if (sectors == 32) + if(eobool_true == s_eo_appEncReader_IsValidValue_AMO(jomo, &diagn, &prop.valueinfo->errortype)) { - aux_rawvalue = (aux_rawvalue >> 1) & 0x3FFF; + eOmc_joint_t *joint = (eOmc_joint_t*) eoprot_entity_ramof_get(eoprot_board_localboard, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, jomo); + + int16_t sectors = (int16_t)(joint->config.gearbox_E2J + 0.5f); + + aux_rawvalue = rawValue; + if (sectors == 32) + { + aux_rawvalue = (aux_rawvalue >> 1) & 0x3FFF; + } + else if (sectors == 64) + { + aux_rawvalue = aux_rawvalue & 0x3FFF; + } + + prop.valueinfo->value[0] = s_eo_appEncReader_rescale2icubdegrees(aux_rawvalue, jomo, (eOmc_position_t)prop.descriptor->pos); } - else if (sectors == 64) + else { - aux_rawvalue = aux_rawvalue & 0x3FFF; + // we have a valid reading from hal but ... it is not valid after a check + //errorparam = diagn.info.value.hal_spiencoder_diagnostic_type_amo_notconn; + //TODO: probably we need to add another error + // for now let's just use error generic + prop.valueinfo->errortype = encreader_err_AMO_GENERIC; } - - prop.valueinfo->value[0] = s_eo_appEncReader_rescale2icubdegrees(aux_rawvalue, jomo, (eOmc_position_t)prop.descriptor->pos); } else { // we dont even have a valid reading from hal ..... @@ -816,6 +839,48 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen #endif + if( ++amodiagnostic_counter[jomo] > 10000) + { + // preperare data for diagnostics + eOerrmanDescriptor_t errdes = {0}; + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.par16 = jomo; + errdes.par64 = 0; + + if(s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_master_counter[jomo] > 0 || + s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_nonius_counter[jomo] > 0) + { + errdes.code = eoerror_code_get(eoerror_category_HardWare, eoerror_value_HW_amo_encoder_status0); + errdes.par64 = ((uint64_t)s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_nonius_counter[jomo] << 32) | + ((uint64_t)s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_master_counter[jomo]); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, NULL, &errdes); + + s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_master_counter[jomo] = 0; + s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_nonius_counter[jomo] = 0; + } + if((s_eo_theappencreader.amoDiagnerrorCounters.status1_CRC_counter[jomo] > 0) || + (s_eo_theappencreader.amoDiagnerrorCounters.status1_i2C_communication_counter[jomo] > 0) || + (s_eo_theappencreader.amoDiagnerrorCounters.status1_period_consistency_warning_counter[jomo] > 0) || + (s_eo_theappencreader.amoDiagnerrorCounters.status1_eccessive_signal_freq_counter[jomo] > 0 )) + { + + errdes.code = eoerror_code_get(eoerror_category_HardWare, eoerror_value_HW_amo_encoder_status1); + errdes.par64 = ((uint64_t)s_eo_theappencreader.amoDiagnerrorCounters.status1_CRC_counter[jomo] << 48) | + ((uint64_t)s_eo_theappencreader.amoDiagnerrorCounters.status1_i2C_communication_counter[jomo] << 32) | + ((uint64_t)s_eo_theappencreader.amoDiagnerrorCounters.status1_period_consistency_warning_counter[jomo] << 16) | + ((uint64_t)s_eo_theappencreader.amoDiagnerrorCounters.status1_eccessive_signal_freq_counter[jomo]); + + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, NULL, &errdes); + + s_eo_theappencreader.amoDiagnerrorCounters.status1_CRC_counter[jomo] = 0; + s_eo_theappencreader.amoDiagnerrorCounters.status1_i2C_communication_counter[jomo] = 0; + s_eo_theappencreader.amoDiagnerrorCounters.status1_period_consistency_warning_counter[jomo] = 0; + s_eo_theappencreader.amoDiagnerrorCounters.status1_eccessive_signal_freq_counter[jomo] = 0; + } + amodiagnostic_counter[jomo] = 0; + } + // and now ... use diagn // for legacy diagnostics rawdiagn = ((uint32_t)diagn.type << 16) | (diagn.info.value & 0xFF); @@ -1016,10 +1081,10 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen p->diagnostics.par16[i] |= (encreader_err_GENERIC<<(4*jomo)); // shift by nibbles .. } break; - case encreader_err_AKSIM2_CLOSE_TO_LIMITS: - case encreader_err_AKSIM2_CRC_ERROR: - case encreader_err_AKSIM2_INVALID_DATA: - case encreader_err_AKSIM2_GENERIC: +// case encreader_err_AKSIM2_CLOSE_TO_LIMITS: +// case encreader_err_AKSIM2_CRC_ERROR: +// case encreader_err_AKSIM2_INVALID_DATA: +// case encreader_err_AKSIM2_GENERIC: case encreader_err_AEA_READING: case encreader_err_AEA_PARITY: case encreader_err_AEA_CHIP: @@ -1028,7 +1093,7 @@ extern eOresult_t eo_appEncReader_GetValue(EOappEncReader *p, uint8_t jomo, eOen case encreader_err_MAIS_GENERIC: case encreader_err_PSC_GENERIC: case encreader_err_POS_GENERIC: - case encreader_err_AMO_GENERIC: +// case encreader_err_AMO_GENERIC: case encreader_err_SPICHAINOF2_GENERIC: case encreader_err_SPICHAINOF3_GENERIC: { // in such cases, we report the errortype and the errorparam that someone has prepared @@ -1477,18 +1542,21 @@ static eObool_t s_eo_appEncReader_IsValidValue_AKSIM2(uint8_t jomo, hal_spiencod if(0x04 == (0x04 & diag->info.aksim2_status_crc)) { + *error = encreader_err_AKSIM2_INVALID_DATA; ++s_eo_theappencreader.aksim2DiagnerrorCounters.encoder_error_invalid_data_counter[jomo]; ret = eobool_false; } if(0x02 == (0x02 & diag->info.aksim2_status_crc)) { + *error = encreader_err_AKSIM2_CLOSE_TO_LIMITS; ++s_eo_theappencreader.aksim2DiagnerrorCounters.encoder_error_close_to_limit_counter[jomo]; ret = eobool_false; } if(0x01 == (0x01 & diag->info.aksim2_status_crc)) { + *error = encreader_err_AKSIM2_CRC_ERROR; ++s_eo_theappencreader.aksim2DiagnerrorCounters.encoder_error_crc_counter[jomo]; ret = eobool_false; } @@ -1497,6 +1565,67 @@ static eObool_t s_eo_appEncReader_IsValidValue_AKSIM2(uint8_t jomo, hal_spiencod return ret; } +static eObool_t s_eo_appEncReader_IsValidValue_AMO(uint8_t jomo, hal_spiencoder_diagnostic_t* diag, eOencoderreader_errortype_t *error) +{ + // in case of errors we return false. Initially we assume no errors + eObool_t ret = eobool_true; + + if(hal_spiencoder_diagnostic_type_amo_status0 == diag->type) + { + uint16_t diagnostic_status_0 = diag->info.value; + + switch(diagnostic_status_0) + { + + case 1: + case 2: + { + ++s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_master_counter[jomo]; + + } break; + case 4: + case 8: + { + ++s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_nonius_counter[jomo]; + + } break; + default: + { + ++s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_master_counter[jomo]; + ++s_eo_theappencreader.amoDiagnerrorCounters.status0_poorlevel_clipping_nonius_counter[jomo]; + + } break; + } + } + else if(hal_spiencoder_diagnostic_type_amo_status1 == diag->type) + { + uint16_t diagnostic_status_1 = diag->info.value; + if((embot::core::binary::bit::check(diagnostic_status_1, 1)) || (embot::core::binary::bit::check(diagnostic_status_1, 2))) + { + // eccessive frequency error + ++s_eo_theappencreader.amoDiagnerrorCounters.status1_eccessive_signal_freq_counter[jomo]; + } + if((embot::core::binary::bit::check(diagnostic_status_1, 3))) + { + // period counter consistency error --> counted period != calculated Nonius position + ++s_eo_theappencreader.amoDiagnerrorCounters.status1_period_consistency_warning_counter[jomo]; + } + if((embot::core::binary::bit::check(diagnostic_status_1, 6))) + { + // i2c comunication error --> no eeprom or i2c communication error + ++s_eo_theappencreader.amoDiagnerrorCounters.status1_i2C_communication_counter[jomo]; + } + if((embot::core::binary::bit::check(diagnostic_status_1, 7))) + { + // crc error --> invalid check sum internal RAM + ++s_eo_theappencreader.amoDiagnerrorCounters.status1_CRC_counter[jomo]; + } + } + + return ret; +} + + static eObool_t s_eo_appEncReader_IsValidValue_SPICHAIN2(uint32_t *valueraw, eOencoderreader_errortype_t *error) { uint16_t first = *valueraw & 0xffff; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader_hid.h b/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader_hid.h index 335d18e811..50ca76358d 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader_hid.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader_hid.h @@ -126,6 +126,17 @@ typedef struct } eOappEncReader_Aksim2_DiagnosticError_Counters_t; +// structure defined for counting the errors related to amo encoder +typedef struct +{ + uint16_t status0_poorlevel_clipping_master_counter[eOappEncReader_jomos_maxnumberof]; + uint16_t status0_poorlevel_clipping_nonius_counter[eOappEncReader_jomos_maxnumberof]; + uint16_t status1_CRC_counter[eOappEncReader_jomos_maxnumberof]; + uint16_t status1_i2C_communication_counter[eOappEncReader_jomos_maxnumberof]; + uint16_t status1_period_consistency_warning_counter[eOappEncReader_jomos_maxnumberof]; + uint16_t status1_eccessive_signal_freq_counter[eOappEncReader_jomos_maxnumberof]; +} eOappEncReader_Amo_DiagnosticError_Counters_t; + struct EOappEncReader_hid { eObool_t initted; @@ -139,6 +150,7 @@ struct EOappEncReader_hid eOappEncReader_hallAdc_conversionData_t hallAdcConversionData; eo_appEncReader_amodiag_t amodiag; eOappEncReader_Aksim2_DiagnosticError_Counters_t aksim2DiagnerrorCounters; + eOappEncReader_Amo_DiagnosticError_Counters_t amoDiagnerrorCounters; eOencoderreader_RawValuesOfJomo_t genericEncoderRawData[eOappEncReader_jomos_maxnumberof]; }; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index 4128bea3ea..8d0f5c35dc 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -383,7 +383,6 @@ BOOL JointSet_do_check_faults(JointSet* o) encoder_fault = TRUE; } } - //if an encoder of this set is in fault ten set hw fault on each joint of this set. if(encoder_fault) { for (int k=0; kfault_state.bits.I2C_CommFailure && !o->fault_state_prec.bits.I2C_CommFailure) + { + Motor_send_error(o->ID, eoerror_value_MC_motor_tdb_not_reading, 0); + fault_state.bits.I2C_CommFailure = FALSE; + } + // #define CAN_GENERIC_ERROR 0x00003D00 // // if ((o->fault_state.bitmask & CAN_GENERIC_ERROR) && ((o->fault_state.bitmask & CAN_GENERIC_ERROR) != (o->fault_state_prec.bitmask & CAN_GENERIC_ERROR))) @@ -959,6 +965,15 @@ static void Motor_raise_fault_overheating(Motor* o) o->control_mode = icubCanProto_controlmode_hwFault; } +static void Motor_raise_fault_i2c_communication_failure(Motor* o) +{ + hal_motor_disable(static_cast(o->motorlocation.adr)); + + o->fault_state.bits.I2C_CommFailure = TRUE; + + o->control_mode = icubCanProto_controlmode_hwFault; +} + void Motor_raise_fault_i2t(Motor* o) { hal_motor_disable(static_cast(o->motorlocation.adr));