YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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>
12#include <yarp/dev/PolyDriver.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
51constexpr 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
144{
145protected:
146 bool verbose;
147 short status;
154 double m_timeStamp;
156 mutable std::mutex mutex;
157 bool i2c_flag;
158
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
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
195public:
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
408private:
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.
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.
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:31
constexpr int MAX_MSG_LENGTH
STL namespace.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.