17#include <yarp/os/Time.h>
23#include "EoAnalogSensors.h"
24#include "EOconstarray.h"
25#include "EoProtocolAS.h"
29#pragma warning(once:4355)
36using namespace yarp::os;
40#define GET_privData(x) (*((static_cast<eo_ftsens_privData*>(x))))
55std::string embObjFTsensor::getBoardInfo(
void)
const
66bool 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 <<
")";
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";
207void embObjFTsensor::cleanup(
void)
212 enableTemperatureTransmission(
false);
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 ";
251bool 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) );
296bool 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();
340 return yarp::dev::MAS_OK;
357 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
365 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
381 return yarp::dev::MAS_OK;
403 std::lock_guard<std::mutex> lck(
GET_privData(mPriv).mtx);
406 for (
size_t k = 0; k<
GET_privData(mPriv).analogdata.size(); k++)
virtual bool getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const override
bool open(yarp::os::Searchable &config)
virtual size_t getNrOfTemperatureSensors() const override
virtual size_t getNrOfSixAxisForceTorqueSensors() const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
virtual eth::iethresType_t type()
virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const override
virtual bool getTemperatureSensorName(size_t sens_index, std::string &name) const override
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
virtual bool getTemperatureSensorMeasure(size_t sens_index, double &out, double ×tamp) const override
virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override
virtual bool initialised()
virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sens_index) const override
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Copyright (C) 2008 RobotCub Consortium.
eOmn_serv_parameter_t ethservice