eObool_t feat_manage_motioncontrol_data(eOipv4addr_t ipv4, eOprotID32_t id32, void* rxdata)
{
IethResource* mc = NULL;
if(NULL == _interface2ethManager)
{
return eobool_false;
}
mc = _interface2ethManager->getInterface(ipv4, id32);
if(NULL == mc)
{
char ipinfo[20];
char nvinfo[128];
eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
yDebug("feat_manage_motioncontrol_data() fails to get a handle of embObjMotionControl for IP = %s and NV = %s", ipinfo, nvinfo);
return eobool_false;
}
if(false == mc->initialised())
{
return eobool_false;
}
else
{
mc->update(id32, yarp::os::Time::now(), rxdata);
}
return eobool_true;
}
eObool_t feat_manage_skin_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *arrayofcandata)
{
IethResource* skin;
if(NULL == _interface2ethManager)
{
return eobool_false;
}
skin = _interface2ethManager->getInterface(ipv4, id32);
if(NULL == skin)
{
char ipinfo[20];
char nvinfo[128];
eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
yDebug("feat_manage_skin_data() fails to get a handle of embObjSkin for IP = %s and NV = %s", ipinfo, nvinfo);
return eobool_false;
}
if(false == skin->initialised())
{
return eobool_false;
}
else
{
skin->update(id32, yarp::os::Time::now(), (void *)arrayofcandata);
}
return eobool_true;
}
eObool_t feat_manage_analogsensors_data(eOipv4addr_t ipv4, eOprotID32_t id32, void *data)
{
IethResource* sensor;
if(NULL == _interface2ethManager)
{
return eobool_false;
}
sensor = _interface2ethManager->getInterface(ipv4, id32);
if(NULL == sensor)
{
char ipinfo[20];
char nvinfo[128];
eo_common_ipv4addr_to_string(ipv4, ipinfo, sizeof(ipinfo));
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
yDebug("feat_manage_analogsensors_data() fails to get a handle of embObjAnalogSensor for IP = %s and NV = %s", ipinfo, nvinfo);
return eobool_false;
}
if(false == sensor->initialised())
{
return eobool_false;
}
else
{ // data is a EOarray* in case of mais or strain but it is a eOas_inertial_status_t* in case of inertial sensor
sensor->update(id32, yarp::os::Time::now(), data);
}
return eobool_true;
}
fakestdbool_t feat_manage_skin_data(FEAT_boardnumber_t boardnum, eOprotID32_t id32, void *arrayofcandata)
{
static int error = 0;
IethResource* skin;
ethFeatType_t type;
bool ret = _interface2ethManager->getHandle(boardnum, id32, &skin, &type);
if(!ret)
{
yDebug("EMS callback was unable to get access to the embObjSkin");
return fakestdbool_false;
}
if(type != ethFeatType_Skin)
{ // the ethmanager does not know this object yet or the dynamic cast failed because it is not an embObjSkin
char nvinfo[128];
eoprot_ID2information(id32, nvinfo, sizeof(nvinfo));
if(0 == (error%1000) )
yWarning() << "feat_manage_skin_data() received a request from BOARD" << boardnum << "with ID:" << nvinfo << "but no class was jet instatiated for it";
error++;
return fakestdbool_false;
}
else if(false == skin->initialised())
{ // the ethmanager has the object, but the device was not fully opened yet. cannot use it
return fakestdbool_false;
}
else
{ // the object exists and is completed: it can be used
skin->update(id32, yarp::os::Time::now(), (void *)arrayofcandata);
}
return fakestdbool_true;
}
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);
}
}
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);
}
}
