YARP
Yet Another Robot Platform
imuBosch_BNO055.h
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 #ifndef BOSCH_IMU_DEVICE
7 #define BOSCH_IMU_DEVICE
8 
9 #include <atomic>
10 #include <yarp/sig/Vector.h>
11 #include <yarp/os/PeriodicThread.h>
12 #include <yarp/dev/PolyDriver.h>
13 #include <yarp/os/ResourceFinder.h>
14 #include <yarp/dev/ISerialDevice.h>
17 #include <yarp/math/Quaternion.h>
18 #include <mutex>
19 
20 
21 /* Serial protocol description
22  *
23  * Write operation on a register:
24  * | Byte 1 | Byte 2 | Byte 3 | Byte 4 | Byte 5 | ... | Byte (n+4) |
25  * | Start | Write | Reg Addr| Length | Data 1 | ... | Data n |
26  * | 0xAA | 0x00 | <...> | <...> | <...> | ... | <...> |
27  *
28  * Response to write command:
29  * | 0xEE | <code> |
30  * 0x01: Write_success
31  * all other values are errors ... TODO: improve doc
32  *
33  * Read operation on a register:
34  * | Byte 1 | Byte 2 | Byte 3 | Byte 4 |
35  * | Start | Read | Reg Addr| Length |
36  * | 0xAA | 0x01 | <...> | <...> |
37  *
38  * Response to a successful read command:
39  * | Byte 1 | Byte 2 | Byte 3 | ... | Byte (n+2) |
40  * | Resp | Length | Data 1 | ... | Data n |
41  * | 0xBB | <...> | <...> | ... | <...> |
42  *
43  * Response to a failed read command:
44  * | Byte 1 | Byte 2 |
45  * | Resp | Status |
46  * | 0xEE | <...> |
47  *
48  * Read error code: TODO
49  */
50 
51 constexpr int MAX_MSG_LENGTH = 128;
52 
53 // Commands
54 #define START_BYTE 0xAA
55 #define WRITE_CMD 0x00
56 #define READ_CMD 0x01
57 
58 // Responses
59 #define REPLY_HEAD 0xBB
60 #define ERROR_HEAD 0xEE
61 #define WRITE_SUCC 0x01
62 #define READ_FAIL 0x02
63 #define WRITE_FAIL 0x03
64 
65 // Error code
66 #define REGISTER_NOT_READY 0x07
67 
68 // Registers
69 // Page 0 // Device has 2 pages of registers
70 #define REG_CHIP_ID 0x00
71 #define REG_SW_VERSION 0x04 // 2 software revision bytes
72 #define REG_BOOTLOADER 0x06 // 1 byte bootloader version
73 #define REG_PAGE_ID 0x07 // page ID number
74 
75 #define REG_ACC_DATA 0x08 // 3*2 bytes: LSB first (LSB 0x08, MSB 0x09) for X
76 #define REG_MAGN_DATA 0x0E // 3*2 bytes: LSB first
77 #define REG_GYRO_DATA 0x14 // 3*2 bytes: LSB first
78 #define REG_RPY_DATA 0x1A // 3*2 bytes: LSB first (raw order is Yaw, Roll, Pitch)
79 #define REG_QUATERN_DATA 0x20 // 4*2 bytes: LSB first (raw order is w, x, y, z)
80 #define REG_GRAVITY 0x2E // Gravity Vector data
81 #define REG_CALIB_STATUS 0x35 // Check if sensors are calibrated, 2 bits each. SYS - GYRO - ACC - MAG. 3 means calibrated, 0 not calbrated
82 #define REG_SYS_CLK_STATUS 0x38 // only 1 last LSB
83 #define REG_SYS_STATUS 0x39
84 #define REG_SYS_ERR 0x3A
85 #define REG_UNIT_SEL 0x3B
86 #define REG_OP_MODE 0x3D
87 #define REG_POWER_MODE 0x3E
88 #define REG_SYS_TRIGGER 0x3F
89 
90 // Values
91 #define CONFIG_MODE 0x00
92 #define AMG_MODE 0x07
93 #define IMU_MODE 0x08
94 #define M4G_MODE 0x0A
95 #define NDOF_MODE 0x0C
96 
97 // Sys trigger values (in OR if more than one is to be activated)
98 #define TRIG_EXT_CLK_SEL 0x80 // 1 for external clock (if available), 0 for internal clock
99 #define TRIG_RESET_INT 0x40 // reset interrupts
100 #define TRIG_RESET_SYSTEM 0x20 // reset system
101 #define TRIG_SELF_TEST 0x01 // Start self test
102 
103 #define BNO055_ID 0xA0
104 
105 #define RESP_HEADER_SIZE 2
106 // Time to wait while switching to and from config_mode & any operation_mode
107 #define SWITCHING_TIME 0.020 // 20ms
108 #define TIME_REPORT_INTERVAL 30
109 //number of attempts of sending config command
110 #define ATTEMPTS_NUM_OF_SEND_CONFIG_CMD 3
111 
112 
113 
136 class BoschIMU:
144 {
145 protected:
146  bool verbose;
147  short status;
148  int nChannels;
154  double m_timeStamp;
155  double timeLastReport;
156  mutable std::mutex mutex;
157  bool i2c_flag;
158 
159  bool checkError;
160 
161  int fd;
164 
165  using ReadFuncPtr = bool (BoschIMU::*)(unsigned char, int, unsigned char*, std::string);
167 
168  unsigned char command[MAX_MSG_LENGTH];
169  unsigned char response[MAX_MSG_LENGTH];
170 
171 
172  bool checkWriteResponse(unsigned char *response);
173  bool checkReadResponse(unsigned char *response);
174 
175  void printBuffer(unsigned char *buffer, int length);
176  int readBytes(unsigned char *buffer, int bytes);
177  void dropGarbage();
178 
179  long int totMessagesRead;
182 
183  void readSysError();
184  // Serial
185  bool sendReadCommandSer(unsigned char register_add, int len, unsigned char* buf, std::string comment = "");
186  bool sendWriteCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment = "");
187  bool sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment);
188 
189  // i2c
190  bool sendReadCommandI2c(unsigned char register_add, int len, unsigned char* buf, std::string comment = "");
191 
192  int errs;
193  std::atomic<bool> dataIsValid;
194 
195 public:
196  BoschIMU();
197 
199 
205  bool open(yarp::os::Searchable& config) override;
206 
211  bool close() override;
212 
218  bool read(yarp::sig::Vector &out) override;
219 
225  bool getChannels(int *nc) override;
226 
233  bool calibrate(int ch, double v) override;
234 
235  /* IThreeAxisGyroscopes methods */
240  size_t getNrOfThreeAxisGyroscopes() const override;
241 
247  yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override;
248 
255  bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override;
256 
263  bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override;
264 
272  bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
273 
274  /* IThreeAxisLinearAccelerometers methods */
279  size_t getNrOfThreeAxisLinearAccelerometers() const override;
280 
286  yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override;
287 
294  bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override;
295 
302  bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override;
303 
311  bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
312 
313  /* IThreeAxisMagnetometers methods */
318  size_t getNrOfThreeAxisMagnetometers() const override;
319 
325  yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override;
326 
333  bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override;
334 
341  bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override;
342 
350  bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
351 
352  /* IOrientationSensors methods */
357  size_t getNrOfOrientationSensors() const override;
358 
364  yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override;
365 
372  bool getOrientationSensorName(size_t sens_index, std::string &name) const override;
373 
380  bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override;
381 
389  bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy, double& timestamp) const override;
390 
395  bool threadInit() override;
396 
397 
401  void threadRelease() override;
402 
406  void run() override;
407 
408 private:
409  yarp::dev::MAS_status genericGetStatus(size_t sens_index) const;
410  bool genericGetSensorName(size_t sens_index, std::string &name) const;
411  bool genericGetFrameName(size_t sens_index, std::string &frameName) const;
412 
413  std::string m_sensorName;
414  std::string m_frameName;
415 };
416 
417 
418 #endif // BOSCH_IMU_DEVICE
contains the definition of a Vector type
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.
yarp::sig::Vector errorReading
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
short status
device status - UNUSED
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
yarp::os::ResourceFinder rf
resource finder object to load config parameters
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.
void dropGarbage()
bool threadInit() override
Initialize process with desired device configurations.
long int totMessagesRead
bool verbose
Flag to get verbose output.
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
bool(BoschIMU::*)(unsigned char, int, unsigned char *, std::string) ReadFuncPtr
Functor to choose between i2c or serial comm.
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.
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
A generic interface to sensors – gyro, a/d converters etc.
Device interface to one or multiple orientation sensors, such as IMUs with on board estimation algori...
Device interface to one or multiple three axis gyroscopes.
Device interface to one or multiple three axis linear accelerometers.
Device interface to one or multiple three axis magnetometers.
An abstraction for a periodic thread.
Helper class for finding config files and other external resources.
A base class for nested structures that can be searched.
Definition: Searchable.h:63
constexpr int MAX_MSG_LENGTH
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.