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  bool ret = GET_privData(mPriv).fillTemperatureEthServiceInfo(serviceConfig.ethservice, servparamtemp);
115  if(!ret)
116  return false;
117 
118  const eOmn_serv_parameter_t* servparamtemp_ptr = &servparamtemp;
119  const eOmn_serv_parameter_t* servparamstrain = &serviceConfig.ethservice;
120 
121 #else
122  const eOmn_serv_parameter_t* servparamstrain = NULL;
123  const eOmn_serv_parameter_t* servparamtemp_ptr = NULL;
124 #endif
125 
126  if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_strain, servparamstrain, 5.0))
127  {
128  yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()";
129  cleanup();
130  return false;
131  }
132 
133  if(false == GET_privData(mPriv).res->serviceVerifyActivate(eomn_serv_category_temperatures, servparamtemp_ptr, 5.0))
134  {
135  yError() << getBoardInfo() << "open() has an error in call of ethResources::serviceVerifyActivate()";
136  cleanup();
137  return false;
138  }
139 
140  // we always prepare the fullscales.
141  if(false == GET_privData(mPriv).fillScaleFactor(serviceConfig))
142  {
143  yError() << getBoardInfo() << "open() has failed in calling embObjFTsensor::fillScaleFactor()";
144  return false;
145  }
146 
147  if(false == GET_privData(mPriv).sendConfig2Strain(serviceConfig))
148  {
149  cleanup();
150  return false;
151  }
152 
153  if(false == GET_privData(mPriv).initRegulars(serviceConfig))
154  {
155  cleanup();
156  return false;
157  }
158 
159 
160  if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_strain))
161  {
162  yError() << getBoardInfo() << "open() fails to start service strain";
163  cleanup();
164  return false;
165  }
166  else
167  {
168  if(GET_privData(mPriv).isVerbose())
169  {
170  yDebug() << getBoardInfo() << "open() correctly starts as service strain";
171  }
172  }
173 
174  if(false == GET_privData(mPriv).res->serviceStart(eomn_serv_category_temperatures))
175  {
176  yError() << getBoardInfo() << "open() fails to start service temperature";
177  cleanup();
178  return false;
179  }
180  else
181  {
182  if(GET_privData(mPriv).isVerbose())
183  {
184  yDebug() << getBoardInfo() << "open() correctly starts as service temperature";
185  }
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  GET_privData(mPriv).setOpen(true);
197  return true;
198 }
199 
200 bool embObjFTsensor::close()
201 {
202  cleanup();
203  return true;
204 }
205 
206 void embObjFTsensor::cleanup(void)
207 {
208 
209  // disable temperature
210  enableTemperatureTransmission(false);
211 
212  GET_privData(mPriv).cleanup(static_cast <eth::IethResource*> (this));
213 }
214 
215 
216 
217 //------------------------------- IethResource --------------------------------
218 bool embObjFTsensor::update(eOprotID32_t id32, double timestamp, void* rxdata)
219 {
220  bool ret = false;
221 
222  if(false == GET_privData(mPriv).isOpen())
223  {
224  return ret;;
225  }
226  eOprotEntity_t entity = eoprot_ID2entity(id32);
227 
228  switch(entity)
229  {
230  case eoas_entity_strain:
231  {
232  ret = updateStrainValues(id32, timestamp, rxdata);
233  }break;
234 
235  case eoas_entity_temperature:
236  {
237  ret = updateTemperatureValues(id32, timestamp, rxdata);
238  }break;
239  default:
240  {
241  ret = false;
242  yError() << getBoardInfo() << "update() failed ";
243  }
244  };
245  return ret;
246 }
247 
248 bool embObjFTsensor::updateStrainValues(eOprotID32_t id32, double timestamp, void* rxdata)
249 {
250  id32 = id32;
251  timestamp = timestamp;
252 
253 
254  // called by feat_manage_analogsensors_data() which is called by:
255  // eoprot_fun_UPDT_as_strain_status_calibratedvalues() or eoprot_fun_UPDT_as_strain_status_uncalibratedvalues()
256  // the void* parameter inside this function is a eOas_arrayofupto12bytes_t*
257  // and can be treated as a EOarray
258 
259  EOarray *array = (EOarray*)rxdata;
260  uint8_t size = eo_array_Size(array);
261  uint8_t itemsize = eo_array_ItemSize(array); // marco.accame: must be 2, as the code after uses this convention
262  if((0 == size) || (2 != itemsize))
263  {
264  return false;
265  }
266 
267  // lock analogdata
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++)
271  {
272  // Get the kth element of the array as a 2 bytes msg
273  char* tmp = (char*) eo_array_At(array, k);
274  // marco.accame: i am nervous about iterating for strain_Channels instead of size of array....
275  // thus i add a protection. if k goes beyond size of array, eo_array_At() returns NULL.
276  if(NULL != tmp)
277  {
278  uint8_t msg[2] = {0};
279  memcpy(msg, tmp, 2);
280  // Got from canBusMotionControl
281  GET_privData(mPriv).analogdata[k] = (short)( ( (((unsigned short)(msg[1]))<<8)+msg[0]) - (unsigned short) (0x8000) );
282 
283  if(true == GET_privData(mPriv).useCalibValues)
284  {
285  GET_privData(mPriv).analogdata[k] = GET_privData(mPriv).analogdata[k]* GET_privData(mPriv).scaleFactor[k]/float(0x8000);
286  }
287  }
288  }
289 
290  return true;
291 }
292 
293 bool embObjFTsensor::updateTemperatureValues(eOprotID32_t id32, double timestamp, void* rxdata)
294 {
295  eOas_temperature_status_t *temp_st = (eOas_temperature_status_t *)rxdata;
296 
297  EOconstarray* arrayofvalues = eo_constarray_Load(reinterpret_cast<const EOarray*>(&(temp_st->arrayofdata)));
298 
299  uint8_t numofIntem2update = eo_constarray_Size(arrayofvalues);
300 
301  if(numofIntem2update>1)
302  yError() << getBoardInfo() << "updateTemperature: I expect 1 item, but I received " << numofIntem2update ;
303 
304  for(int i=0; i<numofIntem2update; i++)
305  {
306  eOas_temperature_data_t *data = (eOas_temperature_data_t*) eo_constarray_At(arrayofvalues, i);
307  if(data == NULL)
308  {
309  yError() << getBoardInfo() << "update(): I have to update " << numofIntem2update << "items, but the " << i << "-th item is null.";
310  continue;
311  //NOTE: I signal this strange situation with an arror for debug porpouse...maybe we can convert in in warning when the device is stable....
312  }
313  else
314  {
315  //yDebug() << "embObjFTStrain" << getBoardInfo() << ": val "<< i<< "/" << numofIntem2update << ": value=" << data->value << ", time=" << data->timestamp;
316  std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
317  GET_privData(mPriv).lastTemperature = static_cast<float>(data->value);
318  GET_privData(mPriv).timestampTemperature = yarp::os::Time::now();
319  }
320  }
321  return true;
322 }
323 
324 
325 // ----------------------------- yarp::dev::IAnalogSensor --------------------------------------
326 
327 
332 int embObjFTsensor::read(yarp::sig::Vector &out)
333 {
334  // This method gives analogdata to the analogServer
335 
336  if(false == GET_privData(mPriv).isOpen())
337  {
338  return false;
339  }
340 
341  std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
342 
343  out.resize(GET_privData(mPriv).analogdata.size());
344  for (size_t k = 0; k<GET_privData(mPriv).analogdata.size(); k++)
345  {
346  out[k] = GET_privData(mPriv).analogdata[k] + GET_privData(mPriv).offset[k];
347  }
348 
349  return IAnalogSensor::AS_OK;;
350 }
351 
352 int embObjFTsensor::getState(int ch)
353 {
354  return AS_OK;
355 }
356 
357 
358 int embObjFTsensor::getChannels()
359 {
360  return GET_privData(mPriv).strain_Channels;
361 }
362 
363 
364 int embObjFTsensor::calibrateSensor()
365 {
366  std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
367  for (size_t i = 0; i < GET_privData(mPriv).analogdata.size(); i++)
368  {
369  GET_privData(mPriv).offset[i] = - GET_privData(mPriv).analogdata[i];
370  }
371  return AS_OK;
372 }
373 
374 int embObjFTsensor::calibrateSensor(const yarp::sig::Vector& value)
375 {
376  return AS_OK;
377 }
378 
379 int embObjFTsensor::calibrateChannel(int ch)
380 {
381  return AS_OK;
382 }
383 
384 int embObjFTsensor::calibrateChannel(int ch, double v)
385 {
386  return AS_OK;
387 }
388 
389 eth::iethresType_t embObjFTsensor::type()
390 {
392 }
393 
394 
395 
396 // ---------------------- ITemperatureSensors --------------------------------------------------------
397 size_t embObjFTsensor::getNrOfTemperatureSensors() const
398 {
399  return 1;
400 }
401 
402 yarp::dev::MAS_status embObjFTsensor::getTemperatureSensorStatus(size_t sens_index) const
403 {
404  return yarp::dev::MAS_OK;
405 }
406 
407 bool embObjFTsensor::getTemperatureSensorName(size_t sens_index, std::string &name) const
408 {
409  name = GET_privData(mPriv).devicename;
410  return true;
411 }
412 
413 bool embObjFTsensor::getTemperatureSensorFrameName(size_t sens_index, std::string &frameName) const
414 {
415  frameName = GET_privData(mPriv).frameName;
416  return true;
417 }
418 
419 bool embObjFTsensor::getTemperatureSensorMeasure(size_t sens_index, double& out, double& timestamp) const
420 {
421  std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
422  out = GET_privData(mPriv).lastTemperature/10.0; //I need to convert from tenths of degree centigrade to degree centigrade
423  timestamp = GET_privData(mPriv).timestampTemperature;
424  return true;
425 }
426 
427 bool embObjFTsensor::getTemperatureSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
428 {
429  std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
430  out.resize(1);
431  out[0] = GET_privData(mPriv).lastTemperature/10.0; //I need to convert from tenths of degree centigrade to degree centigrade
432  timestamp = GET_privData(mPriv).timestampTemperature;
433  return true;
434 }
435 
436 //------------------------- ISixAxisForceTorqueSensors -------------------------
437 
438 size_t embObjFTsensor::getNrOfSixAxisForceTorqueSensors() const
439 {
440  return 1;
441 }
442 
443 yarp::dev::MAS_status embObjFTsensor::getSixAxisForceTorqueSensorStatus(size_t sens_index) const
444 {
445  return yarp::dev::MAS_OK;
446 }
447 
448 bool embObjFTsensor::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const
449 {
450  name = GET_privData(mPriv).devicename;
451  return true;
452 }
453 
454 bool embObjFTsensor::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
455 {
456  frameName = GET_privData(mPriv).frameName;
457  return true;
458 }
459 
460 bool embObjFTsensor::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
461 {
462  if(false == GET_privData(mPriv).isOpen())
463  {
464  return false;
465  }
466 
467  std::lock_guard<std::mutex> lck(GET_privData(mPriv).mtx);
468 
469  out.resize(GET_privData(mPriv).analogdata.size());
470  for (size_t k = 0; k<GET_privData(mPriv).analogdata.size(); k++)
471  {
472  out[k] = GET_privData(mPriv).analogdata[k] + GET_privData(mPriv).offset[k];
473  }
474 
475  timestamp = GET_privData(mPriv).timestampAnalogdata;
476 
477  return true;
478 }
479 
480 // eof
481 
@ data
cmd
Definition: dataTypes.h:30
#define GET_privData(x)
static int v
Definition: iCub_Sim.cpp:42
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92
iethresType_t
Definition: IethResource.h:61
@ iethres_analogstrain
Definition: IethResource.h:65
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8
eOmn_serv_parameter_t ethservice
Definition: serviceParser.h:43