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_eom_thecreation(void)
{
char str[96];
snprintf(str, sizeof(str), "OSAL::tskinit: just started");
eo_errman_Trace(eo_errman_GetHandle(), str, s_eobj_ownname);
snprintf(str, sizeof(str), "OSAL::tskinit: about to init EOMtheTimerManager + EOMtheCallbackManager");
eo_errman_Trace(eo_errman_GetHandle(), str, s_eobj_ownname);
// start the services offered by embobj: timer manager and callback manager
eom_timerman_Initialise(s_eom_system.tmrmancfg);
eom_callbackman_Initialise(s_eom_system.cbkmancfg);
snprintf(str, sizeof(str), "OSAL::tskinit: calling user-defined function");
eo_errman_Trace(eo_errman_GetHandle(), str, s_eobj_ownname);
// run user defined initialisation ...
if(NULL != s_eom_system.user_init_fn)
{
s_eom_system.user_init_fn();
}
snprintf(str, sizeof(str), "OSAL::tskinit: has finished user-defined function");
eo_errman_Trace(eo_errman_GetHandle(), str, s_eobj_ownname);
snprintf(str, sizeof(str), "OSAL::tskinit: about to stop itself and start scheduling");
eo_errman_Trace(eo_errman_GetHandle(), str, s_eobj_ownname);
}
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);
}
}
}
extern eOresult_t eov_mutex_hid_SetVTABLE(EOVmutex *p, eOres_fp_voidp_uint32_t v_take, eOres_fp_voidp_t v_release, eOres_fp_voidp_t v_delete)
{
eo_errman_Assert(eo_errman_GetHandle(), (NULL != v_take), "eov_mutex_hid_SetVTABLE(): NULL v_take", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
eo_errman_Assert(eo_errman_GetHandle(), (NULL != v_release), "eov_mutex_hid_SetVTABLE(): NULL v_release", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
eo_errman_Assert(eo_errman_GetHandle(), (NULL != v_delete), "eov_mutex_hid_SetVTABLE(): NULL v_delete", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
p->vtable[VF00_take] = (void*) v_take;
p->vtable[VF01_release] = (void*) v_release;
p->vtable[VF02_delete] = (void*) v_delete;
return(eores_OK);
}
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);
}
}
}
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 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);
}
}
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);
}
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);
}
extern EOVtheCallbackManager * eov_callbackman_hid_Initialise(eOres_fp_vcbkmanp_cbk_voidp_uint32_t execute_fn, EOVtaskDerived *task)
{
if(NULL != s_eov_callbackmanager.vtable[0])
{
// already initialised
return(&s_eov_callbackmanager);
}
eo_errman_Assert(eo_errman_GetHandle(), NULL != execute_fn, "eov_callbackman_hid_Initialise(): NULL execute_fn", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
eo_errman_Assert(eo_errman_GetHandle(), NULL != task, "eov_callbackman_hid_Initialise(): NULL task", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
s_eov_callbackmanager.vtable[VF00_execute] = execute_fn;
s_eov_callbackmanager.task = task;
return(&s_eov_callbackmanager);
}
// 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);
}
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 EOfifo * eo_fifo_New(eOsizeitem_t item_size, eOsizecntnr_t capacity,
eOres_fp_voidp_uint32_t item_init, uint32_t init_arg,
eOres_fp_voidp_voidp_t item_copy, eOres_fp_voidp_t item_clear,
EOVmutexDerived *mutex)
{
EOfifo *retptr = NULL;
// i get memory for a mutexfifo. it will never be null
retptr = (EOfifo*) eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOfifo), 1);
eo_errman_Assert(eo_errman_GetHandle(), (0 != item_size), "eo_fifo_New(): 0 item_size", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
eo_errman_Assert(eo_errman_GetHandle(), (0 != capacity), "eo_fifo_New(): 0 capacity", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
// now i fill the mutexfifo with a deque
retptr->dek = eo_deque_New(item_size, capacity, item_init, init_arg, item_copy, item_clear);
// now i copy the passed mutex into mutexfifo. beware for future use, ... it may be NULL
retptr->mutex = mutex;
// ok, done
return(retptr);
}
static void s_eom_emsdiscoverylistener_task_startup(EOMtask *p, uint32_t t)
{
// open the socket
EOaction_strg astg = {0};
EOaction *onrx = (EOaction*)&astg;
eo_action_SetEvent(onrx, emsdiscoverylistener_evt_packet_received, p);
eo_socketdtg_Open(s_emsdiscoverylistener_singleton.socket, s_emsdiscoverylistener_singleton.cfg.localport, eo_sktdir_TXRX, eobool_false,
NULL, onrx, NULL);
eo_errman_Trace(eo_errman_GetHandle(), "called _task_startup()", s_eobj_ownname);
}
extern EOMtheTimerManager * eom_timerman_Initialise(const eOmtimerman_cfg_t *tmrmancfg)
{
if(NULL != s_eom_thetimermanager.tmrman)
{
// already initialised
return(&s_eom_thetimermanager);
}
if(NULL == tmrmancfg)
{
tmrmancfg = &eom_timerman_DefaultCfg;
}
// trying to initialise with wrong params error
eo_errman_Assert(eo_errman_GetHandle(), (0 != tmrmancfg->messagequeuesize), "eom_timerman_Initialise(): 0 messagequeuesize", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
eo_errman_Assert(eo_errman_GetHandle(), (0 != tmrmancfg->stacksize), "eom_timerman_Initialise(): 0 stacksize", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
eo_errman_Assert(eo_errman_GetHandle(), (0 != tmrmancfg->priority), "eom_timerman_Initialise(): 0 priority", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
// i get a basic timer manager with onnew, add and rem functions proper for osal. and an EOMmutex (by the way ... eom_mutex_New() never returns NULL).
s_eom_thetimermanager.tmrman = eov_timerman_hid_Initialise(s_eom_timerman_OnNewTimer, s_eom_timerman_OnDelTimer, s_eom_timerman_AddTimer, s_eom_timerman_RemTimer, eom_mutex_New());
// i prepare the task able to process actions associated to expiry of the timers
s_eom_thetimermanager.tskproc = eom_task_New(eom_mtask_MessageDriven, // type
tmrmancfg->priority, // priority
tmrmancfg->stacksize, // stacksize
NULL, // startup_fn
s_eom_timerman_tskproc_forever, // run_fn
tmrmancfg->messagequeuesize, // maxmessages: msg fifo can hold all timers
eok_reltimeINFINITE, // timeoutorperiod
NULL,
sys_timerman, // nameofthetask_fn
"timerman");
return(&s_eom_thetimermanager);
}
extern EOsm * eo_sm_New(const eOsm_cfg_t * cfg)
{
EOsm *retptr = NULL;
// verify that we have a non NULL cfg
eo_errman_Assert(eo_errman_GetHandle(), (NULL != cfg), "eo_sm_New(): NULL cfg", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
// i get the memory for the object. no need to check versus NULL because the memory pool already does it
retptr = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOsm), 1);
// now the obj has valid memory. i need to initialise it with other user-defined data, by means of cfg.
s_eo_sm_Specialise(retptr, cfg);
return(retptr);
}
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);
}
extern EOtheARMenvironment * eo_armenv_Initialise(const eEmoduleInfo_t *modinfo, const eEboardInfo_t *brdinfo)
{
if(NULL == s_the_armenv.env)
{
// allow initialisation with null cfg ..... but not without the moduleinfo
eo_errman_Assert(eo_errman_GetHandle(), (NULL != modinfo), "eo_armenv_Initialise(): NULL modinfo", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
s_eo_armenv_prepare(modinfo, brdinfo);
// i initialise the base environment
s_the_armenv.env = eov_env_hid_Initialise(s_eo_armenv_shareddata_sync, s_eo_armenv_code_proc_offset_get,
s_eo_armenv_shareddata_ipnet_get,
s_eo_armenv_shareddata_cannets_get,
s_eo_armenv_eproc_get);
// #warning --> acemor says: we make sure that the oui is not the infamous 0x112233 but ...
//
//
// shalinfo_deviceinfo_t deviceinfo;
// const uint64_t thenewoui = 0x0B9A70; //0xabcdef; 0x332211;
//
// if(ee_res_OK != shalinfo_deviceinfo_get(&s_the_armenv.devinfo))
// {
// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_fatal, "cannot read deviceinfo", s_eobj_ownname, NULL);
// //return(eores_NOK_generic);
// }
//
// if(thenewoui != (0x0000000000ffffff & s_the_armenv.devinfo->ipnetwork.macaddress))
// {
// memcpy(&deviceinfo, s_the_armenv.devinfo, sizeof(shalinfo_deviceinfo_t));
//
// deviceinfo.ipnetwork.macaddress = 0x0000FFFFFF000000 & s_the_armenv.devinfo->ipnetwork.macaddress;
// deviceinfo.ipnetwork.macaddress |= thenewoui;
//
// shalinfo_deviceinfo_set(&deviceinfo);
// }
}
return(&s_the_armenv);
}
extern EOfifoWord* eo_fifoword_New(eOsizecntnr_t capacity, EOVmutexDerived *mutex)
{
EOfifoWord *retptr = NULL;
// i get memory for a fifoword. it can never be NULL
retptr = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOfifoWord), 1);
eo_errman_Assert(eo_errman_GetHandle(), (0 != capacity), "eo_fifoword_New(): 0 capacity", s_eobj_ownname, &eo_errman_DescrWrongParamLocal);
// now i create a mutexfifo made of word (no ctor or dtor!) and i fill it into the fifo of
// the fifoword. it can never be NULL
retptr->fifo = eo_fifo_New(4, capacity, NULL, 0, NULL, NULL, mutex);
// ok, done
return(retptr);
}
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);
}
}
extern EOfakeStorage* eo_fakestrg_New(uint32_t id, uint32_t capacity, const void *defvalue, EOVmutexDerived *mtx)
{
EOfakeStorage *retptr = NULL;
eov_strg_memmap_t *strgmem = NULL;
// i get the memory for the object
retptr = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOfakeStorage), 1);
eo_errman_Assert(eo_errman_GetHandle(), (0 != capacity), s_eobj_ownname, "capacity cannot be zero");
// get the base object
retptr->storage = eov_strg_hid_New(id, capacity, defvalue, mtx);
// pass to it the virtual table
eov_strg_hid_SetVTABLE(retptr->storage,
(eOres_fp_voidp_uint32_uint32_cvoidp_t) s_eo_fakestrg_set,
(eOres_fp_voidp_uint32_uint32_voidp_t) s_eo_fakestrg_get);
// now initialise the derived object
strgmem = s_eo_fakestrg_get_storagememory(retptr->storage);
if((NULL == strgmem) || (id != strgmem->id))
{
// create new memory partition, mark it with id and capacity, set its content to defvalue
strgmem = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(uint32_t) + sizeof(uint32_t) + capacity, 1);
strgmem->id = id;
strgmem->capacity = capacity;
memcpy(strgmem->data, defvalue, strgmem->capacity);
}
// assign the partition to the object
retptr->ram = (void*)strgmem;
return(retptr);
}
extern EOYmutex* eoy_mutex_New(void)
{
EOYmutex *retptr = NULL;
// i get the memory for the yarp mutex object
retptr = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOYmutex), 1);
// i get the base mutex
retptr->mutex = eov_mutex_hid_New();
// init its vtable
eov_mutex_hid_SetVTABLE(retptr->mutex, s_eoy_mutex_take, s_eoy_mutex_release, s_eoy_mutex_delete);
// i get a new yarp mutex
retptr->acemutex = ace_mutex_new();
// need to check because yarp may return NULL
eo_errman_Assert(eo_errman_GetHandle(), (NULL != retptr->acemutex), s_eobj_ownname, "eoy_mutex_New(): ace cannot give a mutex", &eo_errman_DescrRuntimeErrorLocal);
return(retptr);
}
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 EOconstLookupTbl * eo_constLookupTbl_New(eOsizecntnr_t capacity, uint16_t offset , eOres_fp_voidp_t exceptionMngFn, const void* itemsList)
{
EOconstLookupTbl *retptr = NULL;
if(NULL == itemsList)
{
return(retptr);
}
// i get the memory for the object. no need to check versus NULL because the memory pool already does it
retptr = eo_mempool_GetMemory(eo_mempool_GetHandle(), eo_mempool_align_32bit, sizeof(EOconstLookupTbl), 1);
// now the obj has valid memory. i need to initialise it with user-defined data,
eo_errman_Assert(eo_errman_GetHandle(), (0 != capacity), s_eobj_ownname, "capacity is zero");
retptr->capacity = capacity;
retptr->offset = offset;
retptr->exceptionMngFn = exceptionMngFn;
retptr->itemsList = itemsList;
return(retptr);
}
extern void eom_emsapplcfg_hid_userdef_OnError(eOerrmanErrorType_t errtype, const char *info, eOerrmanCaller_t *caller, const eOerrmanDescriptor_t *des)
{
const char empty[] = "EO?";
const char *err = eo_errman_ErrorStringGet(eo_errman_GetHandle(), errtype);
char str[128];
EOMtheEMSapplCfg *emsapplcfg = eom_emsapplcfg_GetHandle();
const char *eobjstr = (NULL != caller) ? (caller->eobjstr) : (empty);
uint32_t taskid = (NULL != caller) ? (caller->taskid) : (0);
if(emsapplcfg->errmng_haltrace_enabled)
{
snprintf(str, sizeof(str), "[%s] %s in tsk %d: %s", err, eobjstr, taskid, info);
hal_trace_puts(str);
}
if(errtype <= eo_errortype_error)
{
return;
}
eov_sys_Stop(eov_sys_GetHandle());
// init leds
overridden_appl_led_error_init();
// compute the mask of led to be toggled.
uint8_t ledmask = 0xff;
if((NULL != caller) && (NULL != caller->eobjstr))
{
if(0 == strcmp(caller->eobjstr, "HAL"))
{
hl_bits_byte_bitclear(&ledmask, 0);
}
else if(0 == strcmp(caller->eobjstr, "OSAL"))
{
hl_bits_byte_bitclear(&ledmask, 1);
}
else if(0 == strcmp(caller->eobjstr, "IPAL"))
{
hl_bits_byte_bitclear(&ledmask, 2);
}
}
for(;;)
{
hal_sys_delay(100);
if(hl_true == hl_bits_byte_bitcheck(ledmask, 0))
{
hal_led_toggle(hal_led0);
}
if(hl_true == hl_bits_byte_bitcheck(ledmask, 1))
{
hal_led_toggle(hal_led1);
}
if(hl_true == hl_bits_byte_bitcheck(ledmask, 2))
{
hal_led_toggle(hal_led2);
}
hal_led_toggle(hal_led3);
hal_led_toggle(hal_led4);
hal_led_toggle(hal_led5);
}
}
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_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;
}
}
}
}
