YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
18extern "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>
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
37using namespace yarp::os;
38using namespace yarp::dev;
39
40namespace {
41YARP_LOG_COMPONENT(IMUBOSCH_BNO055, "yarp.device.imuBosch_BNO055")
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
70BoschIMU::~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
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{
204 //stop the thread
206 return true;
207}
208
209bool 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 {
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 }
225 readSysError();
226 return false;
227 }
228
229 errorCounter[0]++;
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
238bool 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 {
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 }
253 readSysError();
254 return false;
255 }
256
257 errorCounter[0]++;
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
266bool 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 {
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);
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
326bool 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);
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
367int 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
393void 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
423bool BoschIMU::sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment)
424{
426 bool ret;
427 do
428 {
430 attempts++;
431 }while((attempts<= ATTEMPTS_NUM_OF_SEND_CONFIG_CMD) && (ret==false));
432
433 return(ret);
434}
435
436bool BoschIMU::sendReadCommandI2c(unsigned char register_add, int len, unsigned char* buf, std::string comment)
437{
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
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
468 {
469 yCError(IMUBOSCH_BNO055) << "Unable to set the Config mode";
470 return false;
471 }
472
474
476 {
477 yCError(IMUBOSCH_BNO055) << "Unable to set external clock";
478 return false;
479 }
480
482
483 // Perform any required configuration
484
485
487 {
488 yCError(IMUBOSCH_BNO055) << "Unable to set the page ID";
489 return false;
490 }
491
493 // Set the device into operative mode
494
496 {
497 yCError(IMUBOSCH_BNO055) << "Unable to set the Operative mode";
498 return false;
499 }
500
502 }
503 else
504 {
505 unsigned char msg;
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{
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 {
635 }
636
637 // Get 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
656
657 if( (sum_squared < 0.9) || (sum_squared > 1.2) )
658 {
659 dataIsValid = false;
660 return;
661 }
662
663 dataIsValid = true;
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;
691 }
692
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;
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
730bool 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
740yarp::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
751bool 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
763bool 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
786
788{
789 return genericGetSensorName(sens_index, name);
790}
791
792bool BoschIMU::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string& frameName) const
793{
794 return genericGetFrameName(sens_index, frameName);
795}
796
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
827bool BoschIMU::getThreeAxisGyroscopeName(size_t sens_index, std::string& name) const
828{
829 return genericGetSensorName(sens_index, name);
830}
831
832bool BoschIMU::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string& frameName) const
833{
834 return genericGetFrameName(sens_index, frameName);
835}
836
837bool 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
865bool BoschIMU::getOrientationSensorName(size_t sens_index, std::string& name) const
866{
867 return genericGetSensorName(sens_index, name);
868}
869
870bool BoschIMU::getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const
871{
872 return genericGetFrameName(sens_index, frameName);
873}
874
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
903bool BoschIMU::getThreeAxisMagnetometerName(size_t sens_index, std::string& name) const
904{
905 return genericGetSensorName(sens_index, name);
906}
907
908bool BoschIMU::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string& frameName) const
909{
910 return genericGetFrameName(sens_index, frameName);
911}
912
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}
#define M_PI
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.
unsigned char command[MAX_MSG_LENGTH]
packet to be written to the device
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="")
std::atomic< bool > dataIsValid
std::mutex mutex
mutex to avoid resource clash
size_t responseOffset
bool checkReadResponse(unsigned char *response)
ReadFuncPtr readFunc
Functor object.
yarp::math::Quaternion quaternion_tmp
orientation in quaternion representation
size_t getNrOfThreeAxisMagnetometers() const override
Get the number of three axis magnetometers in the device.
void printBuffer(unsigned char *buffer, int length)
bool checkError
flag to check read error of sensor data
yarp::sig::Vector data
sensor data buffer
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 i2c_flag
flag to check if device connected through i2c commununication
double m_timeStamp
device timestamp
unsigned char response[MAX_MSG_LENGTH]
packet to be read from 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.
bool threadInit() override
Initialize process with desired device configurations.
long int totMessagesRead
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.
yarp::sig::Vector errorCounter
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.
int nChannels
number of channels in the output port. Default 12. If 16, also includes quaternion data
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.
yarp::math::Quaternion quaternion
orientation in quaternion representation
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.
double timeLastReport
timestamp of last reported data
int readBytes(unsigned char *buffer, int bytes)
size_t getNrOfThreeAxisGyroscopes() const override
Get the number of three axis gyroscopes in the device.
int fd
file descriptor to open device at system level
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.
yarp::sig::Vector data_tmp
sensor data temporary buffer
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::sig::Vector RPY_angle
orientation in Euler angle representation
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.
yarp::sig::Matrix toRotationMatrix4x4() const
Converts a quaternion to a rotation matrix.
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the 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:31
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.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
static double nowSystem()
static void delaySystem(double seconds)
A single value (typically within a Bottle).
Definition Value.h:43
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
void zero()
Zero the elements of the vector.
Definition Vector.h:344
#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,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
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.