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

Work on update of AMO encoder diagnostic #564

Open
wants to merge 1 commit into
base: devel
Choose a base branch
from
Open

Work on update of AMO encoder diagnostic #564

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions ...Bcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_info.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
8 changes: 4 additions & 4 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,18 +81,18 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 99
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 100
// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2025
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 1
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 2
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 22
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 7
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 16
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 0
// </h>build date
Expand Down
10 changes: 5 additions & 5 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,21 +75,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 78
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 79

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2025
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 01
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 02
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 22
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 7
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 21
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00
// </h>build date

// </h>Info
Expand Down
10 changes: 5 additions & 5 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 @@ -84,21 +84,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>

#define EOMTHEEMSAPPLCFG_VERSION_MINOR 99
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 100

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2025
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 01
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 02
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 22
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 7
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 22
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00

// </h>build date

Expand Down
165 changes: 147 additions & 18 deletions emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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 )
{
Expand All @@ -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 },
Expand Down Expand Up @@ -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;
Expand All @@ -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 .....
Expand All @@ -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);
Expand Down Expand Up @@ -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:
Expand All @@ -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
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand Down
12 changes: 12 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/board/EOappEncodersReader_hid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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];
};

Expand Down
1 change: 0 additions & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; k<N; ++k)
Expand Down
Loading