iCub-main
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 
35 using namespace yarp;
36 using namespace yarp::os;
37 using namespace yarp::dev;
38 
39 
40 #define GET_privData(x) (*((static_cast<eo_ftsens_privData*>(x))))
41 
42 embObjFTsensor::embObjFTsensor()
43 {
44  mPriv = new eo_ftsens_privData("embObjFTsensor");
45 }
46 
47 
48 embObjFTsensor::~embObjFTsensor()
49 {
50  close();
51  delete &GET_privData(mPriv);
52 }
53 
54 
55 std::string embObjFTsensor::getBoardInfo(void) const
56 {
57  return GET_privData(mPriv).getBoardInfo();
58 }
59 
60 bool embObjFTsensor::initialised()
61 {
62  return GET_privData(mPriv).isOpen();
63 }
64 
65 
66 bool 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 
90 bool 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 
201 bool embObjFTsensor::close()
202 {
203  cleanup();
204  return true;
205 }
206 
207 void 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 --------------------------------
221 bool 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 
251 bool 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 
296 bool 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 
327 eth::iethresType_t embObjFTsensor::type()
328 {
330 }
331 
332 // ---------------------- ITemperatureSensors --------------------------------------------------------
333 size_t embObjFTsensor::getNrOfTemperatureSensors() const
334 {
335  return GET_privData(mPriv).useTemperature ? 1 : 0;
336 }
337 
338 yarp::dev::MAS_status embObjFTsensor::getTemperatureSensorStatus(size_t sens_index) const
339 {
340  return yarp::dev::MAS_OK;
341 }
342 
343 bool embObjFTsensor::getTemperatureSensorName(size_t sens_index, std::string &name) const
344 {
345  name = GET_privData(mPriv).devicename;
346  return true;
347 }
348 
349 bool embObjFTsensor::getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const
350 {
351  frameName = GET_privData(mPriv).frameName;
352  return true;
353 }
354 
355 bool 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 
363 bool 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 
374 size_t embObjFTsensor::getNrOfSixAxisForceTorqueSensors() const
375 {
376  return 1;
377 }
378 
379 yarp::dev::MAS_status embObjFTsensor::getSixAxisForceTorqueSensorStatus(size_t sens_index) const
380 {
381  return yarp::dev::MAS_OK;
382 }
383 
384 bool embObjFTsensor::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const
385 {
386  name = GET_privData(mPriv).devicename;
387  return true;
388 }
389 
390 bool embObjFTsensor::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
391 {
392  frameName = GET_privData(mPriv).frameName;
393  return true;
394 }
395 
396 bool 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
cmd
Definition: dataTypes.h:30
#define GET_privData(x)
iethresType_t
Definition: IethResource.h:62
@ iethres_analogstrain
Definition: IethResource.h:66
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:43