eObool_t feat_MC_mutex_post(void *mchandler, uint32_t prognum)
{
eoThreadEntry *th = NULL;
IethResource *ier = static_cast<IethResource*>(mchandler);
IethResource *mc = ier; //dynamic_cast<embObjMotionControl *>(ier);
if(NULL == mc)
{
return eobool_false;
}
else if(false == mc->initialised())
{ // it can be that the object is already created but its open() not yet completed. it is in open() that we allocate requestQueue ....
return eobool_false;
}
int threadId;
eoThreadFifo *fuffy = mc->getFifo(prognum);
if( (threadId = fuffy->pop()) < 0)
{
yError() << "Received an answer message nobody is waiting for (MCmutex_post)";
return eobool_false;
}
else
{
th = mc->getThreadTable(threadId);
if(NULL == th)
yError() << "MCmutex_post error at line " << __LINE__;
th->push();
return eobool_true;
}
return eobool_false;
}
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_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;
}
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;
}
fakestdbool_t feat_manage_motioncontrol_data(FEAT_boardnumber_t boardnum, eOprotID32_t id32, void* rxdata)
{
IethResource* mc = NULL;
ethFeatType_t type;
bool ret = _interface2ethManager->getHandle(boardnum, id32, &mc, &type);
if(!ret)
{
yDebug("EMS callback was unable to get access to the embObjMotionControl");
return fakestdbool_false;
}
if(type != ethFeatType_MotionControl)
{
return fakestdbool_false;
}
else if(false == mc->initialised())
{
return fakestdbool_false;
}
else
{
mc->update(id32, yarp::os::Time::now(), rxdata);
}
return fakestdbool_true;
}
fakestdbool_t feat_manage_analogsensors_data(FEAT_boardnumber_t boardnum, eOprotID32_t id32, void *as_array)
{
IethResource* sensor;
ethFeatType_t type;
bool ret = _interface2ethManager->getHandle(boardnum, id32, &sensor, &type);
if(!ret)
{
yDebug("EMS callback was unable to get access to the embObjAnalogSensor");
return fakestdbool_false;
}
if(! ((type == ethFeatType_AnalogMais) || (type == ethFeatType_AnalogStrain)) )
{
yError("EMS analog sensor callback - the ethmanager does not know this object YET or a wrong pointer has been returned because it is not an embObjAnalogSensor");
return fakestdbool_false;
}
else if(false == sensor->initialised())
{ // the ethmanager has the object, but the obiect was not full initted yet. cannot use it
return fakestdbool_false;
}
else
{ // the object exists and is completed: it can be used
sensor->update(id32, yarp::os::Time::now(), as_array);
}
return fakestdbool_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;
}
