iCub-main
CanBusVirtualAnalogSensor.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 
8 
9 #include <yarp/os/Time.h>
10 #include <yarp/os/Log.h>
11 #include <iostream>
12 #include <string.h>
13 #include <yarp/os/Log.h>
14 #include <yarp/os/LogStream.h>
15 
16 const int CAN_DRIVER_BUFFER_SIZE=2047;
17 
18 using namespace std;
19 
20 bool CanBusVirtualAnalogSensor::open(yarp::os::Searchable& config)
21 {
22  bool correct=true;
23 
24  //debug
25  fprintf(stderr, "%s\n", config.toString().c_str());
26 
27  correct &= config.check("canbusDevice");
28  correct &= config.check("canDeviceNum");
29  correct &= config.check("canAddress");
30  correct &= config.check("format");
31  correct &= config.check("period");
32  correct &= config.check("channels");
33  correct &= config.check("fullScale");
34 
35  if (!correct)
36  {
37  yError()<<"insufficient parameters to CanBusVirtualAnalogSensor\n";
38  return false;
39  }
40 
41  int period=config.find("period").asInt32();
42  setPeriod((double)period/1000.0);
43 
44  Property prop;
45 
46  prop.put("device", config.find("canbusDevice").asString().c_str());
47  prop.put("physDevice", config.find("physDevice").asString().c_str());
48  prop.put("canTxTimeout", 500);
49  prop.put("canRxTimeout", 500);
50  prop.put("canDeviceNum", config.find("canDeviceNum").asInt32());
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\n");
62  return false;
63  }
64  driver.view(pCanBus);
65  if (!pCanBus)
66  {
67  yError ("Error opening can device not available\n");
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->canId = config.find("canDeviceNum").asInt32();
82  this->channelsNum = config.find("channels").asInt32();
83  this->useCalibration = (config.find("useCalibration").asInt32()==1);
84  unsigned int tmpFormat = config.find("format").asInt32();
85  if (tmpFormat == 8)
86  this->dataFormat = ANALOG_FORMAT_8_BIT;
87  else if (tmpFormat == 16)
88  this->dataFormat = ANALOG_FORMAT_16_BIT;
89  else
90  this->dataFormat = ANALOG_FORMAT_ERR;
91 
92  //open the can mask for the specific canDeviceId
93  for (int id=0; id<16; ++id)
94  {
95  pCanBus->canIdAdd(0x300+(boardId<<4)+id);
96  }
97  pCanBus->canIdAdd(0x200+boardId);
98  pCanBus->canIdAdd(0x200+(boardId<<4));
99 
100  //create the data vector:
101  int chan=config.find("channels").asInt32();
102  data.resize(channelsNum);
103  Bottle fullScaleTmp = config.findGroup("fullScale");
104  this->scaleFactor.resize(channelsNum);
105  for (unsigned int i=0; i<channelsNum; i++)
106  {
107  double tmp = fullScaleTmp.get(i+1).asFloat64();
108  this->scaleFactor[i] = tmp;
109  }
110 
111  //start the sensor broadcast
112  sensor_start(config);
113 
114  PeriodicThread::start();
115  return true;
116 }
117 
119 {
120  //NOT YET IMPLEMENTED
121  return false;
122 }
123 
125 {
126  int fakeId = 0;
127  short int val[6] = {0,0,0,0,0,0};
128  static int count_saturation = 0;
129 
130  static double curr_time = Time::now();
131  for (int i=0; i<6; i++)
132  {
133  double fullScale = scaleFactor[i];
134  if (dval[i] > fullScale)
135  {
136  if (Time::now() - curr_time > 2)
137  {
138 // fprintf(stderr, "**** PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", deviceName.c_str(), i, dval[i], count_saturation);
139  yWarning("VIRTUAL FT SENSOR can:%d id:%d SATURATED CH:%d : %+4.4f COUNT: %d \n",this->canId,this->boardId, i, dval[i], count_saturation);
140  curr_time = Time::now();
141  }
142  dval[i] = fullScale;
143  count_saturation++;
144  }
145  else if (dval[i] < -fullScale)
146  {
147  if (Time::now() - curr_time > 2)
148  {
149 // fprintf(stderr, "**** PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", deviceName.c_str(), i, dval[i], count_saturation);
150  yWarning("VIRTUAL FT SENSOR can:%d id:%d SATURATED CH:%d : %+4.4f COUNT: %d \n",this->canId, this->boardId, i, dval[i], count_saturation);
151  curr_time = Time::now();
152  }
153  dval[i] = -fullScale;
154  count_saturation++;
155  }
156  val[i] = (short int)(dval[i] / fullScale * 0x7fff)+0x8000; //check this!
157  }
158 
159  unsigned int canMessages=0;
160 
161  CanMessage &msg0=outBuffer[0];
162  canMessages=0;
163  fakeId = 0x300 + (boardId<<4)+ 0x0A;
164  msg0.setId(fakeId);
165  msg0.getData()[1]=(val[0] >> 8) & 0xFF;
166  msg0.getData()[0]= val[0] & 0xFF;
167  msg0.getData()[3]=(val[1] >> 8) & 0xFF;
168  msg0.getData()[2]= val[1] & 0xFF;
169  msg0.getData()[5]=(val[2] >> 8) & 0xFF;
170  msg0.getData()[4]= val[2] & 0xFF;
171  msg0.setLen(6);
172  pCanBus->canWrite(outBuffer, 1, &canMessages);
173 
174  CanMessage &msg1=outBuffer[0];
175  canMessages=0;
176  fakeId = 0x300 + (boardId<<4)+ 0x0B;
177  msg1.setId(fakeId);
178  msg1.getData()[1]=(val[3] >> 8) & 0xFF;
179  msg1.getData()[0]= val[3] & 0xFF;
180  msg1.getData()[3]=(val[4] >> 8) & 0xFF;
181  msg1.getData()[2]= val[4] & 0xFF;
182  msg1.getData()[5]=(val[5] >> 8) & 0xFF;
183  msg1.getData()[4]= val[5] & 0xFF;
184  msg1.setLen(6);
185  canMessages=0;
186  pCanBus->canWrite(outBuffer, 1, &canMessages);
187 
188  return true;
189 }
190 
191 bool CanBusVirtualAnalogSensor::readFullScaleAnalog(int ch)
192 {
193  scaleFactor[ch]=1e-20;
194 
195  unsigned int canMessages=0;
196  unsigned id = 0x200 + boardId;
197  CanMessage &msg=outBuffer[0];
198  msg.setId(id);
199  msg.getData()[0]=0x18;
200  msg.getData()[1]=ch;
201  msg.setLen(2);
202  canMessages=0;
203  pCanBus->canWrite(outBuffer, 1, &canMessages);
204 
205  long int timeout=0;
206  bool full_scale_read=false;
207  do
208  {
209  unsigned int max_messages=CAN_DRIVER_BUFFER_SIZE;
210  unsigned int read_messages = 0;
211  bool b = pCanBus->canRead(inBuffer,max_messages,&read_messages,false);
212 
213  for (unsigned int i=0; i<read_messages; i++)
214  {
215  CanMessage& m= inBuffer[i];
216  unsigned int currId=m.getId();
217  if (currId==(0x0200 | boardId << 4))
218  if (m.getLen()==4 &&
219  m.getData()[0]==0x18 &&
220  m.getData()[1]==ch)
221  {
222  scaleFactor[ch]=m.getData()[2]<<8 | m.getData()[3];
223  full_scale_read=true;
224  break;
225  }
226  }
227  yarp::os::Time::delay(0.002);
228  timeout++;
229  }
230  while(timeout<32 && full_scale_read==false);
231 
232  if (full_scale_read==false)
233  {
234  yError("Trying to get fullscale data from sensor 0x%X: no answer received or message lost (ch:%d)\n", boardId, ch);
235  return false;
236  }
237 
238  return true;
239 }
240 
241 bool CanBusVirtualAnalogSensor::sensor_start(yarp::os::Searchable& analogConfig)
242 {
243  fprintf(stderr, "--> Initializing analog device %s\n", analogConfig.find("deviceId").toString().c_str());
244  /*
245  unsigned int canMessages=0;
246  unsigned id = 0x200 + boardId;
247 
248  if (analogConfig.check("period"))
249  {
250  int period=analogConfig.find("period").asInt32();
251  CanMessage &msg=outBuffer[0];
252  msg.setId(id);
253  msg.getData()[0]=0x08;
254  msg.getData()[1]=period;
255  msg.setLen(2);
256  canMessages=0;
257  pCanBus->canWrite(outBuffer, 1, &canMessages);
258  }
259 
260  //init message for mais board
261  if (channelsNum==16 && dataFormat==ANALOG_FORMAT_8_BIT)
262  {
263  CanMessage &msg=outBuffer[0];
264  msg.setId(id);
265  msg.getData()[0]=0x07;
266  msg.getData()[1]=0x00;
267  msg.setLen(2);
268  canMessages=0;
269  pCanBus->canWrite(outBuffer, 1, &canMessages);
270  }
271 
272  //init message for strain board
273  else if (channelsNum==6 && dataFormat==ANALOG_FORMAT_16_BIT)
274  {
275  //calibrated astrain board
276  if (useCalibration)
277  {
278  //get the full scale values from the strain board
279  for (int ch=0; ch<6; ch++)
280  {
281  bool b=false;
282  int attempts = 0;
283  while(attempts<15)
284  {
285  b = readFullScaleAnalog(ch);
286  if (b==true)
287  {
288  if (attempts>0) fprintf(stderr, "*** WARNING: Trying to get fullscale data from sensor 0x%X: channel recovered (ch:%d)\n", boardId, ch);
289  break;
290  }
291  attempts++;
292  yarp::os::Time::delay(0.020);
293  }
294  if (attempts>=15)
295  {
296  yError("Trying to get fullscale data from sensor 0x%X: all attempts failed (ch:%d)\n", boardId, ch);
297  }
298  }
299 
300  // debug messages
301  #if 1
302  fprintf(stderr, "Sensor Fullscale Id %#4X: ",boardId);
303  fprintf(stderr, " %f ", scaleFactor[0]);
304  fprintf(stderr, " %f ", scaleFactor[1]);
305  fprintf(stderr, " %f ", scaleFactor[2]);
306  fprintf(stderr, " %f ", scaleFactor[3]);
307  fprintf(stderr, " %f ", scaleFactor[4]);
308  fprintf(stderr, " %f ", scaleFactor[5]);
309  fprintf(stderr, " \n ");
310  #endif
311 
312  // start the board
313  CanMessage &msg=outBuffer[0];
314  msg.setId(id);
315  msg.getData()[0]=0x07;
316  msg.getData()[1]=0x00;
317  msg.setLen(2);
318  canMessages=0;
319  pCanBus->canWrite(outBuffer, 1, &canMessages);
320  }
321  //not calibrated strain board
322  else
323  {
324  CanMessage &msg=outBuffer[0];
325  msg.setId(id);
326  msg.getData()[0]=0x07;
327  msg.getData()[1]=0x03;
328  msg.setLen(2);
329  canMessages=0;
330  pCanBus->canWrite(outBuffer, 1, &canMessages);
331  }
332  }*/
333  return true;
334 }
335 
336 bool CanBusVirtualAnalogSensor::sensor_stop()
337 {
338  unsigned int canMessages=0;
339  unsigned id = 0x200 + boardId;
340  CanMessage &msg=outBuffer[0];
341  msg.setId(id);
342  msg.getData()[0]=0x07;
343  msg.getData()[1]=0x01;
344  msg.setLen(2);
345  canMessages=0;
346  pCanBus->canWrite(outBuffer, 1, &canMessages);
347  return true;
348 }
349 
351 {
352  //stop the thread
354 
355  //stop the sensor
356  sensor_stop();
357 
358  //stop the driver
359  if (pCanBufferFactory)
360  {
361  pCanBufferFactory->destroyBuffer(inBuffer);
362  pCanBufferFactory->destroyBuffer(outBuffer);
363  }
364  driver.close();
365 
366  return true;
367 }
368 
370 {
371  return yarp::dev::VAS_status::VAS_OK;
372 }
373 
375 {
376  return channelsNum;
377 }
378 
380 {
381  return true;
382 }
383 
384 bool CanBusVirtualAnalogSensor::decode16(const unsigned char *msg, int msg_id, double *data)
385 {
386  const char groupId=(msg_id & 0x00f);
387  int baseIndex=0;
388  {
389  switch (groupId)
390  {
391  case 0xA:
392  {
393  for(int k=0;k<3;k++)
394  {
395  data[k]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
396  if (useCalibration==1)
397  {
398  data[k]=data[k]*scaleFactor[k]/float(0x8000);
399  }
400  }
401  }
402  break;
403  case 0xB:
404  {
405  for(int k=0;k<3;k++)
406  {
407  data[k+3]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
408  if (useCalibration==1)
409  {
410  data[k+3]=data[k+3]*scaleFactor[k+3]/float(0x8000);
411  }
412  }
413  }
414  break;
415  case 0xC:
416  {} //skip these, they are not for us
417  break;
418  case 0xD:
419  {} //skip these, they are not for us
420  break;
421  default:
422  fprintf(stderr, "Warning, got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
423  return false;
424  break;
425  }
426  //@@@DEBUG ONLY
427  //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]);
428  }
429 
430  return true;
431 }
432 
433 bool CanBusVirtualAnalogSensor::decode8(const unsigned char *msg, int msg_id, double *data)
434 {
435  const char groupId=(msg_id & 0x00f);
436  int baseIndex=0;
437  {
438  switch (groupId)
439  {
440  case 0xC:
441  {
442  for(int k=0;k<=6;k++)
443  data[k]=msg[k];
444  }
445  break;
446  case 0xD:
447  {
448  for(int k=0;k<=7;k++)
449  data[7+k]=msg[k];
450  }
451  break;
452  case 0xA:
453  {} //skip these, they are not for us
454  break;
455  case 0xB:
456  {} //skip these, they are not for us
457  break;
458  default:
459  fprintf(stderr, "Warning, got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
460  return false;
461  break;
462  }
463  //@@@DEBUG ONLY
464  //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]);
465  }
466  return true;
467 }
468 
470 {
471  std::lock_guard<std::mutex> lck(mtx);
472 
473  unsigned int canMessages=0;
474  bool ret=true; //return true by default
475 
476  bool res=pCanBus->canRead(inBuffer,CAN_DRIVER_BUFFER_SIZE,&canMessages);
477  if (!res)
478  {
479  std::cerr<<"canRead failed\n";
480  }
481 
482  double timeNow=Time::now();
483  for (unsigned int i=0; i<canMessages; i++)
484  {
485  CanMessage &msg=inBuffer[i];
486 
487  unsigned int msgid = msg.getId();
488  unsigned char *buff = msg.getData();
489  unsigned int len = msg.getLen();
490  const char type = ((msgid&0x700)>>8);
491 
492  //parse data here
493  status=yarp::dev::VAS_status::VAS_OK;
494 
495  if (type==0x02) //configuration command
496  {
497  unsigned int id = (msgid & 0x00f);
498  if (id==boardId)
499  {
500  switch (buff[0])
501  {
502  case 0x18:
503  unsigned int channel = buff[1];
504  //send the fullscale data
505  unsigned int canMessages=0;
506  unsigned id = 0x200 + (boardId << 4);
507  CanMessage &msg=outBuffer[0];
508  msg.setId(id);
509  int tmp_fullscale = (int) scaleFactor[channel];
510  unsigned char h_scale = (tmp_fullscale >> 8) & 0xff;
511  unsigned char l_scale = tmp_fullscale & 0xff;
512  msg.getData()[0]=0x18;
513  msg.getData()[1]=channel;
514  msg.getData()[2]=h_scale;
515  msg.getData()[3]=l_scale;
516  msg.setLen(4);
517  canMessages=0;
518  pCanBus->canWrite(outBuffer, 1, &canMessages);
519  break;
520  }
521  }
522  }
523  else if (type==0x03) //analog data
524  {
525  unsigned int id = (msgid & 0x00f0)>>4;
526  if (id==boardId)
527  {
528  timeStamp=Time::now();
529  switch (dataFormat)
530  {
531  case ANALOG_FORMAT_8_BIT:
532  ret=decode8(buff, msgid, data.data());
533  status=yarp::dev::VAS_status::VAS_OK;
534  break;
535  case ANALOG_FORMAT_16_BIT:
536  if (len==6)
537  {
538  ret=decode16(buff, msgid, data.data());
539  status=yarp::dev::VAS_status::VAS_OK;
540  }
541  else
542  {
543  if (len==7 && buff[6] == 1)
544  {
545  status= yarp::dev::VAS_status::VAS_OVF;
546  }
547  else
548  {
549  status= yarp::dev::VAS_status::VAS_ERROR;
550  }
551  ret=decode16(buff, msgid, data.data());
552  }
553  break;
554  default:
555  status=yarp::dev::VAS_status::VAS_ERROR;
556  ret=false;
557  }
558  }
559  }
560  }
561 
562  //if 100ms have passed since the last received message
563  if (timeStamp+0.1<timeNow)
564  {
565  status= yarp::dev::VAS_status::VAS_TIMEOUT;
566  }
567 }
568 
570 {
571  yTrace("CanBusVirtualAnalogSensor Thread released\n");
572 }
573 
const int CAN_DRIVER_BUFFER_SIZE
@ data
virtual yarp::dev::VAS_status getVirtualAnalogSensorStatus(int ch)
virtual bool updateVirtualAnalogSensorMeasure(int ch, double &measure)
virtual bool open(yarp::os::Searchable &config)
static int stop
Definition: iCub_Sim.cpp:41
fprintf(fid,'\n')