iCub-main
CanBusSkin.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) 2010 RobotCub Consortium
4 // Authors: Lorenzo Natale, Francesco Giovannini
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #include <CanBusSkin.h>
8 
9 #include <iCubCanProtocol.h>
10 #include <yarp/os/Time.h>
11 #include <yarp/os/Log.h>
12 #include <yarp/os/LogStream.h>
13 
14 #include <iostream>
15 #include <yarp/os/LogStream.h>
16 #include <deque>
17 
18 
19 const int CAN_DRIVER_BUFFER_SIZE = 2047;
20 
21 #define SPECIAL_TRIANGLE_CFG_MAX_NUM 20
22 #define TRIANGLE_MAX_NUM_IN_BOARD 16
23 
24 #define SKIN_DEBUG 0
25 
26 using namespace std;
27 using namespace iCub::skin::diagnostics;
28 using yarp::os::Bottle;
29 using yarp::os::Property;
30 using yarp::os::Value;
31 using yarp::dev::CanMessage;
32 
33 
34 CanBusSkin::CanBusSkin() : PeriodicThread(0.02),
35  _verbose(false),
36  _isDiagnosticPresent(false)
37 { }
38 
39 
40 bool CanBusSkin::open(yarp::os::Searchable& config)
41 {
42  bool ret=true;
43 #if SKIN_DEBUG
44  fprintf(stderr, "%s\n", config.toString().c_str());
45 #endif
46 
47  bool correct=true;
48  // Mandatory parameters for all skin patches
49  correct &= config.check("canbusDevice");
50  correct &= config.check("canDeviceNum");
51  correct &= config.check("skinCanIds");
52  correct &= config.check("period");
53 
54  if (!correct)
55  {
56  yError()<< "Insufficient parameters to CanBusSkin.";
57  return false;
58  }
59 
60 
61  _canBusNum = config.find("canDeviceNum").asInt32();
62  char name[80];
63  sprintf(name, "canSkin on bus %d", _canBusNum);
64  _cfgReader.setName(name);
65 
66  int period=config.find("period").asInt32();
67  setPeriod((double)period/1000.0);
68 
69  netID = config.find("canDeviceNum").asInt32();
70 
71  Bottle ids=config.findGroup("skinCanIds").tail();
72 
73  if (ids.size()>1)
74  {
75  yWarning()<< "CanBusSkin id list contains more than one entry -> devices will be merged. ";
76  }
77 
78  for (int i=0; i<ids.size(); i++)
79  {
80 
81  int id = ids.get(i).asInt32();
82  cardId.push_back (id);
83  #if SKIN_DEBUG
84  yDebug<< "Id reading from: " << id;
85  #endif
86  }
87 
88  //elements are: // this is needed duringg initialization (readDefaultBoard)
89  sensorsNum=16*12*cardId.size();
90  data.resize(sensorsNum);
91  data.zero();
92 
93  Property prop;
94  prop.put("device", config.find("canbusDevice").asString().c_str());
95  prop.put("physDevice", config.find("physDevice").asString().c_str());
96  prop.put("canTxTimeout", 500);
97  prop.put("canRxTimeout", 500);
98  prop.put("canDeviceNum", config.find("canDeviceNum").asInt32());
99  prop.put("canMyAddress", 0);
100  prop.put("canTxQueueSize", CAN_DRIVER_BUFFER_SIZE);
101  prop.put("canRxQueueSize", CAN_DRIVER_BUFFER_SIZE);
102 
103  pCanBus=0;
105 
106  driver.open(prop);
107  if (!driver.isValid())
108  {
109  yError()<< "Error opening PolyDriver check parameters";
110  return false;
111  }
112 
113  driver.view(pCanBus);
114 
115  if (!pCanBus)
116  {
117  yError()<< "Error opening /ecan device not available";
118  return false;
119  }
120 
122  pCanBus->canSetBaudRate(0); //default 1MB/s
123 
126 
127  /* when the message will be available in the firmware
128  if(!checkFirmwareVersion())
129  {
130  diagnosticAvailable = false;
131  }
132  else
133  diagnosticAvailable = true;
134  */
135 
136 
137  if( _cfgReader.isDefaultBoardCfgPresent(config) && _cfgReader.isDefaultTriangleCfgPresent(config))
138  {
139  _newCfg = true;
140  yInfo() << "Skin on can bus " << _canBusNum << " uses NEW configuration version!!!";
141  ret = readNewConfiguration(config);
142  }
143  else
144  {
145  _newCfg = false;
146  yWarning() << "Skin on can bus " << _canBusNum << " uses old configuration version!!!";
147  ret = readOldConfiguration(config);
148  }
149 
150  if(!ret)
151  {
152  yError() << "Error reading config";
153  if (pCanBufferFactory)
154  {
155  pCanBufferFactory->destroyBuffer(inBuffer);
156  pCanBufferFactory->destroyBuffer(outBuffer);
157  }
158  driver.close();
159  return false;
160  }
161 
162  //set filter
163  uint8_t can_msg_class = 0;
164  if(_newCfg)
165  {
166  can_msg_class = ICUBCANPROTO_CLASS_PERIODIC_SKIN;
167  }
168  else
169  {
170  can_msg_class = ICUBCANPROTO_CLASS_PERIODIC_ANALOGSENSOR;
171  }
172  for (unsigned int i=0; i<cardId.size(); i++)
173  for (unsigned int id=0; id<16; ++id)
174  {
175  //yDebug() << "---------- aggiungo id = " << (can_msg_class << 8)+(cardId[i]<<4)+id;
176  pCanBus->canIdAdd((can_msg_class << 8)+(cardId[i]<<4)+id);
177  }
178 
179 
180 
181  /* ****** Skin diagnostics ****** */
182  portSkinDiagnosticsOut.open("/diagnostics/skin/errors:o");
183 
184  //if I 'm here, config is ok ==> send message to enable transmission
185  //(only in case of new configuration, skin boards need of explicit message in order to enable tx.)
186  yarp::os::Time::delay(0.01);
187  if(_newCfg)
188  {
189  uint8_t txmode = 1;
190  for(size_t i=0; i< cardId.size(); i++)
191  {
192  if(! sendCANMessage(cardId[i], ICUBCANPROTO_POL_AS_CMD__SET_TXMODE, &txmode))
193  return false;
194  }
195  }
196 
197  PeriodicThread::start();
198  return true;
199 }
200 
201 bool CanBusSkin::sendCANMessage(uint8_t destAddr, uint8_t command, void *data)
202 {
203  int len = 0;
204  unsigned int msgSent = 0;
205  CanMessage &msg = outBuffer[0];
206  unsigned int id = 0;
207  id= ((ICUBCANPROTO_CLASS_POLLING_ANALOGSENSOR << 8 ) | (destAddr & 0x0f));
208 
209  msg.setId(id);
210  //set command in message
211  msg.getData()[0] = command;
212 
213  switch(command)
214  {
215  case ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG: //77
216  {
218  msg.getData()[1] = 0;
219  msg.getData()[1] = brdCfg->skinType & 0x0f;
220  msg.getData()[2] = brdCfg->period;
221  msg.getData()[3] = brdCfg->noLoad;
222  len = 4;
223  }break;
224 
225  case ICUBCANPROTO_POL_SK_CMD__SET_TRIANG_CFG: //80
226  {
228  msg.getData()[1] = trCfg->triangleStart;
229  msg.getData()[2] = trCfg->triangleEnd;
230  msg.getData()[3] = trCfg->cfg.shift;
231  msg.getData()[4] = 0;
232 
233  if(trCfg->cfg.enabled)
234  msg.getData()[4] = 1;
235 
236  msg.getData()[5] = trCfg->cfg.cdcOffset & 0xff;
237  msg.getData()[6] = (trCfg->cfg.cdcOffset & 0xff00) >> 8;
238  len = 7;
239  }break;
240  case ICUBCANPROTO_POL_AS_CMD__SET_TXMODE: //7
241  {
242  uint8_t *txmode = (uint8_t *)data;
243  if(*txmode)
244  {
245  msg.getData()[1] = 0;
246  }
247  else
248  {
249  msg.getData()[1] = 1;
250  }
251  len = 2;
252  }break;
253  default:
254  {
255  len = 0;
256  }
257  };
258 
259 
260 
261  if(0==len)
262  {
263  yWarning() << "skin on can bus " << _canBusNum << ":try to send a unknown message(command id=" <<command << ")";
264  }
265  msg.setLen(len);
266 
267  if (!pCanBus->canWrite(outBuffer, 1, &msgSent))
268  {
269  yError() << "skin on can bus " << _canBusNum << ": Could not write to the CAN interface.";
270  return false;
271  }
272  if(msgSent != 1)
273  return false;
274 
275  return true;
276 }
277 
278 
279 
280 bool CanBusSkin::readNewSpecialConfiguration(yarp::os::Searchable& config)
281 {
282  unsigned int j;
283  unsigned int numofcfg;
284 
285  /* Read special board configuration */
286  numofcfg = cardId.size(); //set size of my vector boardCfgList;
287  //in output the function returns number of special board cfg are in file xml
288  SpecialSkinBoardCfgParam* boardCfgList = new SpecialSkinBoardCfgParam[numofcfg];
289 
290  if(!_cfgReader.readSpecialBoardCfg(config, boardCfgList, &numofcfg))
291  return false;
292 
293  for(j=0; j<numofcfg; j++) //for each special board config
294  {
295  //check if patch exist
296  if(_canBusNum != boardCfgList[j].patch)
297  {
298  yError() << "Skin on can bus " << _canBusNum << "configured SpecialBoardConfig on patch with a different id from my can bus " << _canBusNum << "patch=" <<boardCfgList[j].patch ;
299  return false;
300  }
301 
302  size_t boardIdx;
303  //check if card address are in patch
304  for(int a=boardCfgList[j].boardAddrStart; a<=boardCfgList[j].boardAddrEnd; a++)
305  {
306  for(boardIdx=0; boardIdx<cardId.size(); boardIdx++)
307  {
308  if(cardId[boardIdx]==a)//card address is in my patch
309  break;
310  }
311  if(boardIdx>=cardId.size())
312  {
313  yError() << "Skin on can bus %d" << _canBusNum << ": card with address " << a << "is not present ";
314  return(false);
315  }
316  }
317 // //uncomment for debug only
318 // yDebug() << "\n special cfg board: num " << j;
319 // boardCfgList[j].debugPrint();
320 
321  //send special board cfg
322  for(int listIdx=boardCfgList[j].boardAddrStart; listIdx<= boardCfgList[j].boardAddrEnd; listIdx++)
323  {
324  if(!sendCANMessage(listIdx, ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG, (void*)&boardCfgList[j].cfg))
325  return false;
326 
327  // Init the data vector with special config values from "noLoad" param in config file.
328  // This is to have a correct initilization for the data sent through yarp port
329  for (unsigned int triangleId = 0; triangleId < 16; triangleId++)
330  {
331  unsigned int index = 16*12*boardIdx + triangleId*12;
332 
333  // Message head
334  for(unsigned int k = 0; k < 12; k++)
335  {
336 // yDebug() << "readNewSpecialConfiguration size is: " << data.size() << " index is " << (index+k) << " value is: " << boardCfgList[j].cfg.noLoad;
337  if((index+k) >= data.size())
338  yError() << "readNewSpecialConfiguration: index too big";
339  data[index + k] = boardCfgList[j].cfg.noLoad;
340  }
341  }
342  }
343  }
344 
345  yarp::os::Time::delay(0.01);
346  /* Read special triangle configuration */
347  numofcfg = SPECIAL_TRIANGLE_CFG_MAX_NUM; //set size of my vector boardCfgList;
348  //in output the function return number of special board cfg are in file xml
349  SpecialSkinTriangleCfgParam* triangleCfg = new SpecialSkinTriangleCfgParam[numofcfg];
350 
351  if(! _cfgReader.readSpecialTriangleCfg(config, &triangleCfg[0], &numofcfg))
352  return false;
353 
354  for(j=0; j<numofcfg; j++)
355  {
356 // yDebug() << "Special triangle cfg: " << numofcfg << " on can bus: " << _canBusNum;
357 
358  //check if patch exist
359  if(_canBusNum != triangleCfg[j].patch)
360  {
361  yError() << "Skin on can bus " << _canBusNum << "configured SpecialTriangleConfig on patch with a different id from my can bus" << _canBusNum << "patch=" <<triangleCfg[j].patch ;
362  return false;
363  }
364 
365  for(size_t i=0; i<cardId.size(); i++)
366  {
367  if(cardId[i]==triangleCfg[j].boardAddr)//card address is in my patch
368  break;
369 
370  if(i>=cardId.size())
371  {
372  yError() << "Skin on can bus %d" << _canBusNum << ": card with address " << triangleCfg[j].boardAddr << "is not present ";
373  return(false);
374  }
375 
376  }
377 // //uncomment for debug only
378 // yDebug() << "\n Special triangle cfg num " << j;
379 // triangleCfg[j].debugPrint();
380 
381  //send triangle cfg
382  if(!sendCANMessage(triangleCfg[j].boardAddr, ICUBCANPROTO_POL_SK_CMD__SET_TRIANG_CFG, (void*)&triangleCfg[j]))
383  return false;
384  }
385 
386  return true;
387 }
388 
389 bool CanBusSkin::readNewConfiguration(yarp::os::Searchable& config)
390 {
391  /*read skin board default configuration*/
392  _brdCfg.setDefaultValues();
393  if(!_cfgReader.readDefaultBoardCfg(config, &_brdCfg))
394  return false;
395 
396  // Fill the data vector with default values from "noLoad" param in config file.
397  for (size_t board_idx = 0; board_idx < cardId.size(); board_idx++)
398  {
399  for (unsigned int triangleId = 0; triangleId < 16; triangleId++)
400  {
401  int index = 16*12*board_idx + triangleId*12;
402 
403  // Message head
404  for(unsigned int k = 0; k < 12; k++)
405  {
406 // yDebug() << "readNewConfiguration (default) size is: " << data.size() << " index is " << (index+k) << " value is: " << _brdCfg.noLoad;
407  if((index+k) >= data.size())
408  yError() << "readNewConfiguration: index too big";
409  data[index + k] = _brdCfg.noLoad;
410  }
411  }
412  }
413 
414 
415  /* send default board configuration (new configuration style)*/
416  for(size_t i=0; i<cardId.size(); i++)
417  {
418  if(!sendCANMessage(cardId[i], ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG, (void*)&_brdCfg))
419  return false;
420  }
421 
422  /*read skin triangle default configuration*/
423  _triangCfg.setDefaultValues();
424  if(! _cfgReader.readDefaultTriangleCfg(config, &_triangCfg))
425  return false;
427  spTrCfg.cfg = _triangCfg;
428  spTrCfg.triangleStart = 0;
429  spTrCfg.triangleEnd = TRIANGLE_MAX_NUM_IN_BOARD-1; //in each board there are 16 triagle max, their id goes from 0 to TRIANGLE_MAX_NUM_IN_BOARD-1;
430 
431  //send default board and triangle configuration (new configuration style)
432  for(size_t i=0; i<cardId.size(); i++)
433  {
434  spTrCfg.boardAddr = cardId[i];
435  if(!sendCANMessage(cardId[i], ICUBCANPROTO_POL_SK_CMD__SET_TRIANG_CFG, (void*)&spTrCfg))
436  return false;
437  }
438 
439  yarp::os::Time::delay(0.5);
440 
441  /*read and send special cfg for board and triangle */
442  if(!readNewSpecialConfiguration(config))
443  return false;
444 
445  return true;
446 }
447 
448 
449 bool CanBusSkin::readOldConfiguration(yarp::os::Searchable& config)
450 {
451  /* ******* Parameters for hand skin patches. If these are not specified, default values are used. ******* */
452  // Initialise parameter vectors
453 
454  // 4C Message
455  msg4C_Timer = config.findGroup("4C_Timer").tail(); // 4C_Timer
456  msg4C_CDCOffsetL = config.findGroup("4C_CDCOffsetL").tail(); // 4C_CDCOffsetL
457  msg4C_CDCOffsetH = config.findGroup("4C_CDCOffsetH").tail(); // 4C_CDCOffsetR
458  msg4C_TimeL = config.findGroup("4C_TimeL").tail(); // 4C_TimeL
459  msg4C_TimeH = config.findGroup("4C_TimeH").tail(); // 4C_TimeH
460  // 4E Message
461  msg4E_Shift = config.findGroup("4E_Shift").tail(); // 4E_Shift
462  msg4E_Shift3_1 = config.findGroup("4E_Shift3_1").tail(); // 4E_Shift3_1
463  msg4E_NoLoad = config.findGroup("4E_NoLoad").tail(); // 4E_NoLoad
464  msg4E_Param = config.findGroup("4E_Param").tail(); // 4E_Param
465  msg4E_EnaL = config.findGroup("4E_EnaL").tail(); // 4E_EnaL
466  msg4E_EnaH = config.findGroup("4E_EnaH").tail(); // 4E_EnaH
467 
468 // yDebug() << "msg4E_NoLoad size is " << msg4E_NoLoad.size();
469 // yDebug() << "msg4E_NoLoad content is " << msg4E_NoLoad.toString();
470 
471  int numofcards = cardId.size();
472  // Check parameter list length
473  // 4C Message
474  checkParameterListLength("4C_Timer", msg4C_Timer, numofcards, Value(0x01));
475  checkParameterListLength("4C_CDCOffsetL", msg4C_CDCOffsetL, numofcards, Value(0x00));
476  checkParameterListLength("4C_CDCOffsetH", msg4C_CDCOffsetH, numofcards, Value(0x20));
477  checkParameterListLength("4C_TimeL", msg4C_TimeL, numofcards, Value(0x00));
478  checkParameterListLength("4C_TimeH", msg4C_TimeH, numofcards, Value(0x00));
479  // 4C Message
480  checkParameterListLength("4E_Shift", msg4E_Shift, numofcards, Value(0x02));
481  checkParameterListLength("4E_Shift3_1", msg4E_Shift3_1, numofcards, Value(0x22));
482  checkParameterListLength("4E_NoLoad", msg4E_NoLoad, numofcards, Value(0xF0));
483  checkParameterListLength("4E_Param", msg4E_Param, numofcards, Value(0x00));
484  checkParameterListLength("4E_EnaL", msg4E_EnaL, numofcards, Value(0xFF));
485  checkParameterListLength("4E_EnaH", msg4E_EnaH, numofcards, Value(0xFF));
486  /* ********************************************** */
487 
488 // yDebug() << "msg4E_NoLoad size is " << msg4E_NoLoad.size();
489 // yDebug() << "msg4E_NoLoad content is " << msg4E_NoLoad.toString();
490 
491 
492  // Fill the data vector with default values from "noLoad" param in config file.
493  for (size_t idx=0; idx < cardId.size(); idx++)
494  {
495  int board_idx = idx;
496  int baseLine = msg4E_NoLoad.get(idx).asInt32();
497 
498  for (unsigned int triangleId = 0; triangleId < 16; triangleId++)
499  {
500  unsigned int index = 16*12*board_idx + triangleId*12;
501 
502  // Message head
503  for(unsigned int k = 0; k < 12; k++)
504  {
505 // yDebug() << "readOldConfiguration size is: " << data.size() << " index is " << (index+k) << " value is: " << baseLine;
506  if((index+k) >= data.size())
507  yError() << "readOldConfiguration: index too big";
508  data[index + k] = baseLine;
509  }
510  }
511  }
512 
513  if(sendCANMessage4C())
514  {
515  return sendCANMessage4E();
516  }
517  else
518  {
519  return false;
520  }
521 }
522 
524 {
525 
526  if(_newCfg)
527  {
528  uint8_t txmode = 0;
529  for(size_t i=0; i< cardId.size(); i++)
530  {
531  sendCANMessage(cardId[i], ICUBCANPROTO_POL_AS_CMD__SET_TXMODE, &txmode);
532  }
533  }
534 
536  if (pCanBufferFactory)
537  {
538  pCanBufferFactory->destroyBuffer(inBuffer);
539  pCanBufferFactory->destroyBuffer(outBuffer);
540  }
541  driver.close();
542  return true;
543 }
544 
545 int CanBusSkin::read(yarp::sig::Vector &out)
546 {
547  lock_guard<mutex> lck(mtx);
548  out=data;
549  return yarp::dev::IAnalogSensor::AS_OK;
550 }
551 
553 {
554  return yarp::dev::IAnalogSensor::AS_OK;;
555 }
556 
558 {
559  return sensorsNum;
560 }
561 
563  if(!_newCfg)
564  sendCANMessage4C();
565 
566  return AS_OK;
567 }
568 
569 int CanBusSkin::calibrateChannel(int ch, double v)
570 {
571  //NOT YET IMPLEMENTED
572  return calibrateSensor();
573 }
574 
575 int CanBusSkin::calibrateSensor(const yarp::sig::Vector& v)
576 {
577  return 0;
578 }
579 
581 {
582  return 0;
583 }
584 
586 // if(sendCANMessage4C()) {
587 // return sendCANMessage4E();
588 // } else {
589 // return false;
590 // }
591  return true;
592 }
593 
595 
596  lock_guard<mutex> lck(mtx);
597 
598  unsigned int canMessages = 0;
599  bool res = pCanBus->canRead(inBuffer, CAN_DRIVER_BUFFER_SIZE, &canMessages);
600 
601  if (!res)
602  {
603  yError("CanBusSkin: CanRead failed");
604  }
605  else
606  {
607  // Allocate error vector
608  errors.resize(canMessages);
609 
610  for (unsigned int i = 0; i < canMessages; i++) {
611 
612  CanMessage &msg = inBuffer[i];
613 
614  unsigned int msgid = msg.getId();
615  unsigned int id = (msgid & 0x00F0) >> 4;
616  unsigned int sensorId = (msgid & 0x000F);
617  unsigned int msgType = (int) msg.getData()[0];
618  int len = msg.getLen();
619 
620 #if 0
621  cout << "DEBUG: CanBusSkin: Board ID (" << id <<"): Sensor ID (" << sensorId << "): "
622  << "Message type (" << std::uppercase << std::showbase << std::hex << (int) msg.getData()[0] << ") "
623  << std::nouppercase << std::noshowbase << std::dec << " Length (" << len << "): ";
624  cout << "Content: " << std::uppercase << std::showbase << std::hex;
625  for (int k = 0; k < len; ++k) {
626  cout << (int) msg.getData()[k] << " ";
627  }
628  cout << "\n" << std::nouppercase << std::noshowbase << std::dec;
629 #endif
630 
631  for (size_t j = 0; j < cardId.size(); j++) {
632  if (id == cardId[j]) {
633  int index = 16*12*j + sensorId*12;
634 
635  if (msgType == 0x40) {
636  // Message head
637  for(int k = 0; k < 7; k++) {
638  data[index + k] = msg.getData()[k + 1];
639  }
640  } else if (msgType == 0xC0) {
641  // Message tail
642  for(int k = 0; k < 5; k++) {
643  data[index + k + 7] = msg.getData()[k + 1];
644  }
645 
646  // Skin diagnostics
647  if (_brdCfg.useDiagnostic) // if user requests to check the diagnostic
648  {
649  if (len == 8) // firmware is sending diagnostic info
650  {
651  _isDiagnosticPresent = true;
652 
653  // Get error code head and tail
654  short head = msg.getData()[6];
655  short tail = msg.getData()[7];
656  int fullMsg = (head << 8) | (tail & 0xFF);
657 
658  // Store error message
659  errors[i].net = netID;
660  errors[i].board = id;
661  errors[i].sensor = sensorId;
662  errors[i].error = fullMsg;
663 
664  if(fullMsg != SkinErrorCode::StatusOK)
665  {
666  yError() << "canBusSkin error code: " <<
667  "canDeviceNum: " << errors[i].net <<
668  "board: " << errors[i].board <<
669  "sensor: " << errors[i].sensor <<
670  "error: " << iCub::skin::diagnostics::printErrorCode(errors[i].error).c_str();
671 
672  yarp::sig::Vector &out = portSkinDiagnosticsOut.prepare();
673  out.clear();
674 
675  out.push_back(errors[i].net);
676  out.push_back(errors[i].board);
677  out.push_back(errors[i].sensor);
678  out.push_back(errors[i].error);
679 
680  portSkinDiagnosticsOut.write(true);
681 
682  }
683  }
684  else
685  {
686  _isDiagnosticPresent = false;
687  }
688  }
689  }
690  }
691  // else
692  // {
693  // yError()<< "Error: skin received malformed message\n";
694  // }
695  }
696  }
697  }
698 }
699 
701 {
702 #if SKIN_DEBUG
703  yDebug<<"CanBusSkin Thread releasing...";
704  yDebug<<"... done.";
705 #endif
706 }
707 
708 
709 /* *********************************************************************************************************************** */
710 /* ******* Diagnose skin errors. ********************************************** */
711 bool CanBusSkin::diagnoseSkin(void) {
712  using iCub::skin::diagnostics::DetectedError; // FG: Skin diagnostics errors
713  using yarp::sig::Vector;
714 
715  // Write errors to port
716  for (size_t i = 0; i < errors.size(); ++i)
717  {
718  Vector &out = portSkinDiagnosticsOut.prepare();
719  out.clear();
720 
721  out.push_back(errors[i].net);
722  out.push_back(errors[i].board);
723  out.push_back(errors[i].sensor);
724  out.push_back(errors[i].error);
725 
726  portSkinDiagnosticsOut.write(true);
727  }
728 
729  return true;
730 }
731 /* *********************************************************************************************************************** */
732 
733 /* *********************************************************************************************************************** */
734 /* ******* Converts input parameter bottle into a std vector. ********************************************** */
735 void CanBusSkin::checkParameterListLength(const string &i_paramName, Bottle &i_paramList, const int &i_length, const Value &i_defaultValue) {
736  if ((i_paramList.isNull()) || (i_paramList.size() != i_length)) {
737  yWarning() << "CanBusSkin: The number of " << i_paramName << " parameters provided is different than the number of CAN IDs for this BUS. Check your parameters.";
738  yWarning() << "CanBusSkin: Using default values for " << i_paramName << " parameters.";
739 
740  size_t i;
741  for (i= i_paramList.size() ; i <= (unsigned int) i_length; ++i) {
742  i_paramList.add(i_defaultValue);
743  }
744  }
745 }
746 /* *********************************************************************************************************************** */
747 
748 
749 /* *********************************************************************************************************************** */
750 /* ******* Sends CAN setup message type 4C. ********************************************** */
751 bool CanBusSkin::sendCANMessage4C(void) {
752  for (size_t i = 0; i < cardId.size(); ++i) {
753 #if SKIN_DEBUG
754  printf("CanBusSkin: Thread initialising board ID: %d. \n",cardId[i]);
755  printf("CanBusSkin: Sending 0x4C message to skin boards. \n");
756 #endif
757 
758  unsigned int canMessages=0;
759  unsigned id = 0x200 + cardId[i];
760 
761  CanMessage &msg4c=outBuffer[0];
762  msg4c.setId(id);
763  msg4c.getData()[0] = 0x4C; // Message type
764  msg4c.getData()[1] = 0x01;
765  msg4c.getData()[2] = 0x01;
766  msg4c.getData()[3] = msg4C_Timer.get(i).asInt32();
767  msg4c.getData()[4] = msg4C_CDCOffsetL.get(i).asInt32();
768  msg4c.getData()[5] = msg4C_CDCOffsetH.get(i).asInt32();
769  msg4c.getData()[6] = msg4C_TimeL.get(i).asInt32();
770  msg4c.getData()[7] = msg4C_TimeH.get(i).asInt32();
771  msg4c.setLen(8);
772 
773 #if SKIN_DEBUG
774  yDebug << "CanBusSkin: Input parameters (msg 4C) are: " << std::hex << std::showbase
775  << (int) msg4c.getData()[0] << " " << (int) msg4c.getData()[1] << " " << (int) msg4c.getData()[2] << " "
776  << msg4C_Timer.get(i).asInt32() << " " << msg4C_CDCOffsetL.get(i).asInt32() << " " << msg4C_CDCOffsetH.get(i).asInt32() << " "
777  << msg4C_TimeL.get(i).asInt32() << " " << msg4C_TimeH.get(i).asInt32() << std::dec << std::noshowbase
778  << ". \n";
779  yDebug << "CanBusSkin: Output parameters (msg 4C) are: " << std::hex << std::showbase
780  << (int) msg4c.getData()[0] << " " << (int) msg4c.getData()[1] << " " << (int) msg4c.getData()[2] << " "
781  << (int) msg4c.getData()[3] << " " << (int) msg4c.getData()[4] << " " << (int) msg4c.getData()[5] << " "
782  << (int) msg4c.getData()[6] << " " << (int) msg4c.getData()[7]
783  << ". \n";
784 #endif
785 
786  if (!pCanBus->canWrite(outBuffer, 1, &canMessages)) {
787  yError() << "CanBusSkin: Could not write to the CAN interface. \n";
788  return false;
789  }
790  }
791 
792  return true;
793 }
794 
795 
796 /* *********************************************************************************************************************** */
797 /* ******* Sends CAN setup message type 4E. ********************************************** */
798 bool CanBusSkin::sendCANMessage4E(void) {
799  // Send 0x4E message to modify offset
800  for (size_t i = 0; i < cardId.size(); ++i) {
801  // FG: Sending 4E message to all MTBs
802 #if SKIN_DEBUG
803  printf("CanBusSkin: Thread initialising board ID: %d. \n", cardId[i]);
804  printf("CanBusSkin: Sending 0x4E message to skin boards. \n");
805 #endif
806 
807  unsigned int canMessages = 0;
808  unsigned id = 0x200 + cardId[i];
809 
810  CanMessage &msg4e = outBuffer[0];
811  msg4e.setId(id);
812  msg4e.getData()[0] = 0x4E; // Message type
813  msg4e.getData()[1] = msg4E_Shift.get(i).asInt32();
814  msg4e.getData()[2] = msg4E_Shift3_1.get(i).asInt32();
815  msg4e.getData()[3] = msg4E_NoLoad.get(i).asInt32();
816  msg4e.getData()[4] = msg4E_Param.get(i).asInt32();
817  msg4e.getData()[5] = msg4E_EnaL.get(i).asInt32();
818  msg4e.getData()[6] = msg4E_EnaH.get(i).asInt32();
819  msg4e.getData()[7] = 0x0A;
820  msg4e.setLen(8);
821 
822 #if SKIN_DEBUG
823  yDebug << "CanBusSkin: Input parameters (msg 4E) are: " << std::hex << std::showbase
824  << (int) msg4e.getData()[0] << " " << msg4E_Shift.get(i).asInt32() << " " << msg4E_Shift3_1.get(i).asInt32() << " "
825  << msg4E_NoLoad.get(i).asInt32() << " " << msg4E_Param.get(i).asInt32() << " " << msg4E_EnaL.get(i).asInt32() << " "
826  << msg4E_EnaH.get(i).asInt32() << " " << (int) msg4e.getData()[7] << std::dec << std::noshowbase
827  << ". \n";
828  yDebug << "CanBusSkin: Output parameters (msg 4E) are: " << std::hex << std::showbase
829  << (int) msg4e.getData()[0] << " " << (int) msg4e.getData()[1] << " " << (int) msg4e.getData()[2] << " "
830  << (int) msg4e.getData()[3] << " " << (int) msg4e.getData()[4] << " " << (int) msg4e.getData()[5] << " "
831  << (int) msg4e.getData()[6] << " " << (int) msg4e.getData()[7] << std::dec << std::noshowbase
832  << ". \n";
833 #endif
834 
835  if (!pCanBus->canWrite(outBuffer, 1, &canMessages)) {
836  yError() << "CanBusSkin: Could not write to the CAN interface.";
837  return false;
838  }
839  }
840 
841  return true;
842 }
843 /* *********************************************************************************************************************** */
844 
845 
846 /************************************************************************************************************************ */
847 
849 bool CanBusSkin::checkFirmwareVersion(void)
850 {
851  bool firmware_version_ok = true;
852 
853  for (size_t i = 0; i < cardId.size(); ++i)
854  {
855 #if 1 //SKIN_DEBUG
856  yDebug("CanBusSkin: check firmware version for board ID: %d. \n",cardId[i]);
857 #endif
858 
859  unsigned int canMessages=0;
860  unsigned int id = (ICUBCANPROTO_CLASS_POLLING_ANALOGSENSOR << 8) + (0x00 /*master address*/ << 4) + (cardId[i]);
861 
862  // enable reading of messages from the bootloader class (already done?)
863  unsigned int recvId = ( (ICUBCANPROTO_CLASS_POLLING_ANALOGSENSOR << 8) + (cardId[i] << 4) );
864  pCanBus->canIdAdd(recvId);
865 
866  //CanMessage &txBuffer=outBuffer[0];
867  // Send command
868  outBuffer[0].setId(id);
869  outBuffer[0].setLen(1);
870  outBuffer[0].getData()[0]= ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION; // message for firmware version
871 
872  if (!pCanBus->canWrite(outBuffer, 1, &canMessages))
873  {
874  yError() << "CanBusSkin: Could not write to the CAN interface. \n";
875  return false;
876  }
877 
878 
879  // pause
880  yarp::os::Time::delay(0.3);
881 
882  //riceve la risposta
883  bool done=false;
884  unsigned int read_messages=0;
885  while(!done)
886  {
887  bool res = pCanBus->canRead(inBuffer, CAN_DRIVER_BUFFER_SIZE, &read_messages);
888  if (!res)
889  {
890  yError() << "CanBusSkin: CanRead failed \n";
891  }
892 
893  //Timeout: no answers
894  if (read_messages==0)
895  {
896  yError ("No answers\n");
897  return false;
898  }
899 
900  //One (or more) answers received
901  //Counts the number of the boards
902  for (i=0; i<read_messages; i++)
903  {
905 #if 0
906  fprintf(stderr, "%.4x ", inBuffer[i].getId());
907  fprintf(stderr, "%.2x ", inBuffer[i].getData()[0]);
908  fprintf(stderr, "%.2x ", inBuffer[i].getData()[1]);
909  fprintf(stderr, "%.2x ", inBuffer[i].getData()[2]);
910  fprintf(stderr, "%.2x ", inBuffer[i].getData()[3]);
911  fprintf(stderr, "%.2x ", inBuffer[i].getData()[4]);
912  fprintf(stderr, "%.2x ", inBuffer[i].getData()[5]);
913  fprintf(stderr, "%.2x ", inBuffer[i].getData()[6]);
914  fprintf(stderr, "%.2x\n", inBuffer[i].getData()[7]);
915 #endif
917 
918  int type;
919  int version;
920  int release;
921  int build;
922 
923  if (0 /* check the message received is right*/ )
924  {
925  /* parse it somehow */
926  type = 0; // inBuffer[i].getData()[1];
927  version = 0; // inBuffer[i].getData()[2];
928  release = 0; // inBuffer[i].getData()[3];
929  build = 0; // inBuffer[i].getData()[4];
930 
931  fprintf(stderr, "type %d\n", type);
932  fprintf(stderr, "version %d\n", version);
933  fprintf(stderr, "release %d\n", release);
934  fprintf(stderr, "build %d\n", build);
935 
936  if(version > 2) { firmware_version_ok = true; }
937  else
938  if(version < 2) { firmware_version_ok = false; }
939  else
940  {
941  if(release > 16) { firmware_version_ok = true; }
942  else
943  if(release < 16) { firmware_version_ok = false; }
944  else
945  {
946  if(build >= 25) { firmware_version_ok = true; }
947  else
948  { firmware_version_ok = false; }
949  }
950  }
951  done = true;
952  }
953 
954  if(!firmware_version_ok)
955  {
956  yWarning() << "Diagnostic check was enabled in the config file, but the firwmare does not support it yet! \
957  Please verify that the firmware version is at least 2.10.15 as shown in the canLoader\
958  disabling parsing of diagnostic messages";
959  }
960  else
961  yInfo() << "Firwmare check for skin diagnostic messages ok";
962 
963  }
964  }
965  }
966  return firmware_version_ok;
967 }
#define SPECIAL_TRIANGLE_CFG_MAX_NUM
Definition: CanBusSkin.cpp:21
#define TRIANGLE_MAX_NUM_IN_BOARD
Definition: CanBusSkin.cpp:22
const int CAN_DRIVER_BUFFER_SIZE
Definition: CanBusSkin.cpp:19
@ data
virtual int getState(int ch)
Definition: CanBusSkin.cpp:552
yarp::sig::VectorOf< iCub::skin::diagnostics::DetectedError > errors
The detected skin errors.
Definition: CanBusSkin.h:84
virtual void threadRelease()
Definition: CanBusSkin.cpp:700
yarp::dev::CanBuffer inBuffer
Definition: CanBusSkin.h:70
yarp::sig::VectorOf< int > cardId
Definition: CanBusSkin.h:78
yarp::sig::Vector data
Definition: CanBusSkin.h:81
yarp::dev::CanBuffer outBuffer
Definition: CanBusSkin.h:71
yarp::dev::ICanBufferFactory * pCanBufferFactory
Definition: CanBusSkin.h:69
virtual bool threadInit()
Definition: CanBusSkin.cpp:585
yarp::dev::ICanBus * pCanBus
Definition: CanBusSkin.h:68
virtual int getChannels()
Definition: CanBusSkin.cpp:557
virtual int read(yarp::sig::Vector &out)
Definition: CanBusSkin.cpp:545
virtual bool close()
Definition: CanBusSkin.cpp:523
std::mutex mtx
Definition: CanBusSkin.h:73
yarp::dev::PolyDriver driver
Definition: CanBusSkin.h:67
virtual void run()
Definition: CanBusSkin.cpp:594
virtual bool open(yarp::os::Searchable &config)
Definition: CanBusSkin.cpp:40
virtual int calibrateSensor()
Definition: CanBusSkin.cpp:562
int netID
The CAN net ID.
Definition: CanBusSkin.h:76
virtual int calibrateChannel(int ch, double v)
Definition: CanBusSkin.cpp:569
int sensorsNum
Definition: CanBusSkin.h:79
uint8_t skinType
Definition: skinParams.h:36
void setDefaultValues(void)
Definition: skinParams.h:39
void setName(char *name)
bool readDefaultBoardCfg(yarp::os::Searchable &config, SkinBoardCfgParam *boardCfg)
bool isDefaultBoardCfgPresent(yarp::os::Searchable &config)
bool readSpecialBoardCfg(yarp::os::Searchable &config, SpecialSkinBoardCfgParam *boardCfg, unsigned int *numofcfg)
bool readDefaultTriangleCfg(yarp::os::Searchable &config, SkinTriangleCfgParam *triangCfg)
bool readSpecialTriangleCfg(yarp::os::Searchable &config, SpecialSkinTriangleCfgParam *triangleCfg, unsigned int *numofcfg)
bool isDefaultTriangleCfgPresent(yarp::os::Searchable &config)
void setDefaultValues(void)
Definition: skinParams.h:60
SkinTriangleCfgParam cfg
uint8_t board
static uint32_t idx[BOARD_NUM]
static int stop
Definition: iCub_Sim.cpp:41
static int v
Definition: iCub_Sim.cpp:42
bool done
Definition: main.cpp:42
std::string printErrorCode(int code)
fprintf(fid,'\n')
out
Definition: sine.m:8