17 #include <yarp/os/Time.h>
23 #include "EoAnalogSensors.h"
24 #include "EOconstarray.h"
25 #include "EoProtocolAS.h"
29 #pragma warning(once:4355)
36 using namespace yarp::os;
40 #define GET_privData(x) (*((static_cast<eo_ftsens_privData*>(x))))
42 embObjFTsensor::embObjFTsensor()
48 embObjFTsensor::~embObjFTsensor()
55 std::string embObjFTsensor::getBoardInfo(
void)
const
60 bool embObjFTsensor::initialised()
66 bool embObjFTsensor::enableTemperatureTransmission(
bool enable)
79 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_temperature, 0, eoprot_tag_as_temperature_cmmnds_enable);
82 yError() << getBoardInfo() <<
"fails send command enableTemperatureTransmission(" << enable <<
")";
90 bool embObjFTsensor::open(yarp::os::Searchable &config)
94 if(!
GET_privData(mPriv).prerareEthService(config,
this))
100 if(!
GET_privData(mPriv).fromConfig(config, serviceConfig))
103 if(!
GET_privData(mPriv).res->verifyEPprotocol(eoprot_endpoint_analogsensors))
110 #if defined(EMBOBJSTRAIN_USESERVICEPARSER)
113 eOmn_serv_parameter_t servparamtemp;
120 const eOmn_serv_parameter_t* servparamtemp_ptr = &servparamtemp;
121 const eOmn_serv_parameter_t* servparamstrain = &serviceConfig.
ethservice;
124 const eOmn_serv_parameter_t* servparamstrain = NULL;
125 const eOmn_serv_parameter_t* servparamtemp_ptr = NULL;
128 if(
false ==
GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_strain, servparamstrain, 5.0))
130 yError() << getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate()";
135 if(
false ==
GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_temperatures, servparamtemp_ptr, 5.0))
137 yError() << getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate()";
144 if(
false ==
GET_privData(mPriv).fillScaleFactor(serviceConfig))
146 yError() << getBoardInfo() <<
"open() has failed in calling embObjFTsensor::fillScaleFactor()";
150 if(
false ==
GET_privData(mPriv).sendConfig2Strain(serviceConfig))
156 if(
false ==
GET_privData(mPriv).initRegulars(serviceConfig))
163 if(
false ==
GET_privData(mPriv).res->serviceStart(eomn_serv_category_strain))
165 yError() << getBoardInfo() <<
"open() fails to start service strain";
173 yDebug() << getBoardInfo() <<
"open() correctly starts as service strain";
177 if(
false ==
GET_privData(mPriv).res->serviceStart(eomn_serv_category_temperatures))
179 yError() << getBoardInfo() <<
"open() fails to start service temperature";
185 yDebug() << getBoardInfo() <<
"open() correctly starts as service temperature";
189 if(!enableTemperatureTransmission(
true))
191 yError() << getBoardInfo() <<
"open() fails to enable temperature transmission";
201 bool embObjFTsensor::close()
207 void embObjFTsensor::cleanup(
void)
212 enableTemperatureTransmission(
false);
221 bool embObjFTsensor::update(eOprotID32_t id32,
double timestamp,
void* rxdata)
229 eOprotEntity_t entity = eoprot_ID2entity(id32);
233 case eoas_entity_strain:
235 ret = updateStrainValues(id32, timestamp, rxdata);
238 case eoas_entity_temperature:
240 ret = updateTemperatureValues(id32, timestamp, rxdata);
245 yError() << getBoardInfo() <<
"update() failed ";
251 bool embObjFTsensor::updateStrainValues(eOprotID32_t id32,
double timestamp,
void* rxdata)
254 timestamp = timestamp;
262 EOarray *array = (EOarray*)rxdata;
263 uint8_t size = eo_array_Size(array);
264 uint8_t itemsize = eo_array_ItemSize(array);
265 if((0 == size) || (2 != itemsize))
271 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
272 GET_privData(mPriv).timestampAnalogdata = yarp::os::Time::now();
273 for (
size_t k = 0; k<
GET_privData(mPriv).analogdata.size(); k++)
276 char*
tmp = (
char*) eo_array_At(array, k);
281 uint8_t msg[2] = {0};
284 GET_privData(mPriv).analogdata[k] = (short)( ( (((
unsigned short)(msg[1]))<<8)+msg[0]) - (
unsigned short) (0x8000) );
296 bool embObjFTsensor::updateTemperatureValues(eOprotID32_t id32,
double timestamp,
void* rxdata)
298 eOas_temperature_status_t *temp_st = (eOas_temperature_status_t *)rxdata;
300 EOconstarray* arrayofvalues = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(&(temp_st->arrayofdata)));
302 uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
304 if(numofIntem2update>1)
305 yError() << getBoardInfo() <<
"updateTemperature: I expect 1 item, but I received " << numofIntem2update ;
307 for(
int i=0; i<numofIntem2update; i++)
309 eOas_temperature_data_t *
data = (eOas_temperature_data_t*) eo_constarray_At(arrayofvalues, i);
312 yError() << getBoardInfo() <<
"update(): I have to update " << numofIntem2update <<
"items, but the " << i <<
"-th item is null.";
319 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
321 GET_privData(mPriv).timestampTemperature = yarp::os::Time::now();
333 size_t embObjFTsensor::getNrOfTemperatureSensors()
const
338 yarp::dev::MAS_status embObjFTsensor::getTemperatureSensorStatus(
size_t sens_index)
const
340 return yarp::dev::MAS_OK;
343 bool embObjFTsensor::getTemperatureSensorName(
size_t sens_index, std::string &name)
const
349 bool embObjFTsensor::getTemperatureSensorFrameName(
size_t sens_index, std::string &frameName)
const
355 bool embObjFTsensor::getTemperatureSensorMeasure(
size_t sens_index,
double&
out,
double& timestamp)
const
357 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
363 bool embObjFTsensor::getTemperatureSensorMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
365 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
374 size_t embObjFTsensor::getNrOfSixAxisForceTorqueSensors()
const
379 yarp::dev::MAS_status embObjFTsensor::getSixAxisForceTorqueSensorStatus(
size_t sens_index)
const
381 return yarp::dev::MAS_OK;
384 bool embObjFTsensor::getSixAxisForceTorqueSensorName(
size_t sens_index, std::string &name)
const
390 bool embObjFTsensor::getSixAxisForceTorqueSensorFrameName(
size_t sens_index, std::string &frameName)
const
396 bool embObjFTsensor::getSixAxisForceTorqueSensorMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
403 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
406 for (
size_t k = 0; k<
GET_privData(mPriv).analogdata.size(); k++)
Copyright (C) 2008 RobotCub Consortium.
eOmn_serv_parameter_t ethservice