YARP
Yet Another Robot Platform
imuBosch_BNO055.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: LGPL-2.1-or-later
4  */
5 
6 #include <arpa/inet.h>
7 #include <cerrno> // Error number definitions
8 #include <cmath>
9 #include <cstdlib>
10 #include <cstring>
11 #include <fcntl.h> // File control definitions
12 #include <iostream>
13 #include <termios.h> // terminal io (serial port) interface
14 #include <unistd.h>
15 
16 #include <linux/i2c-dev.h>
17 #ifdef I2C_HAS_SMBUS_H
18 extern "C" {
19 # include <i2c/smbus.h>
20 }
21 #endif
22 #include <linux/kernel.h>
23 
24 #include <sys/ioctl.h>
25 #include <sys/stat.h>
26 #include <sys/types.h>
27 
28 #include <mutex>
29 #include <yarp/os/Log.h>
30 #include <yarp/os/LogComponent.h>
31 #include <yarp/os/LogStream.h>
32 #include <yarp/math/Math.h>
33 #include <yarp/os/Time.h>
34 
35 #include "imuBosch_BNO055.h"
36 
37 using namespace yarp::os;
38 using namespace yarp::dev;
39 
40 namespace {
41 YARP_LOG_COMPONENT(IMUBOSCH_BNO055, "yarp.device.imuBosch_BNO055")
42 constexpr uint8_t i2cAddrA = 0x28;
43 }
44 
45 //constexpr uint8_t i2cAddrB = 0x29;
46 
48  verbose(false),
49  status(0),
50  nChannels(12),
51  m_timeStamp(0.0),
52  timeLastReport(0.0),
53  i2c_flag(false),
54  checkError(false),
55  fd(0),
56  responseOffset(0),
57  readFunc(&BoschIMU::sendReadCommandSer),
58  totMessagesRead(0),
59  errs(0),
60  dataIsValid(false)
61 {
62  data.resize(12);
63  data.zero();
64  data_tmp.resize(12);
65  data_tmp.zero();
66  errorCounter.resize(11);
67  errorCounter.zero();
68 }
69 
70 BoschIMU::~BoschIMU() = default;
71 
72 
74 {
75  //debug
76  yCTrace(IMUBOSCH_BNO055, "Parameters are:\n\t%s", config.toString().c_str());
77 
78  if(!config.check("comport") && !config.check("i2c"))
79  {
80  yCError(IMUBOSCH_BNO055) << "Params 'comport' and 'i2c' not found";
81  return false;
82  }
83 
84  if (config.check("comport") && config.check("i2c"))
85  {
86  yCError(IMUBOSCH_BNO055) << "Params 'comport' and 'i2c' both specified";
87  return false;
88  }
89 
90  i2c_flag = config.check("i2c");
91 
93 
94  // In case of reading through serial the first two bytes of the response are the ack, so
95  // they need to be discarded when publishing the data.
96  responseOffset = i2c_flag ? 0 : 2;
97 
98  if (i2c_flag)
99  {
100  if (!config.find("i2c").isString())
101  {
102  yCError(IMUBOSCH_BNO055) << "i2c param malformed, it should be a string, aborting.";
103  return false;
104  }
105 
106  std::string i2cDevFile = config.find("i2c").asString();
107  fd = ::open(i2cDevFile.c_str(), O_RDWR);
108 
109  if (fd < 0)
110  {
111  yCError(IMUBOSCH_BNO055, "Can't open %s, %s", i2cDevFile.c_str(), strerror(errno));
112  return false;
113  }
114 
115  if (::ioctl(fd, I2C_SLAVE, i2cAddrA) < 0)
116  {
117  yCError(IMUBOSCH_BNO055, "ioctl failed on %s, %s", i2cDevFile.c_str(), strerror(errno));
118  return false;
119  }
120 
121  }
122  else
123  {
124  fd = ::open(config.find("comport").toString().c_str(), O_RDWR | O_NOCTTY );
125  if (fd < 0) {
126  yCError(IMUBOSCH_BNO055, "Can't open %s, %s", config.find("comport").toString().c_str(), strerror(errno));
127  return false;
128  }
129  //Get the current options for the port...
130  struct termios options;
131  tcgetattr(fd, &options);
132 
133  cfmakeraw(&options);
134 
135  //set the baud rate to 115200
136  int baudRate = B115200;
137  cfsetospeed(&options, baudRate);
138  cfsetispeed(&options, baudRate);
139 
140  //set the number of data bits.
141  options.c_cflag &= ~CSIZE; // Mask the character size bits
142  options.c_cflag |= CS8;
143 
144  //set the number of stop bits to 1
145  options.c_cflag &= ~CSTOPB;
146 
147  //Set parity to None
148  options.c_cflag &=~PARENB;
149 
150  //set for non-canonical (raw processing, no echo, etc.)
151  // options.c_iflag = IGNPAR; // ignore parity check
152  options.c_oflag = 0; // raw output
153  options.c_lflag = 0; // raw input
154 
155  // SET NOT BLOCKING READ
156  options.c_cc[VMIN] = 0; // block reading until RX x characters. If x = 0, it is non-blocking.
157  options.c_cc[VTIME] = 2; // Inter-Character Timer -- i.e. timeout= x*.1 s
158 
159  //Set local mode and enable the receiver
160  options.c_cflag |= (CLOCAL | CREAD);
161 
162  tcflush(fd, TCIOFLUSH);
163 
164  //Set the new options for the port...
165  if ( tcsetattr(fd, TCSANOW, &options) != 0)
166  {
167  yCError(IMUBOSCH_BNO055, "Configuring comport failed");
168  return false;
169  }
170 
171  }
172 
173  nChannels = config.check("channels", Value(12)).asInt32();
174 
175  double period = config.check("period",Value(10),"Thread period in ms").asInt32() / 1000.0;
176  setPeriod(period);
177 
178  if (config.check("sensor_name") && config.find("sensor_name").isString())
179  {
180  m_sensorName = config.find("sensor_name").asString();
181  }
182  else
183  {
184  m_sensorName = "sensor_imu_bosch_bno055";
185  yCWarning(IMUBOSCH_BNO055) << "Parameter \"sensor_name\" not set. Using default value \"" << m_sensorName << "\" for this parameter.";
186  }
187 
188  if (config.check("frame_name") && config.find("frame_name").isString())
189  {
190  m_frameName = config.find("frame_name").asString();
191  }
192  else
193  {
194  m_frameName = m_sensorName;
195  yCWarning(IMUBOSCH_BNO055) << "Parameter \"frame_name\" not set. Using the same value as \"sensor_name\" for this parameter.";
196  }
197 
198  return PeriodicThread::start();
199 }
200 
202 {
203  yCTrace(IMUBOSCH_BNO055);
204  //stop the thread
206  return true;
207 }
208 
209 bool BoschIMU::checkReadResponse(unsigned char* response)
210 {
211  if(response[0] == (unsigned char) REPLY_HEAD)
212  {
213  return true;
214  }
215 
216  if(response[0] == (unsigned char) ERROR_HEAD)
217  {
218  if(response[1] != REGISTER_NOT_READY) // if error is 0x07, do not print error messages
219  {
220  yCError(IMUBOSCH_BNO055,
221  "Inertial sensor didn't understand the command. \n\
222  If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
223  }
224  errorCounter[response[1]]++;
225  readSysError();
226  return false;
227  }
228 
229  errorCounter[0]++;
230  yCError(IMUBOSCH_BNO055,
231  "Received unknown response message. \n\
232  If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
233  dropGarbage();
234  readSysError();
235  return false;
236 }
237 
238 bool BoschIMU::checkWriteResponse(unsigned char* response)
239 {
240  if(response[0] == (unsigned char) ERROR_HEAD)
241  {
242  if(response[1] == (unsigned char) WRITE_SUCC)
243  {
244  return true;
245  }
246  if(response[1] != REGISTER_NOT_READY) // if error is 0x07, do not print error messages
247  {
248  yCError(IMUBOSCH_BNO055,
249  "Inertial sensor didn't understand the command. \n\
250  If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
251  }
252  errorCounter[response[1]]++;
253  readSysError();
254  return false;
255  }
256 
257  errorCounter[0]++;
258  yCError(IMUBOSCH_BNO055,
259  "Received unknown response message. \n\
260  If this error happens more than once in a row, please check serial communication is working fine and it isn't affected by electrical disturbance.");
261  dropGarbage();
262  readSysError();
263  return false;
264 }
265 
266 bool BoschIMU::sendReadCommandSer(unsigned char register_add, int len, unsigned char* buf, std::string comment)
267 {
268  int command_len;
269  int nbytes_w;
270 
271  //
272  // Create a READ message
273  //
274  bool success = false;
275  for(int trials=0; (trials<3) && (success==false); trials++)
276  {
277  totMessagesRead++;
278  command_len = 4;
279  command[0]= START_BYTE; // start byte
280  command[1]= READ_CMD; // read operation
281  command[2]= register_add; // register to read
282  command[3]= len; // length in bytes
283 
284 // yCTrace(IMUBOSCH_BNO055, "> READ_COMMAND: %s ... ", comment.c_str());
285 // yCTrace(IMUBOSCH_BNO055, "Command is:");
286 // printBuffer(command, command_len);
287 
288  nbytes_w = ::write(fd, (void*)command, command_len);
289 
290  if(nbytes_w != command_len)
291  {
292  yCError(IMUBOSCH_BNO055) << "Cannot correctly send the message: " << comment;
293  // DO NOT return here. If something was sent, then the imu will reply with a message
294  // even an error message. I have to parse it before proceeding and not leave garbage behind.
295  }
296  // Read the write reply
297  memset(buf, 0x00, 20);
298  int readbytes = readBytes(buf, RESP_HEADER_SIZE);
299  if(readbytes != RESP_HEADER_SIZE)
300  {
301  yCError(IMUBOSCH_BNO055, "Expected %d bytes, read %d instead", RESP_HEADER_SIZE, readbytes);
302  success = false;
303  }
304  else if(!checkReadResponse(buf))
305  {
306  success = false;
308  }
309  else
310  {
311  success = true;
312 // yCTrace(IMUBOSCH_BNO055, "> SUCCESS!"); fflush(stdout);
313 
314  // Read the data payload
315  readBytes(&buf[2], (int) buf[1]);
316 // yCTrace(IMUBOSCH_BNO055, "\tReply is:");
317 // printBuffer(buf, buf[1]+2);
318 // yCTrace(IMUBOSCH_BNO055, "***************");
319  }
320  }
321 // if(!success)
322 // yCError(IMUBOSCH_BNO055, "> FAILED reading %s!", comment.c_str());
323  return success;
324 }
325 
326 bool BoschIMU::sendWriteCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment)
327 {
328  int command_len = 4+len;
329  int nbytes_w;
330 
331  command[0]= START_BYTE; // start byte
332  command[1]= WRITE_CMD; // read operation
333  command[2]= register_add; // operation mode register
334  command[3]= (unsigned char) len; // length 1 byte
335  for (int i = 0; i < len; i++) {
336  command[4 + i] = cmd[i]; // data
337  }
338 
339 // yCTrace(IMUBOSCH_BNO055, "> WRITE_COMMAND: %s ... ", comment.c_str());
340 // yCTrace(IMUBOSCH_BNO055, "Command is:");
341 // printBuffer(command, command_len);
342 
343  nbytes_w = ::write(fd, (void*)command, command_len);
344  if(nbytes_w != command_len)
345  {
346  yCError(IMUBOSCH_BNO055) << "Cannot correctly send the message: " << comment;
347  // DO NOT return here. If something was sent, then the imu will reply with a message
348  // even an error message. I have to parse it before proceeding and not leave garbage behind.
349  }
350 
351  // Read the write reply
352  memset(response, 0x00, 20);
353  readBytes(response, 2);
354  if(!checkWriteResponse(response))
355  {
356 // yCTrace(IMUBOSCH_BNO055, "> FAILED!"); fflush(stdout);
357  yCError(IMUBOSCH_BNO055) << "FAILED writing " << comment;
358  return false;
359  }
360 // yCTrace(IMUBOSCH_BNO055, "> SUCCESS!"); fflush(stdout);
361 // yCTrace(IMUBOSCH_BNO055, "\tReply is:");
362 // printBuffer(response, 2);
363 // yCTrace(IMUBOSCH_BNO055, "***************");
364  return true;
365 }
366 
367 int BoschIMU::readBytes(unsigned char* buffer, int bytes)
368 {
369  int r = 0;
370  int bytesRead = 0;
371  do
372  {
373  r = ::read(fd, (void*)&buffer[bytesRead], 1);
374  if (r > 0) {
375  bytesRead += r;
376  }
377  }
378  while(r!=0 && bytesRead < bytes);
379 
380  return bytesRead;
381 }
382 
384 {
385  char byte;
386  while( (::read(fd, (void*) &byte, 1) > 0 ))
387  {
388 // yCTrace(IMUBOSCH_BNO055, "Dropping byte 0x%02X", byte);
389  }
390  return;
391 }
392 
393 void BoschIMU::printBuffer(unsigned char* buffer, int length)
394 {
395  for (int i = 0; i < length; i++) {
396  printf("\t0x%02X ", buffer[i]);
397  }
398  printf("\n");
399 }
400 
402 {
403  // avoid recursive error check
404  if (checkError) {
405  return;
406  }
407 
408  checkError = true;
410  if(!sendReadCommandSer(REG_SYS_STATUS, 1, response, "Read SYS_STATUS register") )
411  {
412  yCError(IMUBOSCH_BNO055) << "@ line " << __LINE__;
413  }
414 
415  if(!sendReadCommandSer(REG_SYS_ERR, 1, response, "Read SYS_ERR register") )
416  {
417  yCError(IMUBOSCH_BNO055) << "@ line " << __LINE__;
418  }
419  checkError = false;
420  return;
421 }
422 
423 bool BoschIMU::sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment)
424 {
425  uint8_t attempts=0;
426  bool ret;
427  do
428  {
429  ret=sendWriteCommandSer(register_add, len, cmd, comment);
430  attempts++;
431  }while((attempts<= ATTEMPTS_NUM_OF_SEND_CONFIG_CMD) && (ret==false));
432 
433  return(ret);
434 }
435 
436 bool BoschIMU::sendReadCommandI2c(unsigned char register_add, int len, unsigned char* buf, std::string comment)
437 {
438  if (i2c_smbus_read_i2c_block_data(fd, register_add, len, buf) < 0)
439  {
440  yCError(IMUBOSCH_BNO055) << "Cannot correctly send the message: " << comment;
441  return false;
442  }
443  return true;
444 }
445 
447 {
448  if (i2c_flag)
449  {
450  int trials = 0;
451  // Make sure we have the right device
452  while (i2c_smbus_read_byte_data(fd, REG_CHIP_ID) != BNO055_ID)
453  {
454  if (trials == 10)
455  {
456  yCError(IMUBOSCH_BNO055) << "Wrong device on the bus, it is not BNO055";
457  return false;
458  }
460  trials++;
461 
462  }
463 
465 
466  // Set the device in config mode
467  if (i2c_smbus_write_byte_data(fd, REG_OP_MODE, CONFIG_MODE) < 0)
468  {
469  yCError(IMUBOSCH_BNO055) << "Unable to set the Config mode";
470  return false;
471  }
472 
474 
475  if (i2c_smbus_write_byte_data(fd, REG_SYS_TRIGGER, TRIG_EXT_CLK_SEL) < 0)
476  {
477  yCError(IMUBOSCH_BNO055) << "Unable to set external clock";
478  return false;
479  }
480 
482 
483  // Perform any required configuration
484 
485 
486  if (i2c_smbus_write_byte_data(fd, REG_PAGE_ID, 0x00) < 0)
487  {
488  yCError(IMUBOSCH_BNO055) << "Unable to set the page ID";
489  return false;
490  }
491 
493  // Set the device into operative mode
494 
495  if (i2c_smbus_write_byte_data(fd, REG_OP_MODE, NDOF_MODE) < 0)
496  {
497  yCError(IMUBOSCH_BNO055) << "Unable to set the Operative mode";
498  return false;
499  }
500 
502  }
503  else
504  {
505  unsigned char msg;
506  timeLastReport = yarp::os::SystemClock::nowSystem();
507 
508  msg = 0x00;
509  if(!sendAndVerifyCommandSer(REG_PAGE_ID, 1, &msg, "PAGE_ID") )
510  {
511  yCError(IMUBOSCH_BNO055) << "Set page id 0 failed";
512  return(false);
513  }
514 
516 
517  //Removed because useless
519  //
520  // Set power mode
521  //
523  // msg = 0x00;
524  // if(!sendAndVerifyCommand(REG_POWER_MODE, 1, &msg, "Set power mode") )
525  // {
526  // yCError(IMUBOSCH_BNO055) << "Set power mode failed";
527  // return(false);
528  // }
529  //
530  // yarp::os::SystemClock::delaySystem(SWITCHING_TIME);
531 
532 
534  //
535  // Set the device in config mode
536  //
538 
539  msg = CONFIG_MODE;
540  if(!sendAndVerifyCommandSer(REG_OP_MODE, 1, &msg, "Set config mode") )
541  {
542  yCError(IMUBOSCH_BNO055) << "Set config mode failed";
543  return(false);
544  }
545 
547 
548 
550  //
551  // Set external clock
552  //
554 
555  msg = TRIG_EXT_CLK_SEL;
556  if(!sendAndVerifyCommandSer(REG_SYS_TRIGGER, 1, &msg, "Set external clock") )
557  {
558  yCError(IMUBOSCH_BNO055) << "Set external clock failed";
559  return(false);
560  }
562 
564  //
565  // Perform any required configuration
566  //
568 
569 
570 
572 
574  //
575  // Set the device into operative mode
576  //
578 
579  msg = NDOF_MODE;
580  if(!sendAndVerifyCommandSer(REG_OP_MODE, 1, &msg, "Set config NDOF_MODE") )
581  {
582  yCError(IMUBOSCH_BNO055) << "Set config NDOF_MODE failed";
583  return false;
584  }
585 
587  }
588 
589  // Do a first read procedure to verify everything is fine.
590  // In case the device fails to read, stop it and quit
591  for(int i=0; i<10; i++)
592  {
593  // read data from IMU
594  run();
595  if (dataIsValid) {
596  break;
597  } else {
599  }
600  }
601 
602  if(!dataIsValid)
603  {
604  yCError(IMUBOSCH_BNO055) << "First read from the device failed, check everything is fine and retry";
605  return false;
606  }
607 
608  return true;
609 }
610 
612 {
613  m_timeStamp = yarp::os::SystemClock::nowSystem();
614 
615  int16_t raw_data[16];
616 
617  // In order to avoid zeros when a single read from a sensor is missing,
618  // initialize the new measure to be equal to the previous one
619  data_tmp = data;
620 
621 
622  if (!(this->*readFunc)(REG_ACC_DATA, 32, response, "Read all"))
623  {
624  yCError(IMUBOSCH_BNO055) << "Failed to read all the data";
625  errs++;
626  dataIsValid = false;
627  return;
628  }
629  else
630  {
631  // Correctly construct int16 data
632  for(int i=0; i<16; i++)
633  {
634  raw_data[i] = response[responseOffset+1+i*2] << 8 | response[responseOffset+i*2];
635  }
636 
637  // Get quaternion
638  quaternion_tmp = quaternion;
639  quaternion_tmp.w() = ((double)raw_data[12]) / (2 << 13);
640  quaternion_tmp.x() = ((double)raw_data[13]) / (2 << 13);
641  quaternion_tmp.y() = ((double)raw_data[14]) / (2 << 13);
642  quaternion_tmp.z() = ((double)raw_data[15]) / (2 << 13);
643 
644  // Convert to RPY angles
645  RPY_angle.resize(3);
646 
647  // Check quaternion values are meaningful. The aim of this check is simply
648  // to verify values are not garbage.
649  // Ideally the correct check is that quaternion.abs ~= 1, but to avoid
650  // calling a sqrt every cicle only for a rough estimate, the check here
651  // is that the self product is nearly 1
652  double sum_squared = quaternion_tmp.w() * quaternion_tmp.w() +
653  quaternion_tmp.x() * quaternion_tmp.x() +
654  quaternion_tmp.y() * quaternion_tmp.y() +
655  quaternion_tmp.z() * quaternion_tmp.z();
656 
657  if( (sum_squared < 0.9) || (sum_squared > 1.2) )
658  {
659  dataIsValid = false;
660  return;
661  }
662 
663  dataIsValid = true;
664  RPY_angle = yarp::math::dcm2rpy(quaternion.toRotationMatrix4x4());
665  data_tmp[0] = RPY_angle[0] * 180 / M_PI;
666  data_tmp[1] = RPY_angle[1] * 180 / M_PI;
667  data_tmp[2] = RPY_angle[2] * 180 / M_PI;
668 
669  // Fill in accel values
670  data_tmp[3] = (double)raw_data[0] / 100.0;
671  data_tmp[4] = (double)raw_data[1] / 100.0;
672  data_tmp[5] = (double)raw_data[2] / 100.0;
673 
674 
675  // Fill in Gyro values
676  data_tmp[6] = (double)raw_data[6] / 16.0;
677  data_tmp[7] = (double)raw_data[7] / 16.0;
678  data_tmp[8] = (double)raw_data[8] / 16.0;
679 
680  // Fill in Magnetometer values
681  data_tmp[9] = (double)raw_data[3] / 16.0;
682  data_tmp[10] = (double)raw_data[4] / 16.0;
683  data_tmp[11] = (double)raw_data[5] / 16.0;
684  }
685 
686  // Protect only this section in order to avoid slow race conditions when gathering this data
687  {
688  std::lock_guard<std::mutex> guard(mutex);
689  data = data_tmp;
690  quaternion = quaternion_tmp;
691  }
692 
693  if (m_timeStamp > timeLastReport + TIME_REPORT_INTERVAL) {
694  // if almost 1 errors occourred in last interval, then print report
695  if(errs != 0)
696  {
697  yCDebug(IMUBOSCH_BNO055, "Periodic error report of last %d sec:", TIME_REPORT_INTERVAL);
698  yCDebug(IMUBOSCH_BNO055, "\t errors while reading data: %d", errs);
699  }
700 
701  errs = 0;
702  timeLastReport=m_timeStamp;
703  }
704 }
705 
707 {
708  std::lock_guard<std::mutex> guard(mutex);
709  out.resize(nChannels);
710  out.zero();
711 
712  out = data;
713  if(nChannels == 16)
714  {
715  out[12] = quaternion.w();
716  out[13] = quaternion.x();
717  out[14] = quaternion.y();
718  out[15] = quaternion.z();
719  }
720 
721  return dataIsValid;
722 }
723 
725 {
726  *nc = nChannels;
727  return true;
728 }
729 
730 bool BoschIMU::calibrate(int ch, double v)
731 {
732  // TODO: start a calib procedure in which the calib status register is read
733  // until all sensors are calibrated (0xFFFF). Then the offsets are saved
734  // into memory for the next run.
735  // This procedure should be abortable by CTRL+C
736  return false;
737 }
738 
739 
740 yarp::dev::MAS_status BoschIMU::genericGetStatus(size_t sens_index) const
741 {
742  if (sens_index != 0)
743  {
744  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
746  }
747 
749 }
750 
751 bool BoschIMU::genericGetSensorName(size_t sens_index, std::string& name) const
752 {
753  if (sens_index != 0)
754  {
755  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
756  return false;
757  }
758 
759  name = m_sensorName;
760  return true;
761 }
762 
763 bool BoschIMU::genericGetFrameName(size_t sens_index, std::string& frameName) const
764 {
765  if (sens_index != 0)
766  {
767  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
768  return false;
769  }
770 
771  frameName = m_frameName;
772  return true;
773 
774 }
775 
777 {
778  return 1;
779 }
780 
781 
783 {
784  return genericGetStatus(sens_index);
785 }
786 
787 bool BoschIMU::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string& name) const
788 {
789  return genericGetSensorName(sens_index, name);
790 }
791 
792 bool BoschIMU::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string& frameName) const
793 {
794  return genericGetFrameName(sens_index, frameName);
795 }
796 
797 bool BoschIMU::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
798 {
799  if (sens_index != 0)
800  {
801  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
802  return false;
803  }
804 
805  out.resize(3);
806  std::lock_guard<std::mutex> guard(mutex);
807  out[0] = data[3];
808  out[1] = data[4];
809  out[2] = data[5];
810 
811  timestamp = m_timeStamp;
812  return true;
813 }
814 
815 
817 {
818  return 1;
819 }
820 
821 
823 {
824  return genericGetStatus(sens_index);
825 }
826 
827 bool BoschIMU::getThreeAxisGyroscopeName(size_t sens_index, std::string& name) const
828 {
829  return genericGetSensorName(sens_index, name);
830 }
831 
832 bool BoschIMU::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const
833 {
834  return genericGetFrameName(sens_index, frameName);
835 }
836 
837 bool BoschIMU::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
838 {
839  if (sens_index != 0)
840  {
841  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
842  return false;
843  }
844 
845  out.resize(3);
846  std::lock_guard<std::mutex> guard(mutex);
847  out[0] = data[6];
848  out[1] = data[7];
849  out[2] = data[8];
850 
851  timestamp = m_timeStamp;
852  return true;
853 }
854 
856 {
857  return 1;
858 }
859 
861 {
862  return genericGetStatus(sens_index);
863 }
864 
865 bool BoschIMU::getOrientationSensorName(size_t sens_index, std::string& name) const
866 {
867  return genericGetSensorName(sens_index, name);
868 }
869 
870 bool BoschIMU::getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const
871 {
872  return genericGetFrameName(sens_index, frameName);
873 }
874 
875 bool BoschIMU::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy, double& timestamp) const
876 {
877  if (sens_index != 0)
878  {
879  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
880  return false;
881  }
882 
883  rpy.resize(3);
884  std::lock_guard<std::mutex> guard(mutex);
885  rpy[0] = data[0];
886  rpy[1] = data[1];
887  rpy[2] = data[2];
888 
889  timestamp = m_timeStamp;
890  return true;
891 }
892 
894 {
895  return 1;
896 }
897 
899 {
900  return genericGetStatus(sens_index);
901 }
902 
903 bool BoschIMU::getThreeAxisMagnetometerName(size_t sens_index, std::string& name) const
904 {
905  return genericGetSensorName(sens_index, name);
906 }
907 
908 bool BoschIMU::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string& frameName) const
909 {
910  return genericGetFrameName(sens_index, frameName);
911 }
912 
913 bool BoschIMU::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
914 {
915  if (sens_index != 0)
916  {
917  yCError(IMUBOSCH_BNO055) << "sens_index must be equal to 0, since there is only one sensor in consideration";
918  return false;
919  }
920 
921  out.resize(3);
922  std::lock_guard<std::mutex> guard(mutex);
923  // The unit measure of Bosch BNO055 is uT
924  out[0] = data[9] / 1000000;
925  out[1] = data[10]/ 1000000;
926  out[2] = data[11]/ 1000000;
927 
928  timestamp = m_timeStamp;
929  return true;
930 }
931 
932 
934 {
935  yCTrace(IMUBOSCH_BNO055, "Thread released");
936  //TBD write more meaningful report
937 // for(unsigned int i=0; i<errorCounter.size(); i++)
938 // yCTrace(IMUBOSCH_BNO055, "Error type %d, counter is %d", i, (int)errorCounter[i]);
939 // yCTrace(IMUBOSCH_BNO055, "On overall read operations of %ld", totMessagesRead);
940  ::close(fd);
941 }
bool ret
imuBosch_BNO055: This device will connect to the proper analogServer and read the data broadcasted ma...
void threadRelease() override
Terminate communication with the device and release the thread.
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get three axis gyroscope measurements.
bool sendReadCommandSer(unsigned char register_add, int len, unsigned char *buf, std::string comment="")
bool checkReadResponse(unsigned char *response)
size_t getNrOfThreeAxisMagnetometers() const override
Get the number of three axis magnetometers in the device.
void printBuffer(unsigned char *buffer, int length)
bool getChannels(int *nc) override
Get the number of channels of the sensor.
bool sendReadCommandI2c(unsigned char register_add, int len, unsigned char *buf, std::string comment="")
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis linear accelerometer measurements are expressed.
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis gyroscope measurements are expressed.
bool close() override
Close the device.
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which orientation sensor measurements are expressed.
void dropGarbage()
bool threadInit() override
Initialize process with desired device configurations.
bool sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char *cmd, std::string comment)
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of three axis gyroscope.
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
Get orientation sensor measurements.
bool checkWriteResponse(unsigned char *response)
bool calibrate(int ch, double v) override
Calibrate the sensor, single channel.
bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame in which three axis magnetometer measurements are expressed.
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of orientation sensor.
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of three axis linear accelerometer.
bool read(yarp::sig::Vector &out) override
Read a vector from the sensor.
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of orientation sensor.
void readSysError()
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of three axis gyroscope.
bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
Get the name of three axis magnetometer.
bool sendWriteCommandSer(unsigned char register_add, int len, unsigned char *cmd, std::string comment="")
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
Get the status of three axis magnetometer.
int readBytes(unsigned char *buffer, int bytes)
size_t getNrOfThreeAxisGyroscopes() const override
Get the number of three axis gyroscopes in the device.
size_t getNrOfThreeAxisLinearAccelerometers() const override
Get the number of three axis linear accelerometers in the device.
bool open(yarp::os::Searchable &config) override
Open the device and set up parameters/communication.
size_t getNrOfOrientationSensors() const override
Get the number of orientation sensors in the device.
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get three axis linear accelerometer measurements.
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of three axis linear accelerometer.
bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get three axis magnetometer measurements.
void run() override
Update loop where measurements are read from the device.
An abstraction for a periodic thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
Definition: Searchable.h:63
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
static double nowSystem()
Definition: SystemClock.cpp:34
static void delaySystem(double seconds)
Definition: SystemClock.cpp:29
A single value (typically within a Bottle).
Definition: Value.h:43
virtual bool isString() const
Checks if value is a string.
Definition: Value.cpp:156
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Value.cpp:356
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:220
void zero()
Zero the elements of the vector.
Definition: Vector.h:343
#define M_PI
#define REG_SYS_ERR
#define NDOF_MODE
#define REPLY_HEAD
#define REG_SYS_STATUS
#define READ_CMD
#define REG_OP_MODE
#define START_BYTE
#define REG_PAGE_ID
#define SWITCHING_TIME
#define ATTEMPTS_NUM_OF_SEND_CONFIG_CMD
#define BNO055_ID
#define RESP_HEADER_SIZE
#define WRITE_SUCC
#define REG_SYS_TRIGGER
#define ERROR_HEAD
#define CONFIG_MODE
#define REGISTER_NOT_READY
#define TRIG_EXT_CLK_SEL
#define REG_CHIP_ID
#define TIME_REPORT_INTERVAL
#define REG_ACC_DATA
#define WRITE_CMD
unsigned char byte
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCTrace(component,...)
Definition: LogComponent.h:84
#define yCWarning(component,...)
Definition: LogComponent.h:192
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_ERROR
The sensor is in generic error state.
@ MAS_OK
The sensor is working correctly.
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition: math.cpp:808
void delay(double seconds)
Wait for a certain number of seconds.
Definition: Time.cpp:111
An interface to the operating system, including Port based communication.
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:915
bool write(const ImageOf< PixelRgb > &src, const std::string &dest, image_fileformat format=FORMAT_PPM)
Definition: ImageFile.cpp:1091