extern void eoprot_fun_UPDT_mc_joint_status(const EOnv* nv, const eOropdescriptor_t* rd)
{
#if defined(ENABLE_DEBUG_CONTROLMODESTATUS)
eOmc_joint_status_t *js = (eOmc_joint_status_t*)rd->data;
s_debug_monitor_controlmodestatus(eo_nv_GetBRD(nv), eoprot_ID2index(rd->id32), js->basic.controlmodestatus);
#endif
feat_manage_motioncontrol_data(nvBoardNum2FeatIdBoardNum(eo_nv_GetBRD(nv)), rd->id32, (void *)rd->data);
}
extern void eoprot_fun_ONSAY_mn(const EOnv* nv, const eOropdescriptor_t* rd)
{
// marco.accame on 18 mar 2014: this function is called when a say<id32, data> rop is received
// and the id32 is about the management endpoint. this function is common to every board.
// it is used this function and not another one because inside the hostTransceiver object it was called:
// eoprot_config_onsay_endpoint_set(eoprot_endpoint_management, eoprot_fun_ONSAY_mn);
// the aim of this function is to wake up a thread which is blocked because it has sent an ask<id32>
// the wake up funtionality is implemented in one mode only:
// a. in initialisation, someone sets some values and then reads them back.
// the read back sends an ask<id32, signature=0xaa000000>. in such a case the board sends back
// a say<id32, data, signature = 0xaa000000>. thus, if the received signature is 0xaa000000, then
// we must unblock using feat_signal_network_reply().
if(0xaa000000 == rd->signature)
{ // case a:
if(fakestdbool_false == feat_signal_network_reply(eo_nv_GetBRD(nv), rd->id32, rd->signature))
{
char str[256] = {0};
char nvinfo[128];
eoprot_ID2information(rd->id32, nvinfo, sizeof(nvinfo));
snprintf(str, sizeof(str), "eoprot_fun_ONSAY_mn() received an unexpected message w/ 0xaa000000 signature for %s", nvinfo);
embObjPrintWarning(str);
return;
}
}
}
extern void eoprot_fun_ONSAY_mc(const EOnv* nv, const eOropdescriptor_t* rd)
{
// marco.accame on 18 mar 2014: this function is called when a say<id32, data> rop is received
// and the id32 is about the motion control endpoint. this function is common to every board.
// it is used this function and not another one because inside the hostTransceiver object it was called:
// eoprot_config_onsay_endpoint_set(eoprot_endpoint_motioncontrol, eoprot_fun_ONSAY_mc);
// the aim of this function is to wake up a thread which is blocked because it has sent an ask<id32>
// the wake up funtionality is implemented in two modes, depending on the wait mechanism used:
// a. in initialisation, embObjMotionControl sets some values and then reads them back.
// the read back sends an ask<id32, signature=0xaa000000>. in such a case the board sends back
// a say<id32, data, signature = 0xaa000000>. thus, if the received signature is 0xaa000000, then
// we must unblock using feat_signal_network_reply().
// b. during runtime, some methods send a blocking ask<id32> without signature. It is the case of instance
// of getPidRaw() which waits with a eoThreadEntry::synch() call. in such a case the board send back a
// normal say<id32, data> with nos signature. in this case we unlock with wake().
if(0xaa000000 == rd->signature)
{ // case a:
if(fakestdbool_false == feat_signal_network_reply(eo_nv_GetBRD(nv), rd->id32, rd->signature))
{
char str[256] = {0};
char nvinfo[128];
eoprot_ID2information(rd->id32, nvinfo, sizeof(nvinfo));
snprintf(str, sizeof(str), "eoprot_fun_ONSAY_mc() received an unexpected message w/ 0xaa000000 signature for %s", nvinfo);
embObjPrintWarning(str);
return;
}
}
else
{ //case b:
wake(nv);
}
}
static void wake(const EOnv* nv)
{
eOprotID32_t id32 = 0;
eOprotProgNumber_t prognum = 0 ;
void *mchandler = (void*) feat_MC_handler_get(eo_nv_GetIP(nv), eo_nv_GetID32(nv));
if(NULL == mchandler)
{
printf("eoMC class not found\n");
return;
}
id32 = eo_nv_GetID32(nv);
prognum = eoprot_endpoint_id2prognum(eo_nv_GetBRD(nv), id32);
if(eobool_false == feat_MC_mutex_post(mchandler, prognum) )
{
char nvinfo[128];
char ipinfo[20];
char str[256] = {0};
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
eo_common_ipv4addr_to_string(eo_nv_GetIP(nv), ipinfo, sizeof(ipinfo));
snprintf(str, sizeof(str),"while releasing mutex for IP %s and NV %s", ipinfo, nvinfo);
feat_PrintWarning(str);
}
}
extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_replyarray(const EOnv* nv, const eOropdescriptor_t* rd)
{
if(fakestdbool_false == feat_signal_network_reply(eo_nv_GetBRD(nv), rd->id32, rd->signature))
{
printf("ERROR: eoprot_fun_UPDT_mn_comm_cmmnds_command_replyarray() has received an unexpected message\n");
return;
}
}
static void wake(const EOnv* nv)
{
eOprotID32_t id32 = 0;
eOprotProgNumber_t prognum = 0 ;
void *mchandler = (void*) feat_MC_handler_get(nvBoardNum2FeatIdBoardNum(eo_nv_GetBRD(nv)), eo_nv_GetID32(nv));
if(NULL == mchandler)
{
printf("eoMC class not found\n");
return;
}
id32 = eo_nv_GetID32(nv);
prognum = eoprot_endpoint_id2prognum(eo_nv_GetBRD(nv), id32);
if(fakestdbool_false == feat_MC_mutex_post(mchandler, prognum) )
{
char nvinfo[128];
char str[256] = {0};
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
snprintf(str, sizeof(str),"while releasing mutex in BOARD %d for variable %s", eo_nv_GetBRD(nv)+1, nvinfo);
embObjPrintWarning(str);
}
}
extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_replyarray(const EOnv* nv, const eOropdescriptor_t* rd)
{
// marco.accame on 19 mar 2014: the ethResource class sends a set<command, value> and it blocks to wait a reply.
// the reply arrives in the form sig<command, value>. the signature in here does not work, as it works only with
// ask<> / say<>.
if(eo_ropcode_sig == rd->ropcode)
{ // in here we have a sig and we cannot have the 0xaa000000 signature
if(fakestdbool_false == feat_signal_network_reply(eo_nv_GetBRD(nv), rd->id32, rd->signature))
{
printf("ERROR: eoprot_fun_UPDT_mn_comm_cmmnds_command_replyarray() has received an unexpected message\n");
return;
}
}
}
void eoprot_fun_UPDT_mn_appl_status(const EOnv* nv, const eOropdescriptor_t* rd)
{
static const char* states[] =
{
"applstate_config",
"applstate_running",
"applstate_error",
"not initted"
};
char str[256] = {0};
eOmn_appl_status_t* appstatus = (eOmn_appl_status_t*) rd->data;
const char* state = (appstatus->currstate > 2) ? (states[3]) : (states[appstatus->currstate]);
snprintf(str, sizeof(str), "MANAGEMENT-appl-status: sign = 0x%x, board EB%d -> name = %s", rd->signature, eo_nv_GetBRD(nv)+1, appstatus->name);
printf("%s\n", str);
snprintf(str, sizeof(str), " version = %d.%d, built on date %d %d %d, at hour %d:%d",
appstatus->version.major,
appstatus->version.minor,
appstatus->buildate.day,
appstatus->buildate.month,
appstatus->buildate.year,
appstatus->buildate.hour,
appstatus->buildate.min);
printf("%s\n", str);
snprintf(str, sizeof(str), " state = %s", state);
printf("%s\n", str);
fflush(stdout);
if((eo_ropcode_say == rd->ropcode) && (0xaa000000 == rd->signature))
{
if(fakestdbool_false == feat_signal_network_reply(eo_nv_GetBRD(nv), rd->id32, rd->signature))
{
printf("ERROR: eoprot_fun_UPDT_mn_appl_status() has received an unexpected message\n");
return;
}
}
}
void boardtransceiver_fun_UPDT_mc_joint_cmmnds_interactionmode(const EOnv* nv, const eOropdescriptor_t* rd)
{
EOnv_hid aNV = {0};
eOnvBRD_t brd = eo_nv_GetBRD(nv);
EOnvSet* mynvset = arrayofnvsets[brd];
eOprotIndex_t index = eoprot_ID2index(rd->id32);
eOenum08_t* pmode = (eOenum08_t*) rd->data;
eOnvID32_t id32status = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, index, eoprot_tag_mc_joint_status);
eo_nvset_NV_Get(mynvset, eok_ipv4addr_localhost, id32status, &aNV);
eOmc_joint_status_t jointstatus = {0};
uint16_t size = 0;
eOresult_t res = eo_nv_Get(&aNV, eo_nv_strg_volatile, &jointstatus, &size);
jointstatus.interactionmodestatus = *pmode;
eo_nv_Set(&aNV, &jointstatus, eobool_true, eo_nv_upd_dontdo);
}
static void s_eoprot_print_mninfo_status(eOmn_info_basic_t* infobasic, uint8_t * extra, const EOnv* nv, const eOropdescriptor_t* rd)
{
#undef DROPCODES_FROM_LIST
#define CAN_PRINT_FULL_PARSING
static const eOerror_code_t codecanprint = EOERRORCODE(eoerror_category_System, eoerror_value_SYS_canservices_canprint);
#if defined(DROPCODES_FROM_LIST)
static const eOerror_code_t codes2drop_value[] =
{
EOERRORCODE(eoerror_category_System, eoerror_value_SYS_canservices_parsingfailure),
EOERRORCODE(eoerror_category_System, eoerror_value_SYS_canservices_rxmaisbug),
EOERRORCODE(eoerror_category_System, eoerror_value_SYS_canservices_rxfromwrongboard),
EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxseqnumber_error)
};
static const int codes2drop_number = sizeof(codes2drop_value) / sizeof(eOerror_code_t);
int i;
for(i=0; i<codes2drop_number; i++)
{
if(codes2drop_value[i] == infobasic->properties.code)
{
return;
}
}
#endif
{
char str[384] = {0};
static const char * sourcestrings[] =
{
"LOCAL",
"CAN1",
"CAN2",
"UNKNOWN"
};
static const char nullverbalextra[] = "no extra info despite we are in verbal mode";
static const char emptyextra[] = "NO MORE";
uint32_t sec = infobasic->timestamp / 1000000;
uint32_t msec = (infobasic->timestamp % 1000000) / 1000;
uint32_t usec = infobasic->timestamp % 1000;
eOmn_info_type_t type = EOMN_INFO_PROPERTIES_FLAGS_get_type(infobasic->properties.flags);
eOmn_info_source_t source = EOMN_INFO_PROPERTIES_FLAGS_get_source(infobasic->properties.flags);
uint16_t address = EOMN_INFO_PROPERTIES_FLAGS_get_address(infobasic->properties.flags);
eOmn_info_extraformat_t extraf = EOMN_INFO_PROPERTIES_FLAGS_get_extraformat(infobasic->properties.flags);
uint16_t forfutureuse = EOMN_INFO_PROPERTIES_FLAGS_get_futureuse(infobasic->properties.flags);
const char * str_source = NULL;
const char * str_code = NULL;
const char * str_extra = NULL;
uint8_t *p64 = NULL;
str_source = (source > eomn_info_source_can2) ? (sourcestrings[3]) : (sourcestrings[source]);
str_code = eoerror_code2string(infobasic->properties.code);
str_extra = NULL;
if(eomn_info_extraformat_verbal == extraf)
{
str_extra = (NULL == extra) ? (nullverbalextra) : ((const char *)extra);
}
else
{
str_extra = emptyextra;
}
p64 = (uint8_t*)&(infobasic->properties.par64);
if(codecanprint == infobasic->properties.code)
{
uint16_t len = 0;
char canframestring[7] = {0};
#if defined(CAN_PRINT_FULL_PARSING)
feat_embObjCANPrintHandler(eo_nv_GetBRD(nv), infobasic);
return;
#endif
// it is a canprint: treat it in a particular way.
// in first step: just print the 6 bytes (at most) of the payload: now on 03/03/15 we have implemented first step
// in second step: do the same inside ethResources
// in third step: inside ethResources it is called the proper class can_string_generic with one instance per can board.
// maybe to save memory, we can instantiate the can_string_generic in runtime only when the can board sends a canprint.
// this third step allows to concatenate the can print frames into a single message as robotInterface
// does with can-based robots
len = infobasic->properties.par16;
if((len > 2) && (len <=8))
{
// we have a valid canframe
memcpy(canframestring, &p64[2], len-2);
canframestring[len-2] = 0; // string terminator
snprintf(str, sizeof(str), " from BOARD %d, src %s, adr %d, time %ds %dm %du: CANPRINT: %s [size = %d, D0 = 0x%.2x, D1 = 0x%.2x]",
eo_nv_GetBRD(nv)+1,
str_source,
address,
sec, msec, usec,
canframestring,
len, p64[0], p64[1]
);
}
else
{
snprintf(str, sizeof(str), " from BOARD %d, src %s, adr %d, time %ds %dm %du: CANPRINT is malformed (code 0x%.8x, par16 0x%.4x par64 0x%.2x%.2x%.2x%.2x%.2x%.2x%.2x%.2x) -> %s + INFO = %s",
eo_nv_GetBRD(nv)+1,
str_source,
address,
sec, msec, usec,
infobasic->properties.code,
infobasic->properties.par16,
p64[7], p64[6], p64[5], p64[4], p64[3], p64[2], p64[1], p64[0],
str_code,
str_extra
);
}
}
else
{ // treat it as the normal case
if(EOERRORCODE(eoerror_category_System, eoerror_value_SYS_transceiver_rxseqnumber_error) == infobasic->properties.code)
{
// marco.accame on 14apr15:
// the ems may issue such an error code at reception of first udp packet from a freshly launched robotInterface.
// the reason is that robotinterface send a ropframe with sequencenumber = 1 and the EMS may expect a bigger number
// because of a previous launch of robotinterface.
// best solution is to change fw of ems so that it sends a info with a different code (eoerror_value_SYS_transceiver_rxseqnumber_restarted).
// that will be from fw version 1.73.
// in the meantime, i transform the first report of eoerror_value_SYS_transceiver_rxseqnumber_error into a info plus an explanation.
// i will remove it later on.
static uint8_t alreadyreceived[32] = {0};
static const char realigned[] = "BUT QUITE SURELY THAT IS DUE to ROBOTINTERFACE JUST RESTARTED THUS RESETTING ITS SEQNUM";
uint8_t brd = eo_nv_GetBRD(nv);
if((brd < sizeof(alreadyreceived)) && (0 == alreadyreceived[eo_nv_GetBRD(nv)]))
{
alreadyreceived[eo_nv_GetBRD(nv)] = 1;
type = eomn_info_type_info;
str_extra = realigned;
}
}
snprintf(str, sizeof(str), " from BOARD %d, src %s, adr %d, time %ds %dm %du: (code 0x%.8x, par16 0x%.4x par64 0x%.2x%.2x%.2x%.2x%.2x%.2x%.2x%.2x) -> %s + INFO = %s",
eo_nv_GetBRD(nv)+1,
str_source,
address,
sec, msec, usec,
infobasic->properties.code,
infobasic->properties.par16,
p64[7], p64[6], p64[5], p64[4], p64[3], p64[2], p64[1], p64[0],
str_code,
str_extra
);
}
if(type == eomn_info_type_debug)
{
embObjPrintDebug(str);
}
if(type == eomn_info_type_info)
{
embObjPrintInfo(str);
}
if(type == eomn_info_type_warning)
{
embObjPrintWarning(str);
}
if(type == eomn_info_type_error)
{
embObjPrintError(str);
}
if(type == eomn_info_type_fatal)
{
embObjPrintFatal(str);
}
}
}
void eoprot_fun_UPDT_mn_appl_status(const EOnv* nv, const eOropdescriptor_t* rd)
{
static const char* states[] =
{
"applstate_config",
"applstate_running",
"applstate_error",
"not initted"
};
char str[256] = {0};
eOmn_appl_status_t* appstatus = (eOmn_appl_status_t*) rd->data;
const char* state = (appstatus->currstate > 2) ? (states[3]) : (states[appstatus->currstate]);
snprintf(str, sizeof(str), "MANAGEMENT-appl-status: sign = 0x%x, board EB%d -> name = %s", rd->signature, eo_nv_GetBRD(nv)+1, appstatus->name);
printf("%s\n", str);
snprintf(str, sizeof(str), " version = %d.%d, built on date %d %d %d, at hour %d:%d",
appstatus->version.major,
appstatus->version.minor,
appstatus->buildate.day,
appstatus->buildate.month,
appstatus->buildate.year,
appstatus->buildate.hour,
appstatus->buildate.min);
printf("%s\n", str);
//if running, print the control loop timings
if (appstatus->currstate == 1)
{
snprintf(str, sizeof(str), " control loop timings: RX->%dus, DO->%dus, TX->%dus",
appstatus->cloop_timings[0],
appstatus->cloop_timings[1],
appstatus->cloop_timings[2]);
printf("%s\n", str);
}
snprintf(str, sizeof(str), " state = %s", state);
printf("%s\n", str);
fflush(stdout);
}
void eoprot_fun_UPDT_mn_appl_status(const EOnv* nv, const eOropdescriptor_t* rd)
{
static const char* states[] =
{
"applstate_config",
"applstate_running",
"applstate_error",
"not initted"
};
char str[128] = {0};
eOmn_appl_status_t* appstatus = (eOmn_appl_status_t*) rd->data;
const char* state = (appstatus->currstate > 2) ? (states[3]) : (states[appstatus->currstate]);
snprintf(str, sizeof(str), "MANAGEMENT-appl-status: sign = 0x%x, board EB%d -> state = %s, msg = %s ", rd->signature, eo_nv_GetBRD(nv)+1, state, appstatus->filler06);
printf("%s\n", str);
}
extern void eoprot_fun_UPDT_mn_info_status(const EOnv* nv, const eOropdescriptor_t* rd)
{
char str[128] = {0};
eOmn_info_status_t* infostatus = (eOmn_info_status_t*) rd->data;
snprintf(str, sizeof(str), "MANAGEMENT-info: sign = 0x%x, board EB%d: -> info.status.type = %d, info.status.string = %s", rd->signature, eo_nv_GetBRD(nv)+1, infostatus->type, infostatus->string);
printf("%s\n", str);
}
static void s_eoprot_print_mninfo_status(eOmn_info_basic_t* infobasic, uint8_t * extra, const EOnv* nv, const eOropdescriptor_t* rd)
{
char str[256] = {0};
#if 0
uint64_t txsec = rd->time / 1000000;
uint64_t txmsec = (rd->time % 1000000) / 1000;
uint64_t txusec = rd->time % 1000;
if(1 == rd->control.plustime)
{
txsec = rd->time / 1000000;
txmsec = (rd->time % 1000000) / 1000;
txusec = rd->time % 1000;
}
else
{
txsec = txmsec = txusec = 0;
}
#endif
static const char * typestrings[] =
{
"[INFO]",
"[DEBUG]",
"[WARNING]",
"[ERROR",
"[FATAL]",
"[UNKNOWN]"
};
static const char * sourcestrings[] =
{
"LOCAL",
"CAN1",
"CAN2",
"UNKNOWN"
};
static const char nullverbalextra[] = "no extra info despite we are in verbal mode";
static const char emptyextra[] = "extra info";
uint32_t sec = infobasic->timestamp / 1000000;
uint32_t msec = (infobasic->timestamp % 1000000) / 1000;
uint32_t usec = infobasic->timestamp % 1000;
eOmn_info_type_t type = EOMN_INFO_PROPERTIES_FLAGS_get_type(infobasic->properties.flags);
eOmn_info_source_t source = EOMN_INFO_PROPERTIES_FLAGS_get_source(infobasic->properties.flags);
uint16_t address = EOMN_INFO_PROPERTIES_FLAGS_get_address(infobasic->properties.flags);
eOmn_info_extraformat_t extraf = EOMN_INFO_PROPERTIES_FLAGS_get_extraformat(infobasic->properties.flags);
uint16_t forfutureuse = EOMN_INFO_PROPERTIES_FLAGS_get_futureuse(infobasic->properties.flags);
const char * str_type = (type > eomn_info_type_fatal) ? (typestrings[5]) : (typestrings[type]);
const char * str_source = (source > eomn_info_source_can2) ? (sourcestrings[3]) : (sourcestrings[source]);;
const char * str_code = eoerror_code2string(infobasic->properties.code);
const char * str_extra = NULL;
if(eomn_info_extraformat_verbal == extraf)
{
str_extra = (NULL == extra) ? (nullverbalextra) : ((const char *)extra);
}
else
{
str_extra = emptyextra;
}
snprintf(str, sizeof(str), "%s from BOARD %d, source %s, address %d, time %ds %dm %du: (code 0x%x, param 0x%x) -> %s + %s",
str_type,
eo_nv_GetBRD(nv)+1,
str_source,
address,
sec, msec, usec,
infobasic->properties.code,
infobasic->properties.param,
str_code,
str_extra
);
printf("%s\n", str);
fflush(stdout);
}
extern void eoprot_fun_UPDT_mn_info_status(const EOnv* nv, const eOropdescriptor_t* rd)
{
char str[256] = {0};
eOmn_info_status_t* infostatus = (eOmn_info_status_t*) rd->data;
uint64_t sec = rd->time / 1000000;
uint64_t msec = (rd->time % 1000000) / 1000;
uint64_t usec = rd->time % 1000;
if(1 == rd->control.plustime)
{
sec = rd->time / 1000000;
msec = (rd->time % 1000000) / 1000;
usec = rd->time % 1000;
}
else
{
sec = msec = usec = 0;
}
const char * infotype[] =
{
"eomn_info_type_info",
"eomn_info_type_debug",
"eomn_info_type_warning",
"eomn_info_type_error",
"unknown"
};
const char * sss = (infostatus->properties.type > 3) ? (infotype[4]) : (infotype[infostatus->properties.type]);
snprintf(str, sizeof(str), "[INFO]-> mn-info: ropsign = 0x%x, roptime = %04ds+%03dms+%03dus, BOARD = %d: -> info.status.properties.type = %s, info.status.data = %s", rd->signature, (uint32_t)sec, (uint32_t)msec, (uint32_t)usec, eo_nv_GetBRD(nv)+1, sss, infostatus->data);
printf("%s\n", str);
fflush(stdout);
}
