iCub-main
CanBusFtSensor.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 // Copyright: (C) 2013 iCub Facility
4 // Authors: Marco Randazzo <marco.randazzo@iit.it> Valentina Vasco <valentina.vasco@iit.it>
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #include <CanBusFtSensor.h>
8 
9 #include <yarp/os/Time.h>
10 #include <yarp/os/LogStream.h>
11 #include <yarp/os/Log.h>
12 #include <iostream>
13 #include <string.h>
14 
15 const int CAN_DRIVER_BUFFER_SIZE=2047;
16 
17 using namespace std;
18 
19 bool CanBusFtSensor::open(yarp::os::Searchable& config)
20 {
21  bool correct=true;
22 
23  //debug
24  fprintf(stderr, "%s\n", config.toString().c_str());
25 
26  correct &= config.check("canbusDevice");
27  correct &= config.check("canDeviceNum");
28  correct &= config.check("canAddress");
29  correct &= config.check("format");
30  correct &= config.check("period");
31  correct &= config.check("channels");
32  correct &= config.check("physDevice");
33  correct &= config.check("sensorName");
34 
35  if (!correct)
36  {
37  yError() << "Insufficient parameters to CanBusFtSensor\n";
38  return false;
39  }
40 
41  if (!config.check("frameName"))
42  {
43  yWarning() << "frameName not found.. using default value unknown_frame_name";
44  }
45 
46  int period=config.find("period").asInt32();
47  setPeriod((double)period/1000.0);
48 
49  Property prop;
50 
51  prop.put("device", config.find("canbusDevice").asString().c_str());
52  prop.put("physDevice", config.find("physDevice").asString().c_str());
53  prop.put("canTxTimeout", 500);
54  prop.put("canRxTimeout", 500);
55  canDeviceNum = config.find("canDeviceNum").asInt32();
56  prop.put("canDeviceNum", canDeviceNum);
57  prop.put("canMyAddress", 0);
58  prop.put("canTxQueueSize", CAN_DRIVER_BUFFER_SIZE);
59  prop.put("canRxQueueSize", CAN_DRIVER_BUFFER_SIZE);
60  pCanBus=0;
61  pCanBufferFactory=0;
62 
63  //open the can driver
64  driver.open(prop);
65  if (!driver.isValid())
66  {
67  yError() << "Error opening PolyDriver check parameters";
68  return false;
69  }
70  driver.view(pCanBus);
71  if (!pCanBus)
72  {
73  yError() << "Error opening can device not available";
74  return false;
75  }
76  driver.view(pCanBufferFactory);
77  outBuffer=pCanBufferFactory->createBuffer(CAN_DRIVER_BUFFER_SIZE);
78  inBuffer=pCanBufferFactory->createBuffer(CAN_DRIVER_BUFFER_SIZE);
79 
80  //select the communication speed
81  pCanBus->canSetBaudRate(0); //default 1MB/s
82 
83 
84  //set the internal configuration
85  //this->isVirtualSensor = false;
86  this->boardId = config.find("canAddress").asInt32();
87  this->useCalibration = config.find("useCalibration").asInt32();
88  //this->SensorFullScale = config.find("fullScale").asInt32();
89  this->sensorName = config.find("sensorName").asString();
90  this->frameName = config.check("frameName", yarp::os::Value("unknown_frame_name")).asString();
91  unsigned int tmpFormat = config.find("format").asInt32();
92  if (tmpFormat == 8)
93  this->dataFormat = ANALOG_FORMAT_8_BIT;
94  else if (tmpFormat == 16)
95  this->dataFormat = ANALOG_FORMAT_16_BIT;
96  else
97  this->dataFormat = ANALOG_FORMAT_ERR;
98 
99  // Parse diagnostic information
100  if( config.check("diagnostic") && config.find("diagnostic").asInt32() == 1)
101  {
102  this->diagnostic = true;
103  }
104  else
105  {
106  this->diagnostic = false;
107  }
108 
109 
110  //open the can mask for the specific canDeviceId
111  for (int id=0; id<16; ++id)
112  {
113  pCanBus->canIdAdd(0x300+(boardId<<4)+id);
114  }
115  pCanBus->canIdAdd(0x200+boardId);
116  pCanBus->canIdAdd(0x200+(boardId<<4));
117 
118  //create the data vector:
119  this->channelsNum = config.find("channels").asInt32();
120  data.resize(channelsNum);
121  data.zero();
122  scaleFactor.resize(channelsNum);
123  scaleFactor.zero();
124 
125  //start the sensor broadcast
126  sensor_start(config);
127 
128  PeriodicThread::start();
129  return true;
130 }
131 
132 bool CanBusFtSensor::readFullScaleAnalog(int ch)
133 {
134  scaleFactor[ch]=1e-20;
135 
136  unsigned int canMessages=0;
137  unsigned id = 0x200 + boardId;
138  CanMessage &msg=outBuffer[0];
139  msg.setId(id);
140  msg.getData()[0]=0x18;
141  msg.getData()[1]=ch;
142  msg.setLen(2);
143  canMessages=0;
144  pCanBus->canWrite(outBuffer, 1, &canMessages);
145 
146  long int timeout=0;
147  bool full_scale_read=false;
148  do
149  {
150  unsigned int max_messages=CAN_DRIVER_BUFFER_SIZE;
151  unsigned int read_messages = 0;
152  bool b = pCanBus->canRead(inBuffer,max_messages,&read_messages,false);
153 
154  for (unsigned int i=0; i<read_messages; i++)
155  {
156  CanMessage& m= inBuffer[i];
157  unsigned int currId=m.getId();
158  if (currId==(0x0200 | boardId << 4))
159  if (m.getLen()==4 &&
160  m.getData()[0]==0x18 &&
161  m.getData()[1]==ch)
162  {
163  scaleFactor[ch]=m.getData()[2]<<8 | m.getData()[3];
164  full_scale_read=true;
165  break;
166  }
167  }
168  yarp::os::Time::delay(0.002);
169  timeout++;
170  }
171  while(timeout<32 && full_scale_read==false);
172 
173  if (full_scale_read==false)
174  {
175  yError("Trying to get fullscale data from sensor %d net [%d]: no answer received or message lost (ch:%d)\n", boardId, canDeviceNum, ch);
176  return false;
177  }
178 
179  return true;
180 }
181 
182 bool CanBusFtSensor::sensor_start(yarp::os::Searchable& analogConfig)
183 {
184  yTrace("Initializing analog device %s\n", analogConfig.find("deviceId").toString().c_str());
185 
186  unsigned int canMessages=0;
187  unsigned id = 0x200 + boardId;
188 
189  if (analogConfig.check("period"))
190  {
191  int period=analogConfig.find("period").asInt32();
192  CanMessage &msg=outBuffer[0];
193  msg.setId(id);
194  msg.getData()[0]=0x08;
195  msg.getData()[1]=period;
196  msg.setLen(2);
197  canMessages=0;
198  pCanBus->canWrite(outBuffer, 1, &canMessages);
199  yDebug("using broadcast period %d on device %s\n", period, analogConfig.find("deviceId").toString().c_str());
200  }
201 
202  //init message for mais board
203  if (channelsNum==16 && dataFormat==ANALOG_FORMAT_8_BIT)
204  {
205  CanMessage &msg=outBuffer[0];
206  msg.setId(id);
207  msg.getData()[0]=0x07;
208  msg.getData()[1]=0x00;
209  msg.setLen(2);
210  canMessages=0;
211  pCanBus->canWrite(outBuffer, 1, &canMessages);
212  }
213 
214  //init message for strain board
215  else if (channelsNum==6 && dataFormat==ANALOG_FORMAT_16_BIT)
216  {
217  //calibrated astrain board
218  if (useCalibration>0)
219  {
220  yDebug("Using internal calibration on device %s\n", analogConfig.find("deviceId").toString().c_str());
221  //get the full scale values from the strain board
222  for (int ch=0; ch<6; ch++)
223  {
224  bool b=false;
225  int attempts = 0;
226  while(attempts<15)
227  {
228  b = readFullScaleAnalog(ch);
229  if (b==true)
230  {
231  if (attempts>0) yWarning("Trying to get fullscale data from sensor: channel recovered (ch:%d)\n", ch);
232  break;
233  }
234  attempts++;
235  yarp::os::Time::delay(0.020);
236  }
237  if (attempts>=15)
238  {
239  yError("Trying to get fullscale data from sensor: all attempts failed (ch:%d)\n", ch);
240  }
241  }
242 
243  // debug messages
244  #if 1
245  fprintf(stderr, "Sensor Fullscale Id %#4X: ",boardId);
246  fprintf(stderr, " %f ", scaleFactor[0]);
247  fprintf(stderr, " %f ", scaleFactor[1]);
248  fprintf(stderr, " %f ", scaleFactor[2]);
249  fprintf(stderr, " %f ", scaleFactor[3]);
250  fprintf(stderr, " %f ", scaleFactor[4]);
251  fprintf(stderr, " %f ", scaleFactor[5]);
252  fprintf(stderr, " \n ");
253  #endif
254 
255  // start the board
256  CanMessage &msg=outBuffer[0];
257  msg.setId(id);
258  msg.getData()[0]=0x07;
259  if (useCalibration == 1) msg.getData()[1]=0x00;
260  if (useCalibration == 2) msg.getData()[1]=0x00;
261  if (useCalibration == 3) msg.getData()[1]=0x00;
262  msg.setLen(2);
263  canMessages=0;
264  pCanBus->canWrite(outBuffer, 1, &canMessages);
265  }
266  //not calibrated strain board (useCalibration = 0)
267  else
268  {
269  yInfo("Using uncalibrated raw data for device %s\n", analogConfig.find("deviceId").toString().c_str());
270  CanMessage &msg=outBuffer[0];
271  msg.setId(id);
272  msg.getData()[0]=0x07;
273  msg.getData()[1]=0x03;
274  msg.setLen(2);
275  canMessages=0;
276  pCanBus->canWrite(outBuffer, 1, &canMessages);
277  }
278  }
279  return true;
280 }
281 
282 bool CanBusFtSensor::sensor_stop()
283 {
284  unsigned int canMessages=0;
285  unsigned id = 0x200 + boardId;
286  CanMessage &msg=outBuffer[0];
287  msg.setId(id);
288  msg.getData()[0]=0x07;
289  msg.getData()[1]=0x01;
290  msg.setLen(2);
291  canMessages=0;
292  pCanBus->canWrite(outBuffer, 1, &canMessages);
293  return true;
294 }
295 
297 {
298  //stop the thread
300 
301  //stop the sensor
302  sensor_stop();
303 
304  //stop the driver
305  if (pCanBufferFactory)
306  {
307  pCanBufferFactory->destroyBuffer(inBuffer);
308  pCanBufferFactory->destroyBuffer(outBuffer);
309  }
310  driver.close();
311 
312  return true;
313 }
314 
315 int CanBusFtSensor::read(yarp::sig::Vector &out)
316 {
317  mtx.lock();
318  out=data;
319  mtx.unlock();
320 
321  if( this->diagnostic )
322  return status;
323  else
324  return IAnalogSensor::AS_OK;
325 }
326 
328 {
329  if( this->diagnostic )
330  return status;
331  else
332  return IAnalogSensor::AS_OK;
333 }
334 
336 {
337  return channelsNum;
338 }
339 
341 {
342  //NOT YET IMPLEMENTED
343  return 0;
344 }
345 
347 {
348  //NOT YET IMPLEMENTED
349  return 0;
350 }
351 
352 int CanBusFtSensor::calibrateSensor(const yarp::sig::Vector& v)
353 {
354  //NOT YET IMPLEMENTED
355  return 0;
356 }
357 
359 {
360  //NOT YET IMPLEMENTED
361  return 0;
362 }
363 
365 {
366  return true;
367 }
368 
369 bool CanBusFtSensor::decode16(const unsigned char *msg, int msg_id, double *data)
370 {
371  const char groupId=(msg_id & 0x00f);
372  int baseIndex=0;
373  {
374  switch (groupId)
375  {
376  case 0xA:
377  {
378  for(int k=0;k<3;k++)
379  {
380  data[k]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
381  if (useCalibration>0)
382  {
383  data[k]=data[k]*scaleFactor[k]/float(0x8000);
384  }
385  }
386  }
387  break;
388  case 0xB:
389  {
390  for(int k=0;k<3;k++)
391  {
392  data[k+3]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
393  if (useCalibration>0)
394  {
395  data[k+3]=data[k+3]*scaleFactor[k+3]/float(0x8000);
396  }
397  }
398  }
399  break;
400  case 0xC:
401  {} //skip these, they are not for us
402  break;
403  case 0xD:
404  {} //skip these, they are not for us
405  break;
406  default:
407  yWarning("Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
408  return false;
409  break;
410  }
411  //@@@DEBUG ONLY
412  //fprintf(stderr, " %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6]);
413  }
414 
415  return true;
416 }
417 
418 bool CanBusFtSensor::decode8(const unsigned char *msg, int msg_id, double *data)
419 {
420  const char groupId=(msg_id & 0x00f);
421  int baseIndex=0;
422  {
423  switch (groupId)
424  {
425  case 0xC:
426  {
427  for(int k=0;k<=6;k++)
428  data[k]=msg[k];
429  }
430  break;
431  case 0xD:
432  {
433  for(int k=0;k<=7;k++)
434  data[7+k]=msg[k];
435  }
436  break;
437  case 0xA:
438  {} //skip these, they are not for us
439  break;
440  case 0xB:
441  {} //skip these, they are not for us
442  break;
443  default:
444  yWarning("CanBusFtSensor got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
445  return false;
446  break;
447  }
448  //@@@DEBUG ONLY
449  //fprintf(stderr, " %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6]);
450  }
451  return true;
452 }
453 
455 {
456  lock_guard<mutex> lck(mtx);
457 
458  unsigned int canMessages=0;
459  bool ret=true; //return true by default
460 
461  bool res=pCanBus->canRead(inBuffer,CAN_DRIVER_BUFFER_SIZE,&canMessages);
462  if (!res)
463  {
464  yError()<<"CanBusFtSensor canRead failed\n";
465  }
466 
467  double timeNow=Time::now();
468  for (unsigned int i=0; i<canMessages; i++)
469  {
470  CanMessage &msg=inBuffer[i];
471 
472  unsigned int msgid = msg.getId();
473  unsigned char *buff = msg.getData();
474  unsigned int len = msg.getLen();
475  unsigned int id = (msgid & 0x00f0)>>4;
476  const char type = ((msgid&0x700)>>8);
477 
478  //parse data here
479  status=IAnalogSensor::AS_OK;
480 
481  if (type==0x03) //analog data
482  {
483  if (id==boardId)
484  {
485  timeStamp=Time::now();
486  switch (dataFormat)
487  {
488  case ANALOG_FORMAT_8_BIT:
489  ret=decode8(buff, msgid, data.data());
490  status=IAnalogSensor::AS_OK;
491  break;
492  case ANALOG_FORMAT_16_BIT:
493  if (len==6)
494  {
495  ret=decode16(buff, msgid, data.data());
496  status=IAnalogSensor::AS_OK;
497  }
498  else
499  {
500  if ((len==7) && (buff[6] != 0)) // changed from "== 1" in order to maintain compatibility with extra overflow information in byte 6.
501  {
502  status=IAnalogSensor::AS_OVF;
503  }
504  else
505  {
506  status=IAnalogSensor::AS_ERROR;
507  }
508  ret=decode16(buff, msgid, data.data());
509  }
510  break;
511  default:
512  status=IAnalogSensor::AS_ERROR;
513  ret=false;
514  }
515  }
516  }
517  }
518 
519  //if 100ms have passed since the last received message
520  if (timeStamp+0.1<timeNow)
521  {
522  status=IAnalogSensor::AS_TIMEOUT;
523  }
524 }
525 
527 {
528  yTrace("CanBusVirtualAnalogSensor Thread released\n");
529 }
530 
531 //------------------------- ISixAxisForceTorqueSensors -------------------------
533 {
534  return 1;
535 }
536 
537 yarp::dev::MAS_status CanBusFtSensor::getSixAxisForceTorqueSensorStatus(size_t sens_index) const
538 {
539  return yarp::dev::MAS_OK;
540 }
541 
542 bool CanBusFtSensor::getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const
543 {
544  name = this->sensorName;
545  return true;
546 }
547 
548 bool CanBusFtSensor::getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const
549 {
550  frameName = this->frameName;
551  return true;
552 }
553 
554 bool CanBusFtSensor::getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
555 {
556  std::lock_guard<std::mutex> lck(mtx);
557  out = data;
558  timestamp = timeStamp;
559 
560  return true;
561 }
562 
const int CAN_DRIVER_BUFFER_SIZE
@ data
virtual bool open(yarp::os::Searchable &config)
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sens_index) const override
virtual bool getSixAxisForceTorqueSensorFrameName(size_t sens_index, std::string &frameName) const override
virtual void run()
virtual size_t getNrOfSixAxisForceTorqueSensors() const override
virtual int read(yarp::sig::Vector &out)
virtual int calibrateChannel(int ch, double v)
virtual int getState(int ch)
virtual bool getSixAxisForceTorqueSensorName(size_t sens_index, std::string &name) const override
virtual int getChannels()
virtual bool threadInit()
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
virtual void threadRelease()
virtual bool close()
static int stop
Definition: iCub_Sim.cpp:41
static int v
Definition: iCub_Sim.cpp:42
fprintf(fid,'\n')
out
Definition: sine.m:8