static void s_eo_armenv_sharedservices_synchronise(void)
{
// eEresult_t res;
eEprocess_t proc = (eEprocess_t)s_the_armenv.modinfo->info.entity.signature;
if(ee_res_OK != ee_sharserv_part_proc_synchronise(proc, s_the_armenv.modinfo))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_sharedservices_synchronise(): cannot sync modinfo", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
}
// only the e-loader has a non-null brdinfo
if(NULL != s_the_armenv.brdinfo)
{
eEboardInfo_t boardinfo;
memcpy(&boardinfo, s_the_armenv.brdinfo, sizeof(eEboardInfo_t));
boardinfo.uniqueid = hal_uniqueid_id64bit_get();
if(ee_res_OK != ee_sharserv_info_boardinfo_synchronise(&boardinfo))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_sharedservices_synchronise(): cannot sync brdinfo", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
}
}
if(ee_res_OK != ee_sharserv_info_deviceinfo_get(&s_the_armenv.devinfo))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_sharedservices_synchronise(): cannot read devinfo", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
}
}
static void s_eo_armenv_prepare(const eEmoduleInfo_t *modinfo, const eEboardInfo_t *brdinfo)
{
eEprocess_t proc = (eEprocess_t)modinfo->info.entity.signature;
// eval modinfo
if((ee_entity_process == modinfo->info.entity.type) && (ee_procNone != proc))
{
s_the_armenv.modinfo = modinfo;
}
else
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_prepare(): wrong modinfo", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
}
// load brdinfo. it is non-NULL only in the loader
s_the_armenv.brdinfo = brdinfo;
s_the_armenv.eprocess = proc;
switch(proc)
{
case ee_procLoader:
{
s_the_armenv.codeprocoffset = (EENV_MEMMAP_ELOADER_ROMADDR-EENV_ROMSTART);
} break;
case ee_procUpdater:
{
s_the_armenv.codeprocoffset = (EENV_MEMMAP_EUPDATER_ROMADDR-EENV_ROMSTART);
} break;
case ee_procApplication:
{
s_the_armenv.codeprocoffset = (EENV_MEMMAP_EAPPLICATION_ROMADDR-EENV_ROMSTART);
} break;
default:
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_prepare(): unsupported proc", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
} break;
}
// moved the forceoffset EENV_EAPPLICATION_FORCE_CODE_OFFSET_TO_ZERO isnide the eEmemorymap.h file
// // however, if we have the following macro defined we want the e-proc to work at base address
//#ifdef EE_FORCE_CODE_PROC_OFFSET_TO_ZERO
// s_the_armenv.codeprocoffset = 0;
//#endif
}
extern EOtransceiver * eo_boardtransceiver_Initialise(const eOboardtransceiver_cfg_t *cfg)
{
eOtransceiver_cfg_t txrxcfg = eo_transceiver_cfg_default;
if(NULL != s_eo_theboardtrans.transceiver)
{
return(s_eo_theboardtrans.transceiver);
}
if(NULL == cfg)
{
cfg = &eo_boardtransceiver_cfg_default;
}
if(NULL == cfg->vectorof_endpoint_cfg)
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, s_eobj_ownname, "need a vector of endpoints");
}
if((0 == cfg->sizes.capacityoftxpacket) || (0 == cfg->sizes.capacityofrop) || (0 == cfg->sizes.capacityofropframeregulars) ||
(0 == cfg->sizes.capacityofropframeoccasionals) || (0 == cfg->sizes.capacityofropframereplies) || (0 == cfg->sizes.maxnumberofregularrops))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, s_eobj_ownname, "a cfg->sizes field is 0");
}
// 1. init the proper transceiver cfg
s_eo_theboardtrans.nvscfg = s_eo_boardtransceiver_nvscfg_get(cfg);
txrxcfg.capacityoftxpacket = cfg->sizes.capacityoftxpacket;
txrxcfg.capacityofrop = cfg->sizes.capacityofrop;
txrxcfg.capacityofropframeregulars = cfg->sizes.capacityofropframeregulars;
txrxcfg.capacityofropframeoccasionals = cfg->sizes.capacityofropframeoccasionals;
txrxcfg.capacityofropframereplies = cfg->sizes.capacityofropframereplies;
txrxcfg.maxnumberofregularrops = cfg->sizes.maxnumberofregularrops;
txrxcfg.remipv4addr = cfg->remotehostipv4addr;
txrxcfg.remipv4port = cfg->remotehostipv4port;
txrxcfg.nvscfg = s_eo_theboardtrans.nvscfg;
txrxcfg.mutex_fn_new = cfg->mutex_fn_new;
txrxcfg.protection = cfg->transprotection;
s_eo_theboardtrans.transceiver = eo_transceiver_New(&txrxcfg);
return(s_eo_theboardtrans.transceiver);
}
extern eOresult_t eo_skin_SendReport(EOtheSKIN *p)
{
if(NULL == p)
{
return(eores_NOK_nullpointer);
}
eo_errman_Error(eo_errman_GetHandle(), p->diagnostics.errorType, NULL, s_eobj_ownname, &p->diagnostics.errorDescriptor);
eOerror_value_t errorvalue = eoerror_code2value(p->diagnostics.errorDescriptor.code);
switch(errorvalue)
{
case eoerror_value_CFG_skin_failed_candiscovery:
{
eo_candiscovery2_SendLatestSearchResults(eo_candiscovery2_GetHandle());
} break;
default:
{
// dont send any additional info
} break;
}
return(eores_OK);
}
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 EOMtheEMSdiscoverylistener * eom_emsdiscoverylistener_Initialise(const eOemsdiscoverylistener_cfg_t *cfg)
{
if(NULL != s_emsdiscoverylistener_singleton.task)
{
return(&s_emsdiscoverylistener_singleton);
}
if(NULL == cfg)
{
cfg = &eom_emsdiscoverylistener_DefaultCfg;
}
memcpy(&s_emsdiscoverylistener_singleton.cfg, cfg, sizeof(eOemsdiscoverylistener_cfg_t));
if(NULL == eom_ipnet_GetHandle())
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "eom_emsdiscoverylistener_Initialise(): EOMtheIPnet not started yet", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
}
// create the socket
s_emsdiscoverylistener_singleton.socket = eo_socketdtg_New(cfg->inpdatagramnumber, cfg->inpdatagramsizeof, (eobool_true == cfg->usemutex) ? (eom_mutex_New()) : (NULL),
cfg->outdatagramnumber, cfg->outdatagramsizeof, (eobool_true == cfg->usemutex) ? (eom_mutex_New()) : (NULL)
);
// create the rx packet
s_emsdiscoverylistener_singleton.rxpkt = eo_packet_New(cfg->inpdatagramsizeof);
// initialise the transceiver
eOemsdiscoverytransceiver_cfg_t dtrcfg;
dtrcfg.hostipv4addr = cfg->remoteipaddr;
dtrcfg.hostipv4port = cfg->remoteport;
dtrcfg.txpktcapacity = cfg->outdatagramsizeof;
dtrcfg.discoveryprotocol = cfg->discoveryprotocol;
dtrcfg.dbgshutdowntime = 0; // if 0 the shutdown timer is not started. otherwise it forces a got to updater. 3*60*eok_reltime1sec;
eom_emsdiscoverytransceiver_Initialise(&dtrcfg);
// create the task
s_emsdiscoverylistener_singleton.task = eom_task_New(eom_mtask_EventDriven, cfg->taskpriority, cfg->taskstacksize,
s_eom_emsdiscoverylistener_task_startup, s_eom_emsdiscoverylistener_task_run,
(eOevent_t)0, eok_reltimeINFINITE, NULL,
tskEMScfg, "tskEMSlis");
return(&s_emsdiscoverylistener_singleton);
}
static void s_eo_armenv_sharedservices_init(void)
{
// eEresult_t res = ee_res_OK;
if(ee_res_OK == ee_sharserv_isvalid())
{
// init sharserv
sharserv_mode_t sharservmode =
{
.onerror = NULL,
.initmode = sharserv_base_initmode_forcestorageinit
};
if(ee_res_OK != ee_sharserv_init(&sharservmode))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_sharedservices_init(): cannot init sharserv", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
}
}
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;
}
}
}
}
// --------------------------------------------------------------------------------------------------------------------
// - definition of extern public functions
// --------------------------------------------------------------------------------------------------------------------
extern EOappTheSP* eo_appTheSP_Initialise(const eOappTheSP_cfg_t *cfg)
{
EOappTheSP *retptr = NULL;
if(NULL == cfg)
{
return(retptr);
}
retptr = &s_theSP;
//save in obj its configuration
memcpy(&(retptr->cfg), cfg, sizeof(eOappTheSP_cfg_t));
retptr->appRunMode = applrunMode__default;
s_eo_appTheSP_services_init(retptr);
retptr->st = eO_appTheSP_st__inited;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, s_eobj_ownname, "Services provider init OK");
return(retptr);
}
static eOresult_t s_eo_armenv_shareddata_ipnet_get(EOVtheEnvironmentDerived *p, const void **ipnet)
{
// ok, get back my type.
//EOVtheEnvironment *armenv = (EOVtheEnvironment *)p;
// #warning: we could remove teh check vs argument not NULL, as long as it is called correctly
// if(NULL == armenv)
// {
// return(eores_NOK_nullpointer);
// }
// it is a singleton boys ... thus i dont need using armenv s_the_armenv
if(ee_res_OK != ee_sharserv_info_deviceinfo_get(&s_the_armenv.devinfo))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "s_eo_armenv_shareddata_ipnet_get(): cannot read devinfo", s_eobj_ownname, &eo_errman_DescrRuntimeErrorLocal);
return(eores_NOK_generic);
}
*ipnet = &s_the_armenv.devinfo->ipnetwork;
return(eores_OK);
}
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);
}
extern eOresult_t eo_nvset_LoadEP(EOnvSet* p, eOprot_EPcfg_t *cfgofep, eObool_t initNVs)
{
eOnvset_brd_t* theBoard = NULL;
eOnvBRD_t brd = 0; // local or 0, 1, 2, etc.
eOnvset_ep_t *theEndpoint = NULL;
uint16_t epnvsnumberof = 0;
uint16_t sizeofram =0;
if((NULL == p) || (NULL == cfgofep))
{
return(eores_NOK_nullpointer);
}
theBoard = &p->theboard;
brd = p->theboard.boardnum;
theEndpoint = eo_mempool_New(eo_mempool_GetHandle(), 1*sizeof(eOnvset_ep_t));
memcpy(&theEndpoint->epcfg, cfgofep, sizeof(eOprot_EPcfg_t));
// ok, now in theEndpoint->epcfg.numberofsentities[] we have some ram. we use it to load the protocol.
eoprot_config_endpoint_entities(brd, theEndpoint->epcfg.endpoint, theEndpoint->epcfg.numberofentities);
// now it is ok to compute the size of the endpoint using the proper protocol function
sizeofram = eoprot_endpoint_sizeof_get(brd, theEndpoint->epcfg.endpoint);
// now that i have loaded the number of entities i verify if they are ok by checking the number of variables in the endpoint.
epnvsnumberof = eoprot_endpoint_numberofvariables_get(brd, cfgofep->endpoint);
if(0 == epnvsnumberof)
{
//#warning TBD: see how we continue in here ....
char str[64] = {0};
snprintf(str, sizeof(str), "EOnvSet: ep %d has 0 nvs", cfgofep->endpoint);
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, str, NULL, &eo_errman_DescrRuntimeErrorLocal);
eoprot_config_endpoint_entities(brd, theEndpoint->epcfg.endpoint, NULL);
eo_mempool_Delete(eo_mempool_GetHandle(), theEndpoint);
return(eores_NOK_generic);
}
theEndpoint->epnvsnumberof = epnvsnumberof;
theEndpoint->initted = eobool_false;
theEndpoint->epram = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_auto, sizeofram, 1);
theEndpoint->mtx_endpoint = (eo_nvset_protection_one_per_endpoint == p->protection) ? p->mtxderived_new() : NULL;
// now we must load the ram in the endpoint
eoprot_config_endpoint_ram(brd, theEndpoint->epcfg.endpoint, theEndpoint->epram, sizeofram);
// now add the vector of mtx if needed.
if(eo_nvset_protection_one_per_netvar == p->protection)
{
uint16_t i;
theEndpoint->themtxofthenvs = eo_vector_New(sizeof(EOVmutexDerived*), epnvsnumberof, NULL, 0, NULL, NULL);
for(i=0; i<epnvsnumberof; i++)
{
EOVmutexDerived* mtx = p->mtxderived_new();
eo_vector_PushBack(theEndpoint->themtxofthenvs, &mtx);
}
}
// now, i must update the mapping function from ep value to vector of endpoints
theBoard->ep2indexlut[theEndpoint->epcfg.endpoint] = eo_vector_Size(theBoard->theendpoints);
// and only now i push back the endpoint
eo_vector_PushBack(theBoard->theendpoints, &theEndpoint);
if(eobool_true == initNVs)
{
s_eo_nvset_NVsOfEP_Initialise(p, theEndpoint, theEndpoint->epcfg.endpoint);
}
return(eores_OK);
}
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);
// 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;
char str[60];
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
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, str, "eoprot_fun_UPDT_mn_appl_cmmnds_go2state", &eo_errman_DescrUnspecified);
return;
// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, str, "eoprot_fun_UPDT_mn_appl_cmmnds_go2state", &eo_errman_DescrUnspecified);
}
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;
default:
{
} break;
}
}
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);
}
}
}
}
}
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);
}
extern EOtransmitter* eo_transmitter_New(const eo_transmitter_cfg_t *cfg)
{
EOtransmitter *retptr = NULL;
if(NULL == cfg)
{
cfg = &eo_transmitter_cfg_default;
}
// i get the memory for the object
retptr = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOtransmitter), 1);
retptr->txpacket = eo_packet_New(cfg->capacityoftxpacket);
retptr->ropframereadytotx = eo_ropframe_New();
retptr->ropframeregulars = eo_ropframe_New();
retptr->ropframeoccasionals = eo_ropframe_New();
retptr->ropframereplies = eo_ropframe_New();
retptr->roptmp = eo_rop_New(cfg->capacityofrop);
retptr->nvscfg = cfg->nvscfg;
retptr->theagent = eo_agent_Initialise(NULL);
retptr->ipv4addr = cfg->ipv4addr;
retptr->ipv4port = cfg->ipv4port;
retptr->bufferropframeregulars = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, cfg->capacityofropframeregulars, 1);
retptr->bufferropframeoccasionals = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, cfg->capacityofropframeoccasionals, 1);
retptr->bufferropframereplies = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, cfg->capacityofropframereplies, 1);
retptr->listofregropinfo = (0 == cfg->maxnumberofregularrops) ? (NULL) : (eo_list_New(sizeof(eo_transm_regrop_info_t), cfg->maxnumberofregularrops, NULL, 0, NULL, NULL));
retptr->currenttime = 0;
retptr->tx_seqnum = 0;
eo_ropframe_Load(retptr->ropframeregulars, retptr->bufferropframeregulars, eo_ropframe_sizeforZEROrops, cfg->capacityofropframeregulars);
eo_ropframe_Clear(retptr->ropframeregulars);
eo_ropframe_Load(retptr->ropframeoccasionals, retptr->bufferropframeoccasionals, eo_ropframe_sizeforZEROrops, cfg->capacityofropframeoccasionals);
eo_ropframe_Clear(retptr->ropframeoccasionals);
eo_ropframe_Load(retptr->ropframereplies, retptr->bufferropframereplies, eo_ropframe_sizeforZEROrops, cfg->capacityofropframereplies);
eo_ropframe_Clear(retptr->ropframereplies);
{ // we set the content of ropframereadytotx with the same memory used by txpacket, so that when we operate on
// ropframereadytotx then we prepare the txpacket.
uint8_t *data;
uint16_t size;
uint16_t capacity;
eo_packet_Payload_Get(retptr->txpacket, &data, &size);
eo_packet_Capacity_Get(retptr->txpacket, &capacity);
eo_ropframe_Load(retptr->ropframereadytotx, data, eo_ropframe_sizeforZEROrops, capacity); // dont use size because size is now zero.
eo_ropframe_Clear(retptr->ropframereadytotx);
if(eobool_true != eo_ropframe_IsValid(retptr->ropframereadytotx))
{
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, s_eobj_ownname, "the ropframeready2tx is not valid... cannot continue");
}
// the destination ipv4addr and ipv4port are constant and are the ones passed through configuration
eo_packet_Addressing_Set(retptr->txpacket, retptr->ipv4addr, retptr->ipv4port);
}
if((NULL != cfg->mutex_fn_new) && (eo_transmitter_protection_total == cfg->protection))
{
retptr->mtx_replies = cfg->mutex_fn_new();
retptr->mtx_regulars = cfg->mutex_fn_new();
retptr->mtx_occasionals = cfg->mutex_fn_new();
}
else
{
retptr->mtx_replies = NULL;
retptr->mtx_regulars = NULL;
retptr->mtx_occasionals = NULL;
}
#if defined(USE_DEBUG_EOTRANSMITTER)
// DEBUG
retptr->debug.txropframeistoobigforthepacket = 0;
#endif
return(retptr);
}
extern void eom_emstransceiver_callback_incaseoferror_in_sequencenumberReceived(EOreceiver *receiver)
{ // the only reason of using the following two variables as static is: to reduce the use of stack.
static int64_t delta = 0;
static eOerrmanDescriptor_t errdes =
{
.code = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxseqnumber_error),
.sourcedevice = eo_errman_sourcedevice_localboard,
.sourceaddress = 0,
.par16 = 0,
.par64 = 0
};
const eOreceiver_seqnum_error_t *err = eo_receiver_GetSequenceNumberError(receiver);
if(NULL == err)
{
errdes.code = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_runtimeerror);
errdes.par64 = 0x123467fabc222;
errdes.par16 = 1;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, NULL, &errdes);
return;
}
if(1 == err->rec_seqnum)
{
// it is the first packet received from a remote transmitter freshly initted (i.e., robotInterface has just re-started bu this board was alive well before)
// thus, we dont issue an error but an info:
errdes.code = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxseqnumber_restarted);
errdes.par64 = err->exp_seqnum;
errdes.par16 = 1;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes);
// ok, now we must set back the error code as it is normal.
errdes.code = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxseqnumber_error);
}
else
{
delta = err->rec_seqnum - err->exp_seqnum;
if(delta > INT16_MAX) delta = INT16_MAX; //32767
else if(delta < INT16_MIN) delta = INT16_MIN; //-32768;
errdes.code = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxseqnumber_error);
errdes.par16 = (int16_t)delta;
errdes.par64 = err->exp_seqnum;
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes);
}
}
extern void eom_emstransceiver_callback_incaseoferror_invalidframe(EOreceiver *receiver)
{
static eOerrmanDescriptor_t errdesinvframe =
{
.code = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxinvalidframe_error),
.sourcedevice = eo_errman_sourcedevice_localboard,
.sourceaddress = 0,
.par16 = 0,
.par64 = 0
};
eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdesinvframe);
}
// --------------------------------------------------------------------------------------------------------------------
// - definition of extern hidden functions
// --------------------------------------------------------------------------------------------------------------------
// --------------------------------------------------------------------------------------------------------------------
// - definition of static functions
// --------------------------------------------------------------------------------------------------------------------
/** @fn static void s_eom_emsappl_main_init(void)
@brief It initialises the emsappl
@details bla bla bla.
**/
static void s_eom_emsappl_main_init(void)
{
// init leds via the EOtheLEDpulser object
eOledpulser_cfg_t ledpulsercfg =
{
.led_enable_mask = (1 << eo_ledpulser_led_zero | 1 << eo_ledpulser_led_one | 1 << eo_ledpulser_led_two | 1 << eo_ledpulser_led_three | 1 << eo_ledpulser_led_four | 1 << eo_ledpulser_led_five),
.led_init = (eOint8_fp_uint8_cvoidp_t) hal_led_init,
.led_on = (eOint8_fp_uint8_t) hal_led_on,
.led_off = (eOint8_fp_uint8_t) hal_led_off,
.led_toggle = (eOint8_fp_uint8_t) hal_led_toggle
};
eo_ledpulser_Initialise(&ledpulsercfg);
// init the ems application
EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle();
eom_emsappl_Initialise(&emscfg->applcfg);
// set the led reserved for link
if(eores_OK == eom_ipnet_ResolveIP(eom_ipnet_GetHandle(), emscfg->applcfg.hostipv4addr, 5*EOK_reltime100ms))
{
// set led0 to ON
eo_ledpulser_On(eo_ledpulser_GetHandle(), eo_ledpulser_led_zero);
}
else
{
eo_ledpulser_Off(eo_ledpulser_GetHandle(), eo_ledpulser_led_zero);
}
}
