iCub-main
Loading...
Searching...
No Matches
embObjFTsensor.cpp
Go to the documentation of this file.
1
2// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
3
4/*
5* Copyright (C) 2012 Robotcub Consortium
6* Author: Valentina Gaggero
7* CopyPolicy: Released under the terms of the GNU GPL v2.0.
8*
9*/
10
11// general purpose stuff.
12#include <string>
13#include <iostream>
14#include <string.h>
15
16// Yarp Includes
17#include <yarp/os/Time.h>
18
19// specific to this device driver.
20#include "embObjFTsensor.h"
21#include "eo_ftsens_privData.h"
22
23#include "EoAnalogSensors.h"
24#include "EOconstarray.h"
25#include "EoProtocolAS.h"
26
27
28#ifdef WIN32
29#pragma warning(once:4355)
30#endif
31
32
33
34
35using namespace yarp;
36using namespace yarp::os;
37using namespace yarp::dev;
38
39
40#define GET_privData(x) (*((static_cast<eo_ftsens_privData*>(x))))
41
43{
44 mPriv = new eo_ftsens_privData("embObjFTsensor");
45}
46
47
49{
50 close();
51 delete &GET_privData(mPriv);
52}
53
54
55std::string embObjFTsensor::getBoardInfo(void) const
56{
57 return GET_privData(mPriv).getBoardInfo();
58}
59
61{
62 return GET_privData(mPriv).isOpen();
63}
64
65
66bool embObjFTsensor::enableTemperatureTransmission(bool enable)
67{
68 if(GET_privData(mPriv).res==nullptr)
69 {
70 //This check is necessary because the function is called by destructor and we cannot be sure that open function has been invoked before its deletion.
71 //Please notice that yarp::dev::DriversHelper::load() function creates this object and then destroys it without calls the open().
72 //Here I don't give error neither warning, because the load function is called to load this devicedriver at yarprobotinterface startup, so it is the wanted behaviour.
73 return false;
74 }
75
76 uint8_t cmd;
77 (enable) ? cmd =1: cmd=0;
78
79 eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_temperature, 0, eoprot_tag_as_temperature_cmmnds_enable);
80 if(false == GET_privData(mPriv).res->setRemoteValue(id32, &cmd))
81 {
82 yError() << getBoardInfo() << "fails send command enableTemperatureTransmission(" << enable << ")";
83 return false;
84 }
85 return true;
86}
87
88//----------------------- DeviceDriver -------------------
89
90bool embObjFTsensor::open(yarp::os::Searchable &config)
91{
92 // - first thing to do is verify if the eth manager is available then i parse info about the eth board.
93
94 if(! GET_privData(mPriv).prerareEthService(config, this))
95 return false;
96
97 // read stuff from config file
98
99 servConfigFTsensor_t serviceConfig;
100 if(!GET_privData(mPriv).fromConfig(config, serviceConfig))
101 return false;
102
103 if(!GET_privData(mPriv).res->verifyEPprotocol(eoprot_endpoint_analogsensors))
104 {
105 cleanup();
106 return false;
107 }
108
109
110#if defined(EMBOBJSTRAIN_USESERVICEPARSER)
111
112 //Fill temperature service data in servparamtemp: some of these data are copied from the serviceconfig of ft because both services used tha same board.
113 eOmn_serv_parameter_t servparamtemp;
114 if (GET_privData(mPriv).useTemperature) {
115 bool ret = GET_privData(mPriv).fillTemperatureEthServiceInfo(serviceConfig.ethservice, servparamtemp);
116 if(!ret)
117 return false;
118 }
119
120 const eOmn_serv_parameter_t* servparamtemp_ptr = &servparamtemp;
121 const eOmn_serv_parameter_t* servparamstrain = &serviceConfig.ethservice;
122
123#else
124 const eOmn_serv_parameter_t* servparamstrain = NULL;
125 const eOmn_serv_parameter_t* servparamtemp_ptr = NULL;
126#endif
127
128 if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_strain, servparamstrain, 5.0))
129 {
130 yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()";
131 cleanup();
132 return false;
133 }
134 if (GET_privData(mPriv).useTemperature) {
135 if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_temperatures, servparamtemp_ptr, 5.0))
136 {
137 yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()";
138 cleanup();
139 return false;
140 }
141 }
142
143 // we always prepare the fullscales.
144 if(false == GET_privData(mPriv).fillScaleFactor(serviceConfig))
145 {
146 yError() << getBoardInfo() << "open() has failed in calling embObjFTsensor::fillScaleFactor()";
147 return false;
148 }
149
150 if(false == GET_privData(mPriv).sendConfig2Strain(serviceConfig))
151 {
152 cleanup();
153 return false;
154 }
155
156 if(false == GET_privData(mPriv).initRegulars(serviceConfig))
157 {
158 cleanup();
159 return false;
160 }
161
162
163 if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_strain))
164 {
165 yError() << getBoardInfo() << "open() fails to start service strain";
166 cleanup();
167 return false;
168 }
169 else
170 {
171 if(GET_privData(mPriv).isVerbose())
172 {
173 yDebug() << getBoardInfo() << "open() correctly starts as service strain";
174 }
175 }
176 if (GET_privData(mPriv).useTemperature) {
177 if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_temperatures))
178 {
179 yError() << getBoardInfo() << "open() fails to start service temperature";
180 cleanup();
181 return false;
182 }
183 else if(GET_privData(mPriv).isVerbose())
184 {
185 yDebug() << getBoardInfo() << "open() correctly starts as service temperature";
186 }
187
188 // start the configured sensors. so far, we must keep it in here. later on we can remove this command
189 if(!enableTemperatureTransmission(true))
190 {
191 yError() << getBoardInfo() << "open() fails to enable temperature transmission";
192 cleanup();
193 return false;
194 }
195 }
196
197 GET_privData(mPriv).setOpen(true);
198 return true;
199}
200
202{
203 cleanup();
204 return true;
205}
206
207void embObjFTsensor::cleanup(void)
208{
209
210 if (GET_privData(mPriv).useTemperature) {
211 // disable temperature
212 enableTemperatureTransmission(false);
213 }
214
215 GET_privData(mPriv).cleanup(static_cast <eth::IethResource*> (this));
216}
217
218
219
220//------------------------------- IethResource --------------------------------
221bool embObjFTsensor::update(eOprotID32_t id32, double timestamp, void* rxdata)
222{
223 bool ret = false;
224
225 if(false == GET_privData(mPriv).isOpen())
226 {
227 return ret;;
228 }
229 eOprotEntity_t entity = eoprot_ID2entity(id32);
230
231 switch(entity)
232 {
233 case eoas_entity_strain:
234 {
235 ret = updateStrainValues(id32, timestamp, rxdata);
236 }break;
237
238 case eoas_entity_temperature:
239 {
240 ret = updateTemperatureValues(id32, timestamp, rxdata);
241 }break;
242 default:
243 {
244 ret = false;
245 yError() << getBoardInfo() << "update() failed ";
246 }
247 };
248 return ret;
249}
250
251bool embObjFTsensor::updateStrainValues(eOprotID32_t id32, double timestamp, void* rxdata)
252{
253 id32 = id32;
254 timestamp = timestamp;
255
256
257 // called by feat_manage_analogsensors_data() which is called by:
258 // eoprot_fun_UPDT_as_strain_status_calibratedvalues() or eoprot_fun_UPDT_as_strain_status_uncalibratedvalues()
259 // the void* parameter inside this function is a eOas_arrayofupto12bytes_t*
260 // and can be treated as a EOarray
261
262 EOarray *array = (EOarray*)rxdata;
263 uint8_t size = eo_array_Size(array);
264 uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 2, as the code after uses this convention
265 if((0 == size) || (2 != itemsize))
266 {
267 return false;
268 }
269
270 // lock analogdata
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++)
274 {
275 // Get the kth element of the array as a 2 bytes msg
276 char* tmp = (char*) eo_array_At(array, k);
277 // marco.accame: i am nervous about iterating for strain_Channels instead of size of array....
278 // thus i add a protection. if k goes beyond size of array, eo_array_At() returns NULL.
279 if(NULL != tmp)
280 {
281 uint8_t msg[2] = {0};
282 memcpy(msg, tmp, 2);
283 // Got from canBusMotionControl
284 GET_privData(mPriv).analogdata[k] = (short)( ( (((unsigned short)(msg[1]))<<8)+msg[0]) - (unsigned short) (0x8000) );
285
286 if(true == GET_privData(mPriv).useCalibValues)
287 {
288 GET_privData(mPriv).analogdata[k] = GET_privData(mPriv).analogdata[k]* GET_privData(mPriv).scaleFactor[k]/float(0x8000);
289 }
290 }
291 }
292
293 return true;
294}
295
296bool embObjFTsensor::updateTemperatureValues(eOprotID32_t id32, double timestamp, void* rxdata)
297{
298 eOas_temperature_status_t *temp_st = (eOas_temperature_status_t *)rxdata;
299
300 EOconstarray* arrayofvalues = eo_constarray_Load(reinterpret_cast<const EOarray*>(&(temp_st->arrayofdata)));
301
302 uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
303
304 if(numofIntem2update>1)
305 yError() << getBoardInfo() << "updateTemperature: I expect 1 item, but I received " << numofIntem2update ;
306
307 for(int i=0; i<numofIntem2update; i++)
308 {
309 eOas_temperature_data_t *data = (eOas_temperature_data_t*) eo_constarray_At(arrayofvalues, i);
310 if(data == NULL)
311 {
312 yError() << getBoardInfo() << "update(): I have to update " << numofIntem2update << "items, but the " << i << "-th item is null.";
313 continue;
314 //NOTE: I signal this strange situation with an arror for debug porpouse...maybe we can convert in in warning when the device is stable....
315 }
316 else
317 {
318 //yDebug() << "embObjFTStrain" << getBoardInfo() << ": val "<< i<< "/" << numofIntem2update << ": value=" << data->value << ", time=" << data->timestamp;
319 std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
320 GET_privData(mPriv).lastTemperature = static_cast<float>(data->value);
321 GET_privData(mPriv).timestampTemperature = yarp::os::Time::now();
322 }
323 }
324 return true;
325}
326
331
332// ---------------------- ITemperatureSensors --------------------------------------------------------
334{
335 return GET_privData(mPriv).useTemperature ? 1 : 0;
336}
337
338yarp::dev::MAS_status embObjFTsensor::getTemperatureSensorStatus(size_t sens_index) const
339{
340 return yarp::dev::MAS_OK;
341}
342
343bool embObjFTsensor::getTemperatureSensorName(size_t sens_index, std::string &name) const
344{
345 name = GET_privData(mPriv).devicename;
346 return true;
347}
348
349bool embObjFTsensor::getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const
350{
351 frameName = GET_privData(mPriv).frameName;
352 return true;
353}
354
355bool embObjFTsensor::getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const
356{
357 std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
358 out = GET_privData(mPriv).lastTemperature/10.0; //I need to convert from tenths of degree centigrade to degree centigrade
359 timestamp = GET_privData(mPriv).timestampTemperature;
360 return true;
361}
362
363bool embObjFTsensor::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
364{
365 std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
366 out.resize(1);
367 out[0] = GET_privData(mPriv).lastTemperature/10.0; //I need to convert from tenths of degree centigrade to degree centigrade
368 timestamp = GET_privData(mPriv).timestampTemperature;
369 return true;
370}
371
372//------------------------- ISixAxisForceTorqueSensors -------------------------
373
375{
376 return 1;
377}
378
379yarp::dev::MAS_status embObjFTsensor::getSixAxisForceTorqueSensorStatus(size_t sens_index) const
380{
381 return yarp::dev::MAS_OK;
382}
383
384bool embObjFTsensor::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const
385{
386 name = GET_privData(mPriv).devicename;
387 return true;
388}
389
390bool embObjFTsensor::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
391{
392 frameName = GET_privData(mPriv).frameName;
393 return true;
394}
395
396bool embObjFTsensor::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
397{
398 if(false == GET_privData(mPriv).isOpen())
399 {
400 return false;
401 }
402
403 std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
404
405 out.resize(GET_privData(mPriv).analogdata.size());
406 for (size_t k = 0; k<GET_privData(mPriv).analogdata.size(); k++)
407 {
408 out[k] = GET_privData(mPriv).analogdata[k] + GET_privData(mPriv).offset[k];
409 }
410
411 timestamp = GET_privData(mPriv).timestampAnalogdata;
412
413 return true;
414}
415
416// eof
417
@ data
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 &timestamp) const override
virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override
virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sens_index) const override
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
cmd
Definition dataTypes.h:30
#define GET_privData(x)
iethresType_t
@ iethres_analogstrain
Copyright (C) 2008 RobotCub Consortium.
out
Definition sine.m:8
eOmn_serv_parameter_t ethservice