extern void eom_emsappl_hid_userdef_on_entry_ERR(EOMtheEMSappl* p)
{
// pulse led3 forever at 4 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/4, 0);
eo_appCanSP_SetRunMode(eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()), eo_appCanSP_runMode__onEvent);
}
extern void eom_emsappl_hid_userdef_on_entry_RUN(EOMtheEMSappl* p)
{
eOresult_t res = eores_NOK_generic;
// enable joints
res = eo_emsapplBody_EnableTxAllJointOnCan(eo_emsapplBody_GetHandle());
if (eores_NOK_generic == res)
{
//handle the error
}
// enable the tx mode of strain, if present and as configured
eo_strain_SendTXmode(eo_strain_GetHandle());
// pulse led3 forever at 1 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/1, 0);
// set mode on demand. then tx all canframes remained in the tx queue
eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_ondemand);
eo_canserv_TXstart(eo_canserv_GetHandle(), eOcanport1, NULL);
eo_canserv_TXstart(eo_canserv_GetHandle(), eOcanport2, NULL);
eo_canserv_TXwaituntildone(eo_canserv_GetHandle(), eOcanport1, 5*eok_reltime1ms);
eo_canserv_TXwaituntildone(eo_canserv_GetHandle(), eOcanport2, 5*eok_reltime1ms);
// Start reading the encoders
eo_appEncReader_StartRead(eo_emsapplBody_GetEncoderReader(eo_emsapplBody_GetHandle()));
}
extern void eom_emsappl_hid_userdef_on_entry_ERR(EOMtheEMSappl* p)
{
// pulse led3 forever at 4 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/4, 0);
eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight);
const uint8_t maxframes2read = 255; // 255 is the max number possible. the function however exits when all canframes are
eo_canserv_Parse(eo_canserv_GetHandle(), eOcanport1, maxframes2read, NULL);
eo_canserv_Parse(eo_canserv_GetHandle(), eOcanport2, maxframes2read, NULL);
}
// it is called by the init task before scheduling is enabled and before we activate the CFG-RUN-ERR state machine
extern void eom_emsappl_hid_userdef_initialise(EOMtheEMSappl* p)
{
// pulse led3 forever at 20 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/20, 0);
// init 5v
hal_5v_init(NULL);
hal_5v_on();
// do whatever is needed to start services.
s_overridden_appl_initialise_services();
}
extern void eom_emsappl_hid_userdef_on_entry_CFG(EOMtheEMSappl* p)
{
// pulse led3 forever at 0.50 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, 2*EOK_reltime1sec, 0);
// set the EOtheCANservice to be on event and force parsing of all packets in the RX queues.
eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight);
const uint8_t maxframes2read = 255; // 255 is the max number possible. the function however exits when all canframes are
eo_canserv_Parse(eo_canserv_GetHandle(), eOcanport1, maxframes2read, NULL);
eo_canserv_Parse(eo_canserv_GetHandle(), eOcanport2, maxframes2read, NULL);
}
extern void eom_emsappl_hid_userdef_on_entry_CFG(EOMtheEMSappl* p)
{
// pulse led3 forever at 0.50 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, 2*EOK_reltime1sec, 0);
EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle());
eo_appCanSP_SetRunMode(appCanSP_ptr, eo_appCanSP_runMode__onEvent);
eo_appCanSP_EmptyCanInputQueue(appCanSP_ptr, eOcanport1);
eo_appCanSP_EmptyCanInputQueue(appCanSP_ptr, eOcanport2);
}
extern void eom_emsappl_hid_userdef_initialise(EOMtheEMSappl* p)
{
EOMtheEMSapplCfg* emsapplcfg = eom_emsapplcfg_GetHandle();
// the led-pulser is initted as first thing
// pulse led3 forever at 20 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/20, 0);
{
// marco.accame on 24 apr 2015: here is how to customise the eth protocol from a generic to a specific board
// so far, we can keep it in here. but further on we shall customise one endpoint at a time in runtime.
EOnvSet* nvset = eom_emstransceiver_GetNVset(eom_emstransceiver_GetHandle());
uint8_t i = 0;
// 1. set the board number. the value of the generic board is 99.
// the correct value is used only for retrieving it later on and perform specific actions based on the board number
eo_nvset_BRDlocalsetnumber(nvset, s_boardnum);
// 2. load all the endpoints specific to this board. the generic board loads only management
for(i=0; i<s_theEPcfgsOthers_NumberOf; i++)
{
eOprot_EPcfg_t* epcfg = (eOprot_EPcfg_t*) &s_theEPcfgsOthers[i];
if(eobool_true == eoprot_EPcfg_isvalid(epcfg))
{
eo_nvset_LoadEP(nvset, epcfg, eobool_true);
}
}
// now we must define the .... proxy rules
// if we have board number equal to 1 or 3 ... (eb2 or eb4) then we set it for mc only
// in teh future we can set this proxy mode on teh basis of the board number received
eOprotBRD_t localboard = eoprot_board_local_get();
if((1 == localboard) || (3 == localboard))
{
eoprot_config_proxied_variables(eoprot_board_localboard, eoprot_endpoint_motioncontrol, eoprot_b02_b04_mc_isproxied);
}
}
// initialise diagnostics
eo_theEMSdgn_Initialize();
// start the application body
//const eOemsapplbody_cfg_t *applbodycfg = &theemsapplbodycfg;
const eOemsapplbody_cfg_t * applbodycfg = (const eOemsapplbody_cfg_t *)emsapplcfg->applbodycfg->thetrueconfig;
eo_emsapplBody_Initialise(applbodycfg);
}
extern void eom_emsappl_hid_userdef_on_entry_RUN(EOMtheEMSappl* p)
{
// pulse led3 forever at 1 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/1, 0);
// EOtheCANservice: set on demand mode. then tx all canframes remained in the tx queues
eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_ondemand);
eo_canserv_TXstartAll(eo_canserv_GetHandle(), NULL, NULL);
eo_canserv_TXwaitAllUntilDone(eo_canserv_GetHandle(), 5*eok_reltime1ms);
// tell the ethmonitor to alert no task, because the runner will tick it now at every cycle
// eo_ethmonitor_SetAlert(eo_ethmonitor_GetHandle(), NULL, 0);
// we dont start services.
}
extern void eom_emsappl_hid_userdef_on_entry_CFG(EOMtheEMSappl* p)
{
// pulse led3 forever at 0.50 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, 2*EOK_reltime1sec, 0);
// EOtheCANservice: set straight mode and force parsing of all packets in the RX queues.
eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight);
eo_canserv_ParseAll(eo_canserv_GetHandle());
// tell the ethmonitor to alert the task of the configurator
// eo_ethmonitor_SetAlert(eo_ethmonitor_GetHandle(), eom_emsconfigurator_GetTask(eom_emsconfigurator_GetHandle()), emsconfigurator_evt_userdef02);
// prefer sending a tx request just in case. because cfg state transmit only if requested an we dont want to have missed a previous request.
eom_task_SetEvent(eom_emsconfigurator_GetTask(eom_emsconfigurator_GetHandle()), emsconfigurator_evt_ropframeTx);
}
extern void eom_emsappl_hid_userdef_initialise(EOMtheEMSappl* p)
{
EOMtheEMSapplCfg* emsapplcfg = eom_emsapplcfg_GetHandle();
// the led-pulser is initted as first thing
// pulse led3 forever at 20 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/20, 0);
// initialise diagnostics
eo_theEMSdgn_Initialize();
// start the application body
//const eOemsapplbody_cfg_t *applbodycfg = &theemsapplbodycfg;
const eOemsapplbody_cfg_t * applbodycfg = (const eOemsapplbody_cfg_t *)emsapplcfg->applbodycfg->thetrueconfig;
eo_emsapplBody_Initialise(applbodycfg);
}
extern void eom_emsappl_hid_userdef_on_entry_ERR(EOMtheEMSappl* p)
{
// pulse led3 forever at 4 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/4, 0);
// EOtheCANservice: set straight mode and force parsing of all packets in the RX queues.
eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight);
eo_canserv_ParseAll(eo_canserv_GetHandle());
// stop services which may have been started
// motion-control
eo_motioncontrol_Stop(eo_motioncontrol_GetHandle());
eo_motioncontrol_Deactivate(eo_motioncontrol_GetHandle());
// stop tx activity of services that may have been started by callback function
// strain... no strain in here
// eo_strain_Stop(eo_strain_GetHandle());
// eo_strain_Deactivate(eo_strain_GetHandle());
// skin
eo_skin_Stop(eo_skin_GetHandle());
eo_skin_Deactivate(eo_skin_GetHandle());
// mais
eo_mais_Stop(eo_mais_GetHandle());
eo_mais_Deactivate(eo_mais_GetHandle());
// inertials
eo_inertials2_Stop(eo_inertials2_GetHandle());
eo_inertials2_Deactivate(eo_inertials2_GetHandle());
// inertials3
eo_inertials3_Stop(eo_inertials3_GetHandle());
eo_inertials3_Deactivate(eo_inertials3_GetHandle());
// temperatures
eo_temperatures_Stop(eo_temperatures_GetHandle());
eo_temperatures_Deactivate(eo_temperatures_GetHandle());
}
extern void eom_emsappl_hid_userdef_on_entry_RUN(EOMtheEMSappl* p)
{
eOresult_t res = eores_NOK_generic;
res = eo_emsapplBody_EnableTxAllJointOnCan(eo_emsapplBody_GetHandle());
if (eores_NOK_generic == res)
{
//handle the error
}
// pulse led3 forever at 1 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/1, 0);
EOappCanSP *appCanSP_ptr = eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle());
// Before changing appCanRunMode it is important to be sure that can output queues are empty
eo_appCanSP_EmptyCanOutputQueue(appCanSP_ptr, eOcanport1);
eo_appCanSP_EmptyCanOutputQueue(appCanSP_ptr, eOcanport2);
eo_appCanSP_SetRunMode(eo_emsapplBody_GetCanServiceHandle(eo_emsapplBody_GetHandle()), eo_appCanSP_runMode__onDemand);
// Start reading the encoders
eo_appEncReader_StartRead(eo_emsapplBody_GetEncoderReaderHandle(eo_emsapplBody_GetHandle()));
}
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);
}
}
extern void eom_emsappl_hid_userdef_initialise(EOMtheEMSappl* p)
{
EOMtheEMSapplCfg* emsapplcfg = eom_emsapplcfg_GetHandle();
// the led-pulser is initted as first thing
// pulse led3 forever at 20 hz.
eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/20, 0);
{ // board number is from IP address
s_boardnum = 0;
eOipv4addr_t ipaddress = eom_ipnet_GetIPaddress(eom_ipnet_GetHandle());
s_boardnum = ipaddress >> 24;
s_boardnum --;
if(s_boardnum > 16)
{
//return;
s_boardnum = 0;
}
//s_boardnum = 0; //it imposes that the board is the eb1
}
{ // CAN-MAPPING
// marco.accame on 19 may 2015: here we load the map of can ... and also we map the entities into some can boards
// ... if we have any.
EOtheCANmapping * canmap = eo_canmap_Initialise(NULL);
// now i load the map of can boards
EOconstvector *canboards = eoboardconfig_code2canboards(s_boardnum);
eo_canmap_LoadBoards(canmap, canboards);
// now i load mc-joints, mc-motors, as-strain, as-mais, sk-skin
EOconstvector *entitydes = NULL;
// mc
entitydes = eoboardconfig_code2entitydescriptors(s_boardnum, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint);
eo_canmap_ConfigEntity(canmap, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, entitydes);
entitydes = eoboardconfig_code2entitydescriptors(s_boardnum, eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor);
eo_canmap_ConfigEntity(canmap, eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, entitydes);
// as
entitydes = eoboardconfig_code2entitydescriptors(s_boardnum, eoprot_endpoint_analogsensors, eoprot_entity_as_strain);
eo_canmap_ConfigEntity(canmap, eoprot_endpoint_analogsensors, eoprot_entity_as_strain, entitydes);
entitydes = eoboardconfig_code2entitydescriptors(s_boardnum, eoprot_endpoint_analogsensors, eoprot_entity_as_mais);
eo_canmap_ConfigEntity(canmap, eoprot_endpoint_analogsensors, eoprot_entity_as_mais, entitydes);
// sk
entitydes = eoboardconfig_code2entitydescriptors(s_boardnum, eoprot_endpoint_skin, eoprot_entity_sk_skin);
eo_canmap_ConfigEntity(canmap, eoprot_endpoint_skin, eoprot_entity_sk_skin, entitydes);
}
{ // CAN-PROTOCOL
EOtheCANprotocol * canprot = eo_canprot_Initialise(NULL);
}
{ // ETH-PROTOCOL
// marco.accame on 24 apr 2015: here is how to customise the eth protocol from a generic to a specific board
// so far, we can keep it in here. but further on we shall customise one endpoint at a time in runtime.
EOnvSet* nvset = eom_emstransceiver_GetNVset(eom_emstransceiver_GetHandle());
uint8_t i = 0;
// 1. set the board number. the value of the generic board is 99.
// the correct value is used only for retrieving it later on and perform specific actions based on the board number
eo_nvset_BRDlocalsetnumber(nvset, s_boardnum);
EOconstvector* epcfg_cvector = eoboardconfig_code2EPcfg(s_boardnum);
// 2. load all the endpoints specific to this board. the generic board loads only management
uint16_t numofepcfgs = eo_constvector_Size(epcfg_cvector);
for(i=0; i<numofepcfgs; i++)
{
eOprot_EPcfg_t* epcfg = (eOprot_EPcfg_t*) eo_constvector_At(epcfg_cvector, i);
if(eobool_true == eoprot_EPcfg_isvalid(epcfg))
{
eo_nvset_LoadEP(nvset, epcfg, eobool_true);
}
}
// now we must define the .... proxy rules
// if we have board number equal to 1 or 3 ... (eb2 or eb4) then we set it for mc only
eOprotBRD_t localboard = eoprot_board_local_get();
if((1 == localboard) || (3 == localboard))
{
eoprot_config_proxied_variables(eoprot_board_localboard, eoprot_endpoint_motioncontrol, eoprot_b02_b04_mc_isproxied);
}
}
// start the application body
eOemsapplbody_cfg_t applbodyconfig;
memcpy(&applbodyconfig, &theemsapplbodycfg, sizeof(eOemsapplbody_cfg_t));
// now i get the encoder config
const eOappEncReader_cfg_t *enccfg = eoboardconfig_code2encoderconfig(s_boardnum);
memcpy(&applbodyconfig.encoderreaderconfig, enccfg, sizeof(eOappEncReader_cfg_t));
eo_emsapplBody_Initialise(&applbodyconfig);
}
