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