iCub-main
CanBusInertialMTB.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 and Lorenzo Natale
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #include <CanBusInertialMTB.h>
8 
9 #include <yarp/os/Time.h>
10 #include <yarp/os/Log.h>
11 #include <iostream>
12 #include <cstring>
13 #include <string>
14 
15 #include <iCubCanProto_skinMessages.h>
16 
18 const double CANBUS_INERTIAL_MTB_TIMEOUT=0.1; //100ms
19 
20 
21 // Protocol for inertial sensor messages sent by the MTB
22 const unsigned int CAN_MSG_CLASS_ACC_GYRO = 0x500;
23 const unsigned int MSG_TYPE_GYRO = 0x000;
24 const unsigned int MSG_TYPE_ACC = 0x001;
25 
27 
31 
32 using namespace std;
33 
34 
35 bool checkRequiredParamIsString(yarp::os::Searchable& config,
36  const std::string& paramName)
37 {
38  bool correct = config.check(paramName);
39  if( correct )
40  {
41  correct = config.find(paramName).isString();
42  }
43 
44  if( !correct )
45  {
46  yError("CanBusInertialMTB: problem loading parameter %s as string",paramName.c_str());
47  }
48 
49  return correct;
50 }
51 
52 bool checkRequiredParamIsInt(yarp::os::Searchable& config,
53  const std::string& paramName)
54 {
55  bool correct = config.check(paramName);
56  if( correct )
57  {
58  correct = config.find(paramName).isInt32();
59  }
60 
61  if( !correct )
62  {
63  yError("CanBusInertialMTB: problem loading parameter %s as int",paramName.c_str());
64  }
65 
66  return correct;
67 }
68 
69 bool checkRequiredParamIsVectorOfInt(yarp::os::Searchable& config,
70  const std::string& paramName,
71  std::vector<int> & output_vector)
72 {
73  bool correct = !(config.findGroup(paramName).isNull());
74  if( correct )
75  {
76  Bottle ids = config.findGroup(paramName).tail();
77  output_vector.resize(ids.size());
78  for(int i = 0; i < ids.size(); i++ )
79  {
80  output_vector[i] = ids.get(i).asInt32();
81  }
82  }
83 
84  if( !correct )
85  {
86  yError("CanBusInertialMTB: problem loading parameter %s as vector of int",paramName.c_str());
87  }
88 
89  return correct;
90 }
91 
92 // \todo TODO bug ?
93 bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
94  const std::string& paramName,
95  std::vector<std::string> & output_vector)
96 {
97  bool correct = !(config.findGroup(paramName).isNull());
98  if( correct )
99  correct = true;
100  {
101  Bottle ids = config.findGroup(paramName).tail();
102  std::cout << "ids : " << ids.toString() << std::endl;
103  std::cout << "ids : " << config.find(paramName).toString() << std::endl;
104  output_vector.resize(ids.size());
105  for(int i = 0; i < ids.size(); i++ )
106  {
107  output_vector[i] = ids.get(i).asString().c_str();
108  }
109  }
110 
111  if( !correct )
112  {
113  yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
114  }
115 
116  return correct;
117 }
118 
119 
120 bool CanBusInertialMTB::validateConf(yarp::os::Searchable& config,
121  std::vector<int> & canAddresses)
122 {
123  std::cout << "CanBusInertialMTB::validateConf : " << config.toString() << std::endl;
124  bool correct=true;
125 
126  correct = correct && checkRequiredParamIsString(config,"canbusDevice");
127  correct = correct && checkRequiredParamIsInt(config,"canDeviceNum");
128  correct = correct && checkRequiredParamIsVectorOfInt(config,"canAddress",canAddresses);
129  correct = correct && checkRequiredParamIsString(config,"physDevice");
130  correct = correct && checkRequiredParamIsString(config,"sensorType");
131 
132  return correct;
133 }
134 
135 bool CanBusInertialMTB::open(yarp::os::Searchable& config)
136 {
137  std::vector<int> canAddresses;
138  bool correct = this->validateConf(config,canAddresses);
139 
140  if (!correct)
141  {
142  yError("CanBusInertialMTB: Insufficient parameters to CanBusInertialMTB\n");
143  return false;
144  }
145 
146  //Read sensor period for all sensors
148  if (config.check("sensorPeriod"))
149  {
150  int int_sensorPeriod = config.find("sensorPeriod").asInt32();
151  if( int_sensorPeriod < 1 || int_sensorPeriod > 255 )
152  {
153  yError("CanBusInertialMTB: sensorPeriod is lower than 1 or bigger then 255\n");
154  return false;
155  }
156  sensorPeriod = int_sensorPeriod;
157  }
158 
159  //Parse sensor type and address of all readed sensors
160  this->boards.resize(canAddresses.size());
161  this->nrOfTotalChannels = 0;
162 
163 
164  for(size_t board=0; board < this->boards.size(); board++ )
165  {
166  this->boards[board].boardId = canAddresses[board];
167  if( config.find("sensorType").asString() == "acc" )
168  {
169  this->boards[board].enabledSensors = CANBUS_INERTIAL_MTB_INTERNAL_ACC_BIT;
170  this->boards[board].enabledGyro = false;
171  this->boards[board].nrOfChannels = 3;
172  this->boards[board].vectorOffset = this->nrOfTotalChannels;
173  this->nrOfTotalChannels += this->boards[board].nrOfChannels;
174  }
175  else if( config.find("sensorType").asString() == "extAccAndGyro" )
176  {
177  this->boards[board].enabledSensors = CANBUS_INERTIAL_MTB_EXTERNAL_GYRO_BIT |
179  this->boards[board].enabledGyro = true;
180  this->boards[board].nrOfChannels = 6;
181  this->boards[board].vectorOffset = this->nrOfTotalChannels;
182  this->nrOfTotalChannels += this->boards[board].nrOfChannels;
183  }
184  else
185  {
186  yError("CanBusInertialMTB: unknown sensorType %s",config.find("sensorType").asString().c_str());
187  return false;
188  }
189  }
190 
191  if (config.check("period")==true)
192  {
193  int period=10;
194  period=config.find("period").asInt32();
195  setPeriod((double)period/1000.0);
196  }
197 
198  Property prop;
199 
200  prop.put("device", config.find("canbusDevice").asString().c_str());
201  prop.put("physDevice", config.find("physDevice").asString().c_str());
202  prop.put("canTxTimeout", 500);
203  prop.put("canRxTimeout", 500);
204  prop.put("canDeviceNum", config.find("canDeviceNum").asInt32());
205  prop.put("canMyAddress", 0);
206  prop.put("canTxQueueSize", CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
207  prop.put("canRxQueueSize", CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
208  pCanBus=0;
209  pCanBufferFactory=0;
210 
211  //open the can driver
212  driver.open(prop);
213  if (!driver.isValid())
214  {
215  yError("Unable to open CanBusInertialMTB check parameters\n");
216  return false;
217  }
218  driver.view(pCanBus);
219  if (!pCanBus)
220  {
221  yError("Unable to open CAN device not available\n");
222  return false;
223  }
224  driver.view(pCanBufferFactory);
225  outBuffer=pCanBufferFactory->createBuffer(CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
226  inBuffer=pCanBufferFactory->createBuffer(CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE);
227 
228  //select the communication speed
229  pCanBus->canSetBaudRate(0); //default 1MB/s
230 
231  // open the can mask for the desired canDeviceId
232  // messages class is 0x500
233  for(size_t board=0; board < this->boards.size(); board++ )
234  {
235  unsigned short boardId = this->boards[board].boardId;
236  if( this->boards[board].enabledGyro )
237  {
238  pCanBus->canIdAdd((CAN_MSG_CLASS_ACC_GYRO)+(boardId<<4)+MSG_TYPE_GYRO);
239  }
240  pCanBus->canIdAdd((CAN_MSG_CLASS_ACC_GYRO)+(boardId<<4)+MSG_TYPE_ACC);
241 
242  }
243 
244  data.resize(this->nrOfTotalChannels);
245  data.zero();
246  privateData.resize(this->nrOfTotalChannels);
247  privateData.zero();
248  sharedStatus.resize(this->nrOfTotalChannels,IAnalogSensor::AS_OK);
249  privateStatus.resize(this->nrOfTotalChannels,IAnalogSensor::AS_OK);
250 
251  PeriodicThread::start();
252  return true;
253 }
254 
256 {
257  //stop the thread
259 
260  //stop the driver
261  if (pCanBufferFactory)
262  {
263  pCanBufferFactory->destroyBuffer(inBuffer);
264  pCanBufferFactory->destroyBuffer(outBuffer);
265  }
266  driver.close();
267 
268  return true;
269 }
270 
271 int CanBusInertialMTB::read(yarp::sig::Vector &out)
272 {
273  lock_guard<mutex> lck(mtx);
274  out=data;
275  return this->sharedGlobalStatus;
276 }
277 
279 {
280  lock_guard<mutex> lck(mtx);
281  return this->sharedStatus[ch];
282 }
283 
285 {
286  return this->nrOfTotalChannels;
287 }
288 
290 {
291  //NOT YET IMPLEMENTED
292  return 0;
293 }
294 
296 {
297  //NOT YET IMPLEMENTED
298  return 0;
299 }
300 
301 int CanBusInertialMTB::calibrateSensor(const yarp::sig::Vector& v)
302 {
303  //NOT YET IMPLEMENTED
304  return 0;
305 }
306 
308 {
309  //NOT YET IMPLEMENTED
310  return 0;
311 }
312 
314 {
315  for(size_t board=0; board < this->boards.size(); board++ )
316  {
317  unsigned int canMessages=0;
318  unsigned id = 0x200 + this->boards[board].boardId;
319 
320  CanMessage &msg=outBuffer[0];
321  msg.setId(id);
322  msg.getData()[0]=ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP; // message type
323  msg.getData()[1]=this->boards[board].enabledSensors; // = enable the desired senssors
324  msg.getData()[2]=this->boards[board].sensorPeriod; // period (ms)
325  msg.setLen(3);
326  canMessages=0;
327  bool ret = pCanBus->canWrite(outBuffer, 1, &canMessages);
328 
329  if( !ret )
330  {
331  yError("CanBusInertialMTB: canWrite returned false for sensor with address %x",this->boards[board].boardId);
332  return false;
333  }
334  }
335 
336  return true;
337 }
338 
339 void CanBusInertialMTB::setPrivateBoardStatus(int boardIndex, short status)
340 {
341  int vectorOffset = this->boards[boardIndex].vectorOffset;
342  int nrOfBoardChannels = this->boards[boardIndex].nrOfChannels;
343  for(int i=0; i < nrOfBoardChannels; i++ )
344  {
345  privateStatus[vectorOffset+i] = status;
346  }
347 }
348 
349 void CanBusInertialMTB::setPrivateBoardGyroStatus(int boardIndex, short status)
350 {
351  int vectorOffset = this->boards[boardIndex].vectorOffset;
352  int nrOfBoardChannels = this->boards[boardIndex].nrOfChannels;
353  for(int i=3; i < 6; i++ )
354  {
355  privateStatus[vectorOffset+i] = status;
356  }
357 }
358 
359 void CanBusInertialMTB::setPrivateBoardAccStatus(int boardIndex, short status)
360 {
361  int vectorOffset = this->boards[boardIndex].vectorOffset;
362  int nrOfBoardChannels = this->boards[boardIndex].nrOfChannels;
363  for(int i=0; i < 3; i++ )
364  {
365  privateStatus[vectorOffset+i] = status;
366  }
367 }
368 
370 {
371  unsigned int canMessages=0;
372 
373  bool res=pCanBus->canRead(inBuffer,CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE,&canMessages);
374  if (!res)
375  yError("CanBusInertialMTB::run(): canRead failed\n");
376 
377  double timeNow=Time::now();
378 
379  if(canMessages <0)
380  {
381  yError("CanBusInertialMTB::run() get %d canMessages\n", canMessages);
382  for(size_t board=0; board < this->boards.size(); board++ )
383  {
384  setPrivateBoardStatus(board,IAnalogSensor::AS_ERROR);
385  }
386 
387  }
388 
389  for (unsigned int i=0; i<canMessages; i++)
390  {
391  CanMessage &msg=inBuffer[i];
392 
393  unsigned int msgid = msg.getId();
394  unsigned char *buff = msg.getData();
395  unsigned int len = msg.getLen();
396  unsigned char id = ((msgid & 0x00f0)>>4);
397  unsigned int msg_class = ((msgid & 0x0700));
398  unsigned char msg_type = ((msgid & 0x000f));
399 
400  // \todo TODO substitute the double for with a boardId -> board index vector? Worth?
401  for(size_t board=0; board < this->boards.size(); board++ )
402  {
403  unsigned short boardId = this->boards[board].boardId;
404  unsigned int vectorOffset = this->boards[board].vectorOffset;
405  //parse data here
406  if (id==boardId && msg_class==CAN_MSG_CLASS_ACC_GYRO && msg_type == MSG_TYPE_ACC)
407  {
408  this->boards[board].accTimeStamp=timeNow;
409  privateData[vectorOffset+0]= (signed short) ((buff[1]<<8) + buff[0]);
410  privateData[vectorOffset+1]= (signed short) ((buff[3]<<8) + buff[2]);
411  privateData[vectorOffset+2]= (signed short) ((buff[5]<<8) + buff[4]);
412  setPrivateBoardAccStatus(board,IAnalogSensor::AS_OK);
413  }
414 
415  if (id==boardId && this->boards[board].enabledGyro && msg_class==CAN_MSG_CLASS_ACC_GYRO && msg_type == MSG_TYPE_GYRO)
416  {
417  this->boards[board].gyroTimeStamp=timeNow;
418  privateData[vectorOffset+3]= (signed short) (buff[1]<<8)+buff[0];
419  privateData[vectorOffset+4]= (signed short) (buff[3]<<8)+buff[2];
420  privateData[vectorOffset+5]= (signed short) (buff[5]<<8)+buff[4];
421  setPrivateBoardGyroStatus(board,IAnalogSensor::AS_OK);
422  }
423 
424  }
425 
426  }
427 
428 
429  if(initted)
430  {
431  for(size_t board=0; board < this->boards.size(); board++ )
432  {
433  unsigned short boardId = this->boards[board].boardId;
434 
435  //if 100ms have passed since the last received message for gyro
436  double delay = timeNow-this->boards[board].gyroTimeStamp;
437  if (delay > CANBUS_INERTIAL_MTB_TIMEOUT && this->boards[board].enabledGyro)
438  {
439  yError("CanBusInertialMTB::run(): gyroscope read for board %x timed out (last received %lf sec ago)",boardId,delay);
440  setPrivateBoardGyroStatus(board,IAnalogSensor::AS_TIMEOUT);
441  }
442 
443  //if 100ms have passed since the last received message for acc
444  delay = timeNow-this->boards[board].accTimeStamp;
445  if (delay > CANBUS_INERTIAL_MTB_TIMEOUT)
446  {
447  yError("CanBusInertialMTB::run(): accelerometer read for board %x timed out (last received %lf sec ago)",boardId,delay);
448  setPrivateBoardAccStatus(board,IAnalogSensor::AS_TIMEOUT);
449  }
450  }
451  }
452  else
453  {
454  // wait some time to have the device ready and avoid spurious timeout messages at startup
455  count++;
456  if(count == 10)
457  initted=true;
458  }
459 
460 
461  //Compute the global status
462  this->privateGlobalStatus = IAnalogSensor::AS_OK;
463  for(int ch=0; ch < this->nrOfTotalChannels; ch+= 3)
464  {
465  if( this->privateStatus[ch] == IAnalogSensor::AS_ERROR )
466  {
467  this->privateGlobalStatus = IAnalogSensor::AS_ERROR;
468  break;
469  }
470  if( this->privateStatus[ch] == IAnalogSensor::AS_TIMEOUT )
471  {
472  this->privateGlobalStatus = IAnalogSensor::AS_TIMEOUT;
473  break;
474  }
475  }
476 
477  // Copy the data in the output data
478  lock_guard<mutex> lck(mtx);
479  memcpy(data.data(), privateData.data(), sizeof(double)*privateData.size());
480  for(size_t ch=0; ch < privateStatus.size(); ch++ )
481  {
482  this->sharedStatus[ch] = this->privateStatus[ch];
483  }
484  this->sharedGlobalStatus = this->privateGlobalStatus;
485 
486  return;
487 }
488 
490 {
491  for(size_t board=0; board < this->boards.size(); board++ )
492  {
493  unsigned int canMessages=0;
494  unsigned id = 0x200 + this->boards[board].boardId;
495 
496  CanMessage &msg=outBuffer[0];
497  msg.setId(id);
498  msg.getData()[0]=ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP; // message type
499  msg.getData()[1]=0x0; // disable all the sensors
500  msg.getData()[2]=0x1; // period (ms)
501  msg.setLen(3);
502  canMessages=0;
503  bool ret = pCanBus->canWrite(outBuffer, 1, &canMessages);
504 
505  if( !ret )
506  {
507  yError("CanBusInertialMTB: canWrite returned false for sensor with address %x",this->boards[board].boardId);
508  }
509  }
510  return;
511 }
512 
const char CANBUS_INERTIAL_MTB_EXTERNAL_GYRO_BIT
const unsigned int CAN_MSG_CLASS_ACC_GYRO
const double CANBUS_INERTIAL_MTB_TIMEOUT
bool checkRequiredParamIsInt(yarp::os::Searchable &config, const std::string &paramName)
const char CANBUS_INERTIAL_MTB_EXTERNAL_ACC_BIT
bool checkRequiredParamIsVectorOfInt(yarp::os::Searchable &config, const std::string &paramName, std::vector< int > &output_vector)
const unsigned int MSG_TYPE_GYRO
const char CANBUS_INERTIAL_MTB_INTERNAL_ACC_BIT
const unsigned short CANBUS_INERTIAL_MTB_DEFAULT_SENSOR_PERIOD
const unsigned int MSG_TYPE_ACC
const int CANBUS_INERTIAL_MTB_CAN_DRIVER_BUFFER_SIZE
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable &config, const std::string &paramName, std::vector< std::string > &output_vector)
bool checkRequiredParamIsString(yarp::os::Searchable &config, const std::string &paramName)
@ data
virtual void threadRelease()
virtual int read(yarp::sig::Vector &out)
virtual int getChannels()
virtual bool open(yarp::os::Searchable &config)
virtual int getState(int ch)
virtual int calibrateChannel(int ch, double v)
virtual bool threadInit()
uint8_t board
static int stop
Definition: iCub_Sim.cpp:41
static int v
Definition: iCub_Sim.cpp:42
out
Definition: sine.m:8