extern void eoprot_fun_UPDT_mc_controller_config_jointcoupling(const EOnv* nv, const eOropdescriptor_t* rd)
{
eOmc_jointcouplingmatrix_t *mat = (eOmc_jointcouplingmatrix_t*)rd->data;
/*
float Ji[4][4];
for (int i=0; i<4; ++i)
{
for (int j=0; j<4; ++j)
{
Ji[i][j]=(float)((*mat)[i][j])/16384.0f;
}
}
*/
eo_emsController_set_Jacobian(*mat);
#warning --> marco.accame: put in here the debug messages for jointcoupling (and then remove them)
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00);
errdes.param = 0;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
//char *str = NULL;
char str[eomn_info_status_extra_sizeof] = {0};
for (int i=0; i<4; ++i)
{
snprintf(str, sizeof(str), "r%d: %f %f %f %f", i, Ji[i][0], Ji[i][1], Ji[i][2], Ji[i][3]);
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, str, NULL, &errdes);
}
}
static void s_eo_currents_watchdog_CheckI2T(uint8_t motor, int16_t value)
{
//change sign to check absolute value
if (value < 0)
value = -value;
uint32_t averageCurrent = s_eo_currents_watchdog_averageCalc_addValue( motor, value);
if(!s_eo_currents_watchdog_averageCalc_collectDataIsCompleted(motor))
{
return;
}
// 1) calculate Ep
float Ep_aux= (averageCurrent*averageCurrent) - s_eo_currents_watchdog.nominalCurrent2[motor];
s_eo_currents_watchdog.accomulatorEp[motor] += Ep_aux;
if(s_eo_currents_watchdog.accomulatorEp[motor] < 0) //Ep could not be smaller than zero
{
s_eo_currents_watchdog.accomulatorEp[motor] = 0;
}
// 2) check if current Ep is bigger than threshold then rais fault
if( s_eo_currents_watchdog.accomulatorEp[motor] > s_eo_currents_watchdog.I2T_threshold[motor])
{
s_eo_currents_watchdog.motorinI2Tfault[motor] = eobool_true;
MController_motor_raise_fault_i2t(motor);
}
else
{
//I need to raise I2T fault until the current system energy is not littler than I2T threshold divided 2. (we decided so....)
if(s_eo_currents_watchdog.motorinI2Tfault[motor])
{
if(s_eo_currents_watchdog.accomulatorEp[motor] > (s_eo_currents_watchdog.I2T_threshold[motor]/2))
{
MController_motor_raise_fault_i2t(motor);
}
else
{
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00);
errdes.par16 = motor;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
char str[100];
snprintf(str, sizeof(str), "Ep < I2T/2: now it is possible put in idle the motor");
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, str, NULL, &errdes);
s_eo_currents_watchdog.motorinI2Tfault[motor] = eobool_false;
}
}
}
// 3) reset average data
s_eo_currents_watchdog_averageCalc_reset(motor);
}
// marco.accame on Nov 26 2014: this function is triggered if function eom_emssocket_Transmit() fails
// to transmit a udp packet.
extern void eom_emsconfigurator_hid_userdef_onemstransceivererror(EOMtheEMStransceiver* p)
{
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_configurator_udptxfailure);
errdes.par16 = 0;
errdes.par64 = 0;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, "EOMtheEMSconfigurator", &errdes);
}
static void s_osal_cfg_on_fatal_error(void* task, osal_fatalerror_t errorcode, const char * errormsg)
{
uint8_t tskid = 0;
char str[64];
osal_task_id_get((osal_task_t*)task, &tskid);
snprintf(str, sizeof(str), "osalerror %d taskid %d: %s", errorcode, tskid, errormsg);
if(eobool_true == eo_errman_IsErrorHandlerConfigured(eo_errman_GetHandle()))
{
// ok ... use the error manager, either in its simple form or in its networked form
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_osalerror);
errdes.par16 = errorcode;
errdes.par64 = 0;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, str, "OSAL", &errdes);
}
else
{
if(NULL != errormsg)
{
hal_trace_puts(errormsg);
}
// in case of fatal error we blink all leds but led1
hal_led_init(hal_led0, NULL);
hal_led_init(hal_led1, NULL);
hal_led_init(hal_led2, NULL);
hal_led_init(hal_led3, NULL);
hal_led_init(hal_led4, NULL);
hal_led_init(hal_led5, NULL);
hal_led_off(hal_led0);
hal_led_off(hal_led1);
hal_led_off(hal_led2);
hal_led_off(hal_led3);
hal_led_off(hal_led4);
hal_led_off(hal_led5);
for(;;)
{
hal_sys_delay(100);
hal_led_toggle(hal_led0);
//hal_led_toggle(hal_led1);
hal_led_toggle(hal_led2);
hal_led_toggle(hal_led3);
hal_led_toggle(hal_led4);
hal_led_toggle(hal_led5);
}
}
}
static void s_hal_core_cfg_on_fatalerror(hal_fatalerror_t errorcode, const char * errormsg)
{
if(eobool_true == eo_errman_IsErrorHandlerConfigured(eo_errman_GetHandle()))
{
// ok ... use the error manager, either in its simple form or in its networked form
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_halerror);
errdes.par16 = errorcode;
errdes.par64 = 0;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
eo_errman_Error(eo_errman_GetHandle(), (hal_fatalerror_warning == errorcode) ? eo_errortype_warning : eo_errortype_fatal, errormsg, "HAL", &errdes);
}
else
{
if(NULL != errormsg)
{
hal_trace_puts(errormsg);
}
if(hal_fatalerror_warning == errorcode)
{
return;
}
// in case of fatal error we blink all leds but led0
hal_led_init(hal_led0, NULL);
hal_led_init(hal_led1, NULL);
hal_led_init(hal_led2, NULL);
hal_led_init(hal_led3, NULL);
hal_led_init(hal_led4, NULL);
hal_led_init(hal_led5, NULL);
hal_led_off(hal_led0);
hal_led_off(hal_led1);
hal_led_off(hal_led2);
hal_led_off(hal_led3);
hal_led_off(hal_led4);
hal_led_off(hal_led5);
for(;;)
{
hal_sys_delay(100);
//hal_led_toggle(hal_led0);
hal_led_toggle(hal_led1);
hal_led_toggle(hal_led2);
hal_led_toggle(hal_led3);
hal_led_toggle(hal_led4);
hal_led_toggle(hal_led5);
}
}
}
void JointSet_send_debug_message(char *message, uint8_t jid)
{
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01);
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = jid;
errdes.par16 = 0;
errdes.par64 = 0;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, message, NULL, &errdes);
}
extern EOtheSKIN* eo_skin_Initialise(void)
{
EOtheSKIN* p = &s_eo_theskin;
if(eobool_true == p->service.initted)
{
return(p);
}
p->numofskinpatches = 0;
p->numofmtbs = 0;
p->service.servconfig.type = eomn_serv_NONE;
p->sharedcan.boardproperties = eo_vector_New(sizeof(eObrd_canproperties_t), eo_skin_maxnumberofMTBboards, NULL, NULL, NULL, NULL);
p->sharedcan.entitydescriptor = eo_vector_New(sizeof(eOcanmap_entitydescriptor_t), eo_skin_maxnumberofMTBboards, NULL, NULL, NULL, NULL);
uint8_t i=0;
for(i=0; i<eomn_serv_skin_maxpatches; i++)
{
p->skinpatches[i] = NULL;
p->patchisrunning[i] = eobool_false;
p->rxdata[i] = eo_vector_New(sizeof(eOsk_candata_t), 64, NULL, NULL, NULL, NULL);
}
p->id32ofregulars = eo_array_New(skin_maxRegulars, sizeof(uint32_t), NULL);
p->diagnostics.reportTimer = eo_timer_New();
p->diagnostics.errorType = eo_errortype_error;
p->diagnostics.errorDescriptor.sourceaddress = eo_errman_sourcedevice_localboard;
p->diagnostics.errorDescriptor.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_skin_not_verified_yet);
p->service.initted = eobool_true;
p->service.active = eobool_false;
p->service.started = eobool_false;
p->service.state = eomn_serv_state_idle;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, p->service.state);
return(p);
}
static void s_eo_currents_watchdog_CheckI2T(uint8_t motor, int16_t value)
{
float I = value;
s_eo_currents_watchdog.accomulatorEp[motor] += 0.001f*(I*I - s_eo_currents_watchdog.nominalCurrent2[motor]);
if (s_eo_currents_watchdog.accomulatorEp[motor] < 0.0f) s_eo_currents_watchdog.accomulatorEp[motor] = 0.0f;
// 2) check if current Ep is bigger than threshold then rais fault
if( s_eo_currents_watchdog.accomulatorEp[motor] > s_eo_currents_watchdog.I2T_threshold[motor])
{
s_eo_currents_watchdog.motorinI2Tfault[motor] = eobool_true;
MController_motor_raise_fault_i2t(motor);
}
else
{
//I need to raise I2T fault until the current system energy is not littler than I2T threshold divided 2. (we decided so....)
if(s_eo_currents_watchdog.motorinI2Tfault[motor])
{
if(s_eo_currents_watchdog.accomulatorEp[motor] > (0.5f*s_eo_currents_watchdog.I2T_threshold[motor]))
{
MController_motor_raise_fault_i2t(motor);
}
else
{
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00);
errdes.par16 = motor;
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
char str[100];
snprintf(str, sizeof(str), "Ep < I2T/2: now it is possible put in idle the motor");
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, str, NULL, &errdes);
s_eo_currents_watchdog.motorinI2Tfault[motor] = eobool_false;
}
}
}
}
static void s_eocanprotASperiodic_strain_saturation_handler(eOcanframe_t *frame, eOcanport_t port, strainProcessMode_t mode, uint16_t msg_counter)
{
static uint16_t upper_saturations[6] = {0};
static uint16_t lower_saturations[6] = {0};
//there's saturation
if (frame->size == 7)
{
uint8_t info = frame->data[6]; //byte containing info about saturation
if (info != 0)
{
switch (mode)
{
case processForce:
{
icubCanProto_strain_forceSaturationInfo_t* force_info = (icubCanProto_strain_forceSaturationInfo_t*) &info;
if (force_info->saturationInChannel_0 == saturationLOW)
lower_saturations[0]++;
else if (force_info->saturationInChannel_0 == saturationHIGH)
upper_saturations[0]++;
if (force_info->saturationInChannel_1 == saturationLOW)
lower_saturations[1]++;
else if (force_info->saturationInChannel_1 == saturationHIGH)
upper_saturations[1]++;
if (force_info->saturationInChannel_2 == saturationLOW)
lower_saturations[2]++;
else if (force_info->saturationInChannel_2 == saturationHIGH)
upper_saturations[2]++;
} break;
case processTorque:
{
icubCanProto_strain_torqueSaturationInfo_t* torque_info = (icubCanProto_strain_torqueSaturationInfo_t*) &info;
if (torque_info->saturationInChannel_3 == saturationLOW)
lower_saturations[3]++;
else if (torque_info->saturationInChannel_3 == saturationHIGH)
upper_saturations[3]++;
if (torque_info->saturationInChannel_4 == saturationLOW)
lower_saturations[4]++;
else if (torque_info->saturationInChannel_4 == saturationHIGH)
upper_saturations[4]++;
if (torque_info->saturationInChannel_5 == saturationLOW)
lower_saturations[5]++;
else if (torque_info->saturationInChannel_5 == saturationHIGH)
upper_saturations[5]++;
} break;
}
}
else
{
// send diag message about malformed message
// uncomment if you need it for debugging
/*
eOerrmanDescriptor_t errdes = {0};
errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame);
errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01);
errdes.par16 = 0;
errdes.par64 = 0;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, "strain saturation byte 7 (if sent) should be different from 0!", NULL, &errdes);
*/
}
}
//send statistics every second (n.b. --> 2 CAN msgs from STRAIN every ms), but only if something happened
if (msg_counter == 2000)
{
//send saturation message for every channel, if any
for (uint8_t i = 0; i < 6; i++)
{
eOerrmanDescriptor_t errdes = {0};
if (upper_saturations[i] != 0 || lower_saturations[i] != 0)
{
errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame);
errdes.code = eoerror_code_get(eoerror_category_HardWare, eoerror_value_HW_strain_saturation);
errdes.par16 = i; //channel involved
errdes.par64 = (uint64_t) (upper_saturations[i]) << 32 | (uint64_t) lower_saturations[i]; //LSW->lower_sat, MSW->upper_sat
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes);
upper_saturations[i] = 0;
lower_saturations[i] = 0;
}
}
}
}
static eOresult_t s_eocanprotASperiodic_parser_process_forcetorque(eOcanframe_t *frame, eOcanport_t port, strainProcessMode_t mode)
{
// this can frame is from strain only ... i dont do the check that the board must be a strain
// i retrieve the strain entity related to the frame
eOas_strain_t *strain = NULL;
eOprotIndex_t index = EOK_uint08dummy;
if(NULL == (strain = s_eocanprotASperiodic_get_entity(eoprot_endpoint_analogsensors, eoprot_entity_as_strain, frame, port, &index)))
{
return(eores_OK);
}
// set incoming force values
switch(strain->config.mode)
{
case eoas_strainmode_txcalibrateddatacontinuously:
case eoas_strainmode_txalldatacontinuously:
{
eo_array_Assign((EOarray*)(&strain->status.calibratedvalues), 3*mode, &(frame->data[0]), 3);
} break;
case eoas_strainmode_txuncalibrateddatacontinuously:
{
eo_array_Assign((EOarray*)(&strain->status.uncalibratedvalues), 3*mode, &(frame->data[0]), 3);
} break;
case eoas_strainmode_acquirebutdonttx:
{
// i dont do anything in here. but i dont return nok. because it may be that we must empty a rx buffer of canframes rx just before
// that we have silenced the strain.
} break;
default:
{
//i must never be here
//#warning -> TODO: add diagnostics about unknown mode as in s_eo_icubCanProto_mb_send_runtime_error_diagnostics()
}
}
//check saturation
//#define SIMPLE_SATURATION_DIAGNOSTIC
#if defined (SIMPLE_SATURATION_DIAGNOSTIC)
static uint16_t count_message = 0;
if (frame->size == 7)
{
//check 7th byte, which should include the saturation bit
if (frame->data[6] != 0x00)
//send dedicated diagnostics
{
if ((count_message == 0) || (count_message == 300)) //if it's the first time or every 300ms, if it's continuosly saturating
{
eOerrmanDescriptor_t errdes = {0};
errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame);
errdes.code = eoerror_code_get(eoerror_category_HardWare, eoerror_value_HW_strain_saturation);
errdes.par16 = frame->size;
errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame);
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes);
}
if (count_message == 300)
count_message = 0;
count_message++;
}
}
else
{
count_message = 0;
}
#else
static uint16_t counter = 0;
counter++;
if (counter > 2000) // stays for 1 second...
counter = 0;
s_eocanprotASperiodic_strain_saturation_handler(frame, port, mode, counter);
#endif
return(eores_OK);
}
static void s_eo_candiscovery2_sendDiagnosticsToHost(eObool_t allboardsfound, eObool_t allboardsareok)
{
eOerrmanDescriptor_t errdes = {0};
uint8_t i = 0;
uint8_t n = 0;
// 1. at first we send info about what we have discovered
// now we send more info if: board is unmatched, fw is different, prot is different.
// by defining SEND_FOR_ALL we also send and info if everything is ok
for(i=eOcanport1; i<eOcanports_number; i++)
{
uint16_t differentboardtype = s_eo_thecandiscovery2.detection.differentboardtype[i] & s_eo_thecandiscovery2.target.canmap[i];
uint16_t differentfirmwareversion = s_eo_thecandiscovery2.detection.differentfirmwareversion[i] & s_eo_thecandiscovery2.target.canmap[i];
uint16_t differentprotocolversion = s_eo_thecandiscovery2.detection.differentprotocolversion[i] & s_eo_thecandiscovery2.target.canmap[i];
uint16_t maskofdifferences = differentboardtype | differentfirmwareversion | differentprotocolversion;
for(n=0; n<15; n++)
{
if(eobool_true == eo_common_hlfword_bitcheck(maskofdifferences, n))
{ // we have differences
errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_candiscovery_detectedboard);
errdes.sourcedevice = (eOcanport1 == i) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = 0;
errdes.par16 = (n & 0x000f);
errdes.par64 = ((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.firmware.build) |
((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.firmware.minor << 8) | ((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.firmware.major << 16) |
((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.protocol.minor << 24) | ((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.protocol.major << 32) |
((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.type << 40) |
(((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].time) << 48);
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, s_eobj_ownname, &errdes);
}
#if defined(EOCANDISCOVERY2_DIAGNOSTICS_SENDUPDETECTEDBOARDS)
else if(eobool_true == eo_common_hlfword_bitcheck(s_eo_thecandiscovery2.detection.replies[i], n))
{ // we dont have differences. send an info but only if the board has replied
errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_candiscovery_detectedboard);
errdes.sourcedevice = (eOcanport1 == i) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = 0;
errdes.par16 = (n & 0x000f);
errdes.par64 = ((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.firmware.build) |
((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.firmware.minor << 8) | ((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.firmware.major << 16) |
((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.protocol.minor << 24) | ((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.protocol.major << 32) |
((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].info.type << 40) |
(((uint64_t)s_eo_thecandiscovery2.detection.boards[i][n].time) << 48);
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, s_eobj_ownname, &errdes);
}
#endif
}
}
// 2. then we send info about OK or KO
if((eobool_true == allboardsfound) && (eobool_true == allboardsareok))
{
#if defined(EOCANDISCOVERY2_DIAGNOSTICS_SENDUPOKRESULT)
uint8_t numofboards = eo_common_hlfword_bitsetcount(s_eo_thecandiscovery2.target.canmap[0]) + eo_common_hlfword_bitsetcount(s_eo_thecandiscovery2.target.canmap[1]);
if(numofboards > 0)
{
eOerrmanErrorType_t errtype = (eobool_true == s_eo_thecandiscovery2.searchstatus.fakesearch) ? (eo_errortype_warning) : (eo_errortype_info);
errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_candiscovery_ok);
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
errdes.par16 = (numofboards & 0x00ff) | ((uint16_t)s_eo_thecandiscovery2.searchstatus.fakesearch << 12);
errdes.par64 = ((uint64_t)s_eo_thecandiscovery2.target.info.firmware.build) |
((uint64_t)s_eo_thecandiscovery2.target.info.firmware.minor << 8) | ((uint64_t)s_eo_thecandiscovery2.target.info.firmware.major << 16) |
((uint64_t)s_eo_thecandiscovery2.target.info.protocol.minor << 24) | ((uint64_t)s_eo_thecandiscovery2.target.info.protocol.major << 32) |
((uint64_t)s_eo_thecandiscovery2.target.info.type << 40) |
(((uint64_t)s_eo_thecandiscovery2.detection.duration) << 48);
eo_errman_Error(eo_errman_GetHandle(), errtype, NULL, s_eobj_ownname, &errdes);
}
#endif
}
else
{
if(eobool_false == allboardsfound)
{
// "CFG: CANdiscovery cannot find some boards. In p16: board type in 0xff00 and number of missing in 0x00ff. In p64: mask of missing addresses in 0x000000000000ffff"
for(i=eOcanport1; i<eOcanports_number; i++)
{
uint16_t maskofmissing = (~s_eo_thecandiscovery2.detection.replies[i]) & s_eo_thecandiscovery2.target.canmap[i];
if(0 != maskofmissing)
{
uint8_t numofmissing = eo_common_hlfword_bitsetcount(maskofmissing);
errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_candiscovery_boardsmissing);
errdes.sourcedevice = (eOcanport1 == i) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = 0;
errdes.par16 = ((uint16_t)s_eo_thecandiscovery2.target.info.type << 8) | (numofmissing & 0x00ff);
errdes.par64 = (uint64_t)maskofmissing | (((uint64_t)s_eo_thecandiscovery2.detection.duration) << 48);
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, s_eobj_ownname, &errdes);
}
}
}
if(eobool_false == allboardsareok)
{
// "CFG: CANdiscovery detected invalid boards. In p16: target board type in 0xff00 and number of invalid in 0x00ff. In p64: each nibble contains 0x0 if ok, mask 0x1 if wrong type, mask 0x2 if wrong fw, mask 0x4 if wrong prot"
for(i=eOcanport1; i<eOcanports_number; i++)
{
uint16_t maskofinvalid = (s_eo_thecandiscovery2.detection.incompatibilities[i]) & s_eo_thecandiscovery2.target.canmap[i];
if(0 != maskofinvalid)
{
uint8_t numofinvalid = eo_common_hlfword_bitsetcount(maskofinvalid);
uint64_t nibbles64 = 0;
for(n=0; n<15; n++)
{
if(eobool_true == eo_common_hlfword_bitcheck(maskofinvalid, n))
{
// ok, we have an invalid detected board. let us see what nibble to put
uint8_t nib = 0x0;
if(s_eo_thecandiscovery2.target.info.type != s_eo_thecandiscovery2.detection.boards[i][n].info.type)
{
nib |= 0x1;
}
if(eobool_true == s_eo_isFirmwareVersionToBeVerified(&s_eo_thecandiscovery2.target.info.firmware))
{ // signal error only if fw version is to be verified
if(eobool_false == s_eo_isFirmwareVersionCompatible(&s_eo_thecandiscovery2.target.info.firmware, &s_eo_thecandiscovery2.detection.boards[i][n].info.firmware))
{
nib |= 0x2;
}
}
if(eobool_true == s_eo_isProtocolVersionToBeVerified(&s_eo_thecandiscovery2.target.info.protocol))
{ // signal error ony if prot version is be verified
if(eobool_false == s_eo_isProtocolVersionCompatible(&s_eo_thecandiscovery2.target.info.protocol, &s_eo_thecandiscovery2.detection.boards[i][n].info.protocol))
{
nib |= 0x4;
}
}
nibbles64 |= ((uint64_t)nib << (4*n));
}
}
errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_candiscovery_boardsinvalid);
errdes.sourcedevice = (eOcanport1 == i) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
errdes.sourceaddress = 0;
errdes.par16 = ((uint16_t)s_eo_thecandiscovery2.target.info.type << 8) | (numofinvalid & 0x00ff);
errdes.par64 = nibbles64;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, s_eobj_ownname, &errdes);
}
}
}
}
}
extern void eoprot_fun_UPDT_mn_appl_cmmnds_go2state(const EOnv* nv, const eOropdescriptor_t* rd)
{
eOmn_appl_state_t *go2state = (eOmn_appl_state_t *)nv->ram;
eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status);
eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32);
eOresult_t res = eores_NOK_generic;
switch(*go2state)
{
case applstate_config:
{
res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STcfg);
res = res;
// the new currstate is set inside the on-entry of the state machine
//if(eores_OK == res)
//{
// status->currstate = applstate_config;
//}
} break;
case applstate_running:
{
uint32_t canBoardsReady = 0;
uint32_t canBoardsChecked = 0;
if(eobool_false == eo_appTheDB_areConnectedCanBoardsReady(eo_emsapplBody_GetDataBaseHandle(eo_emsapplBody_GetHandle()), &canBoardsReady, &canBoardsChecked))
{
//#warning marco.accame: put a dedicated diagnostics message with list of missing can boards
//snprintf(str, sizeof(str), "only 0x%x of of 0x%x.", canBoardsReady, canBoardsChecked);
// the new currstate is set inside the on-entry of the state machine
//status->currstate = applstate_error;
// it MUST NOT be fatal error because we want to give the ems time to find the boards ready
eOerrmanDescriptor_t errdes = {0};
errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_boards_missing);
errdes.par16 = 0;
errdes.par64 = (uint64_t)canBoardsReady | (((uint64_t)canBoardsChecked) << 32);
errdes.sourcedevice = eo_errman_sourcedevice_localboard;
errdes.sourceaddress = 0;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes);
{
#warning -> marco.accame: send the 4 messages with the searched and found on can1 and can2
}
return;
}
else
{
// marco.accame: if i send diagnostics messages just before going to running mode ... the application crashes. TO BE UNDERSTOOD WHY !
//eo_emsapplBody_SignalDetectedCANboards(eo_emsapplBody_GetHandle());
// // maybe in here we can put an info diagnostics message
// // send message about the ready boards
// uint8_t numcanboards = eo_appTheDB_GetNumberOfCanboards(eo_appTheDB_GetHandle());
// uint8_t i = 0;
// eOappTheDB_board_canlocation_t loc = {0};
// eObrd_cantype_t exptype = eobrd_cantype_unknown;
// eObrd_typeandversions_t detected = {0};
//
// eOerrmanDescriptor_t des = {0};
// des.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag07);
//
// for(i=0; i<numcanboards; i++)
// {
// if(eores_OK == eo_appTheDB_GetCanDetectedInfo(eo_appTheDB_GetHandle(), i, &loc, &exptype, &detected))
// {
// // fill the message. so far i use a debug with can-id-typedetected-typeexpectde
// des.sourcedevice = (eOcanport1 == loc.emscanport) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2);
// des.sourceaddress = loc.addr;
// des.param = (exptype << 8) | (detected.boardtype);
// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &des);
// }
// }
}
res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STrun);
// the new currstate is set inside the on-entry of the state machine
//if(eores_OK == res)
//{
// status->currstate = applstate_running;
//}
} break;
case applstate_error:
{
//I don't expect to receive go to error cmd
res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STerr);
// the new currstate is set inside the on-entry of the state machine
//if(eores_OK == res)
//{
// status->currstate = applstate_error;
//}
} break;
case applstate_resetmicro:
{
// i just reset the micro ... straight away
osal_system_scheduling_suspend();
ee_sharserv_sys_restart();
} break;
case applstate_restartapp:
{
osal_system_scheduling_suspend();
ee_sharserv_ipc_gotoproc_set(ee_procApplication);
ee_sharserv_sys_restart();
} break;
default:
{
} break;
}
}
extern eOresult_t eo_skin_Verify(EOtheSKIN *p, const eOmn_serv_configuration_t * servcfg, eOservice_onendofoperation_fun_t onverify, eObool_t activateafterverify)
{
if((NULL == p) || (NULL == servcfg))
{
s_eo_theskin.service.state = eomn_serv_state_failureofverify;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, s_eo_theskin.service.state);
if(NULL != onverify)
{
onverify(p, eobool_false);
}
return(eores_NOK_nullpointer);
}
if(eomn_serv_SK_skin != servcfg->type)
{
p->service.state = eomn_serv_state_failureofverify;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, p->service.state);
if(NULL != onverify)
{
onverify(p, eobool_false);
}
return(eores_NOK_generic);
}
if(eobool_true == p->service.active)
{
eo_skin_Deactivate(p);
}
p->service.state = eomn_serv_state_verifying;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, p->service.state);
// make sure the timer is not running
eo_timer_Stop(p->diagnostics.reportTimer);
p->service.onverify = onverify;
p->service.activateafterverify = activateafterverify;
p->sharedcan.discoverytarget.info.type = eobrd_cantype_mtb;
p->sharedcan.discoverytarget.info.protocol.major = servcfg->data.sk.skin.version.protocol.major;
p->sharedcan.discoverytarget.info.protocol.minor = servcfg->data.sk.skin.version.protocol.minor;
p->sharedcan.discoverytarget.info.firmware.major = servcfg->data.sk.skin.version.firmware.major;
p->sharedcan.discoverytarget.info.firmware.minor = servcfg->data.sk.skin.version.firmware.minor;
p->sharedcan.discoverytarget.info.firmware.build = servcfg->data.sk.skin.version.firmware.build;
// now i must do discovery of the patches. all patches can be at most on the two can buses ...
// moreover, we cannot have more than .... eo_skin_maxnumberofMTBboards boards
p->sharedcan.discoverytarget.canmap[eOcanport1] = p->sharedcan.discoverytarget.canmap[eOcanport2] = 0x0000;
uint8_t i=0;
for(i=0; i<servcfg->data.sk.skin.numofpatches; i++)
{
p->sharedcan.discoverytarget.canmap[eOcanport1] |= servcfg->data.sk.skin.canmapskin[i][eOcanport1];
p->sharedcan.discoverytarget.canmap[eOcanport2] |= servcfg->data.sk.skin.canmapskin[i][eOcanport2];
}
uint8_t numofboards = eo_common_hlfword_bitsetcount(p->sharedcan.discoverytarget.canmap[eOcanport1]) +
eo_common_hlfword_bitsetcount(p->sharedcan.discoverytarget.canmap[eOcanport2]);
if(numofboards > eo_skin_maxnumberofMTBboards)
{
p->diagnostics.errorDescriptor.sourcedevice = eo_errman_sourcedevice_localboard;
p->diagnostics.errorDescriptor.sourceaddress = 0;
p->diagnostics.errorDescriptor.par16 = (numofboards << 8) | (eo_skin_maxnumberofMTBboards & 0x00ff);
p->diagnostics.errorDescriptor.par64 = (p->sharedcan.discoverytarget.canmap[eOcanport2] << 16) | (p->sharedcan.discoverytarget.canmap[eOcanport1]);
EOaction_strg astrg = {0};
EOaction *act = (EOaction*)&astrg;
eo_action_SetCallback(act, s_eo_skin_send_periodic_error_report, p, eov_callbackman_GetTask(eov_callbackman_GetHandle()));
p->diagnostics.errorDescriptor.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_skin_failed_toomanyboards);
p->diagnostics.errorType = eo_errortype_error;
eo_errman_Error(eo_errman_GetHandle(), p->diagnostics.errorType, NULL, s_eobj_ownname, &p->diagnostics.errorDescriptor);
if(0 != p->diagnostics.reportPeriod)
{
p->diagnostics.errorCallbackCount = EOK_int08dummy;
eo_timer_Start(p->diagnostics.reportTimer, eok_abstimeNOW, p->diagnostics.reportPeriod, eo_tmrmode_FOREVER, act);
}
p->service.state = eomn_serv_state_failureofverify;
eo_service_hid_SynchServiceState(eo_services_GetHandle(), eomn_serv_category_skin, p->service.state);
if(NULL != onverify)
{
onverify(p, eobool_false);
}
return(eores_NOK_generic);
}
p->sharedcan.ondiscoverystop.function = s_eo_skin_onstop_search4mtbs;
p->sharedcan.ondiscoverystop.parameter = (void*)servcfg;
// start discovery
eo_candiscovery2_Start(eo_candiscovery2_GetHandle(), &p->sharedcan.discoverytarget, &p->sharedcan.ondiscoverystop);
return(eores_OK);
}
