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;
118 const eOmn_serv_parameter_t* servparamtemp_ptr = &servparamtemp;
119 const eOmn_serv_parameter_t* servparamstrain = &serviceConfig.
ethservice;
122 const eOmn_serv_parameter_t* servparamstrain = NULL;
123 const eOmn_serv_parameter_t* servparamtemp_ptr = NULL;
126 if(
false ==
GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_strain, servparamstrain, 5.0))
128 yError() << getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate()";
133 if(
false ==
GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_temperatures, servparamtemp_ptr, 5.0))
135 yError() << getBoardInfo() <<
"open() has an error in call of ethResources::serviceVerifyActivate()";
141 if(
false ==
GET_privData(mPriv).fillScaleFactor(serviceConfig))
143 yError() << getBoardInfo() <<
"open() has failed in calling embObjFTsensor::fillScaleFactor()";
147 if(
false ==
GET_privData(mPriv).sendConfig2Strain(serviceConfig))
153 if(
false ==
GET_privData(mPriv).initRegulars(serviceConfig))
160 if(
false ==
GET_privData(mPriv).res->serviceStart(eomn_serv_category_strain))
162 yError() << getBoardInfo() <<
"open() fails to start service strain";
170 yDebug() << getBoardInfo() <<
"open() correctly starts as service strain";
174 if(
false ==
GET_privData(mPriv).res->serviceStart(eomn_serv_category_temperatures))
176 yError() << getBoardInfo() <<
"open() fails to start service temperature";
184 yDebug() << getBoardInfo() <<
"open() correctly starts as service temperature";
189 if(!enableTemperatureTransmission(
true))
191 yError() << getBoardInfo() <<
"open() fails to enable temperature transmission";
200 bool embObjFTsensor::close()
206 void embObjFTsensor::cleanup(
void)
210 enableTemperatureTransmission(
false);
218 bool embObjFTsensor::update(eOprotID32_t id32,
double timestamp,
void* rxdata)
226 eOprotEntity_t entity = eoprot_ID2entity(id32);
230 case eoas_entity_strain:
232 ret = updateStrainValues(id32, timestamp, rxdata);
235 case eoas_entity_temperature:
237 ret = updateTemperatureValues(id32, timestamp, rxdata);
242 yError() << getBoardInfo() <<
"update() failed ";
248 bool embObjFTsensor::updateStrainValues(eOprotID32_t id32,
double timestamp,
void* rxdata)
251 timestamp = timestamp;
259 EOarray *array = (EOarray*)rxdata;
260 uint8_t size = eo_array_Size(array);
261 uint8_t itemsize = eo_array_ItemSize(array);
262 if((0 == size) || (2 != itemsize))
268 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
269 GET_privData(mPriv).timestampAnalogdata = yarp::os::Time::now();
270 for (
size_t k = 0; k<
GET_privData(mPriv).analogdata.size(); k++)
273 char*
tmp = (
char*) eo_array_At(array, k);
278 uint8_t msg[2] = {0};
281 GET_privData(mPriv).analogdata[k] = (short)( ( (((
unsigned short)(msg[1]))<<8)+msg[0]) - (
unsigned short) (0x8000) );
293 bool embObjFTsensor::updateTemperatureValues(eOprotID32_t id32,
double timestamp,
void* rxdata)
295 eOas_temperature_status_t *temp_st = (eOas_temperature_status_t *)rxdata;
297 EOconstarray* arrayofvalues = eo_constarray_Load(
reinterpret_cast<const EOarray*
>(&(temp_st->arrayofdata)));
299 uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
301 if(numofIntem2update>1)
302 yError() << getBoardInfo() <<
"updateTemperature: I expect 1 item, but I received " << numofIntem2update ;
304 for(
int i=0; i<numofIntem2update; i++)
306 eOas_temperature_data_t *
data = (eOas_temperature_data_t*) eo_constarray_At(arrayofvalues, i);
309 yError() << getBoardInfo() <<
"update(): I have to update " << numofIntem2update <<
"items, but the " << i <<
"-th item is null.";
316 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
318 GET_privData(mPriv).timestampTemperature = yarp::os::Time::now();
341 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
344 for (
size_t k = 0; k<
GET_privData(mPriv).analogdata.size(); k++)
349 return IAnalogSensor::AS_OK;;
352 int embObjFTsensor::getState(
int ch)
358 int embObjFTsensor::getChannels()
364 int embObjFTsensor::calibrateSensor()
366 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
367 for (
size_t i = 0; i <
GET_privData(mPriv).analogdata.size(); i++)
374 int embObjFTsensor::calibrateSensor(
const yarp::sig::Vector& value)
379 int embObjFTsensor::calibrateChannel(
int ch)
384 int embObjFTsensor::calibrateChannel(
int ch,
double v)
397 size_t embObjFTsensor::getNrOfTemperatureSensors()
const
402 yarp::dev::MAS_status embObjFTsensor::getTemperatureSensorStatus(
size_t sens_index)
const
404 return yarp::dev::MAS_OK;
407 bool embObjFTsensor::getTemperatureSensorName(
size_t sens_index, std::string &name)
const
413 bool embObjFTsensor::getTemperatureSensorFrameName(
size_t sens_index, std::string &frameName)
const
419 bool embObjFTsensor::getTemperatureSensorMeasure(
size_t sens_index,
double&
out,
double& timestamp)
const
421 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
427 bool embObjFTsensor::getTemperatureSensorMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
429 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
438 size_t embObjFTsensor::getNrOfSixAxisForceTorqueSensors()
const
443 yarp::dev::MAS_status embObjFTsensor::getSixAxisForceTorqueSensorStatus(
size_t sens_index)
const
445 return yarp::dev::MAS_OK;
448 bool embObjFTsensor::getSixAxisForceTorqueSensorName(
size_t sens_index, std::string &name)
const
454 bool embObjFTsensor::getSixAxisForceTorqueSensorFrameName(
size_t sens_index, std::string &frameName)
const
460 bool embObjFTsensor::getSixAxisForceTorqueSensorMeasure(
size_t sens_index, yarp::sig::Vector&
out,
double& timestamp)
const
467 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
470 for (
size_t k = 0; k<
GET_privData(mPriv).analogdata.size(); k++)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Copyright (C) 2008 RobotCub Consortium.
eOmn_serv_parameter_t ethservice