Skip to content

Commit 01ef826

Browse files
vcrochervcrocher
committed
ERRATUM: Addition of new I2C related file missing from previous commit.
1 parent 4afd4c4 commit 01ef826

4 files changed

Lines changed: 845 additions & 0 deletions

File tree

‎src/hardware/IO/I2CBNO55IMU.cpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#include "I2CBNO55IMU.h"
2+
3+
4+
I2CBNO55IMU::I2CBNO55IMU(unsigned char _DeviceAddress, int _BusId) {
5+
setDeviceAddress(_DeviceAddress);
6+
setBusId(_BusId);
7+
#ifndef NOROBOT
8+
initDevice();
9+
#endif
10+
spdlog::debug("Created I2C BNO055 IMU on {}.", _DeviceAddress);
11+
#ifndef NOROBOT
12+
setMode();//Set in default 9DoF fusion
13+
setSIUnits();
14+
#endif
15+
};
16+
17+
18+
void I2CBNO55IMU::readQuat() {
19+
//Read all 4 values as consecutive registers (one block) for faster access
20+
int16_t val[4];
21+
readBlockValues16RdwrS(BNO055_QUATERNION_DATA_W_LSB_ADDR, 4, val);
22+
23+
//Scale everything:
24+
const double scale = (1.0 / (1 << 14));
25+
data.quat[0]=scale * val[0];
26+
data.quat[1]=scale * val[1];
27+
data.quat[2]=scale * val[2];
28+
data.quat[3]=scale * val[3];
29+
}
30+
31+
void I2CBNO55IMU::readLinAcc() {
32+
//Read all 3 values as consecutive registers (one block) for faster access
33+
int16_t val[3];
34+
readBlockValues16RdwrS(BNO055_ACCEL_DATA_X_LSB_ADDR, 3, val);
35+
36+
/* 1m/s^2 = 100 LSB */
37+
data.linAcc[0] = ((double)val[0]) / 100.0;
38+
data.linAcc[1] = ((double)val[1]) / 100.0;
39+
data.linAcc[2] = ((double)val[2]) / 100.0;
40+
}
41+
42+
void I2CBNO55IMU::readGrav() {
43+
int16_t val[3];
44+
readBlockValues16RdwrS(BNO055_GRAVITY_DATA_X_LSB_ADDR, 3, val);
45+
/* 1m/s^2 = 100 LSB */
46+
data.grav[0] = ((double)val[0]) / 100.0;
47+
data.grav[1] = ((double)val[1]) / 100.0;
48+
data.grav[2] = ((double)val[2]) / 100.0;
49+
}
50+
51+
void I2CBNO55IMU::updateInput() {
52+
//readQuat();
53+
readGrav();
54+
//readLinAcc();
55+
std::cout << "Q = " << getQuat().transpose() << "\t G = " << getGravityVec().transpose() << "A = " << getLinAcc().transpose() << "\n";
56+
}
57+
58+

‎src/hardware/IO/I2CBNO55IMU.h‎

Lines changed: 276 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,276 @@
1+
/**
2+
*
3+
* \file I2CBNO55IMU.h
4+
* \author Vincent Crocher
5+
* \version 0.1
6+
* \date 2026-02-27
7+
* \copyright Copyright (c) 2026
8+
*
9+
* \brief Class allowing communication with a BNO055 IMU (Adafruit breakout board) via I2C on a BeagleBone (and maybe other)
10+
* Use registers definition from Adafruit Arduino code (https://github.com/adafruit/Adafruit_BNO055/blob/master/Adafruit_BNO055.h).
11+
* Use i2cdetect -y -r 1 command to list adresses of devices connected on the bus 1 (/dev/i2c-1) of the BB.
12+
*
13+
*/
14+
15+
#ifndef I2CBNO55IMU_H
16+
#define I2CBNO55IMU_H
17+
#include <Eigen/Dense>
18+
19+
#include "InputDevice.h"
20+
#include "I2CDevice.h"
21+
22+
struct IMUData {
23+
Eigen::VectorXd rawAcc = Eigen::VectorXd::Zero(3); // 3D vector for x,y,z acceleration
24+
Eigen::VectorXd linAcc = Eigen::VectorXd::Zero(3); // 3D vector for x,y,z linear acceleration
25+
Eigen::VectorXd quat = Eigen::VectorXd::Zero(4); // 4D vector for w,x,y,z quaternion
26+
Eigen::VectorXd grav = Eigen::VectorXd::Zero(3); // 3D vector for gravity vector
27+
};
28+
29+
30+
/** Operation mode settings **/
31+
typedef enum {
32+
OPERATION_MODE_CONFIG = 0X00,
33+
OPERATION_MODE_ACCONLY = 0X01,
34+
OPERATION_MODE_MAGONLY = 0X02,
35+
OPERATION_MODE_GYRONLY = 0X03,
36+
OPERATION_MODE_ACCMAG = 0X04,
37+
OPERATION_MODE_ACCGYRO = 0X05,
38+
OPERATION_MODE_MAGGYRO = 0X06,
39+
OPERATION_MODE_AMG = 0X07,
40+
OPERATION_MODE_IMUPLUS = 0X08,
41+
OPERATION_MODE_COMPASS = 0X09,
42+
OPERATION_MODE_M4G = 0X0A,
43+
OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
44+
OPERATION_MODE_NDOF = 0X0C
45+
} bno055_opmode_t;
46+
47+
/** BNO055 Registers **/
48+
typedef enum {
49+
/* Page id register definition */
50+
BNO055_PAGE_ID_ADDR = 0X07,
51+
52+
/* PAGE0 REGISTER DEFINITION START*/
53+
BNO055_CHIP_ID_ADDR = 0x00,
54+
BNO055_ACCEL_REV_ID_ADDR = 0x01,
55+
BNO055_MAG_REV_ID_ADDR = 0x02,
56+
BNO055_GYRO_REV_ID_ADDR = 0x03,
57+
BNO055_SW_REV_ID_LSB_ADDR = 0x04,
58+
BNO055_SW_REV_ID_MSB_ADDR = 0x05,
59+
BNO055_BL_REV_ID_ADDR = 0X06,
60+
61+
/* Accel data register */
62+
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
63+
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
64+
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
65+
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
66+
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
67+
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
68+
69+
/* Mag data register */
70+
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
71+
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
72+
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
73+
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
74+
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
75+
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
76+
77+
/* Gyro data registers */
78+
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
79+
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
80+
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
81+
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
82+
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
83+
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
84+
85+
/* Euler data registers */
86+
BNO055_EULER_H_LSB_ADDR = 0X1A,
87+
BNO055_EULER_H_MSB_ADDR = 0X1B,
88+
BNO055_EULER_R_LSB_ADDR = 0X1C,
89+
BNO055_EULER_R_MSB_ADDR = 0X1D,
90+
BNO055_EULER_P_LSB_ADDR = 0X1E,
91+
BNO055_EULER_P_MSB_ADDR = 0X1F,
92+
93+
/* Quaternion data registers */
94+
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
95+
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
96+
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
97+
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
98+
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
99+
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
100+
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
101+
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
102+
103+
/* Linear acceleration data registers */
104+
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
105+
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
106+
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
107+
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
108+
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
109+
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
110+
111+
/* Gravity data registers */
112+
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
113+
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
114+
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
115+
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
116+
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
117+
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
118+
119+
/* Temperature data register */
120+
BNO055_TEMP_ADDR = 0X34,
121+
122+
/* Status registers */
123+
BNO055_CALIB_STAT_ADDR = 0X35,
124+
BNO055_SELFTEST_RESULT_ADDR = 0X36,
125+
BNO055_INTR_STAT_ADDR = 0X37,
126+
127+
BNO055_SYS_CLK_STAT_ADDR = 0X38,
128+
BNO055_SYS_STAT_ADDR = 0X39,
129+
BNO055_SYS_ERR_ADDR = 0X3A,
130+
131+
/* Unit selection register */
132+
BNO055_UNIT_SEL_ADDR = 0X3B,
133+
134+
/* Mode registers */
135+
BNO055_OPR_MODE_ADDR = 0X3D,
136+
BNO055_PWR_MODE_ADDR = 0X3E,
137+
138+
BNO055_SYS_TRIGGER_ADDR = 0X3F,
139+
BNO055_TEMP_SOURCE_ADDR = 0X40,
140+
141+
/* Axis remap registers */
142+
BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
143+
BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
144+
145+
/* SIC registers */
146+
BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
147+
BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
148+
BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
149+
BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
150+
BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
151+
BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
152+
BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
153+
BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
154+
BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
155+
BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
156+
BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
157+
BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
158+
BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
159+
BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
160+
BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
161+
BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
162+
BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
163+
BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
164+
165+
/* Accelerometer Offset registers */
166+
ACCEL_OFFSET_X_LSB_ADDR = 0X55,
167+
ACCEL_OFFSET_X_MSB_ADDR = 0X56,
168+
ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
169+
ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
170+
ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
171+
ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
172+
173+
/* Magnetometer Offset registers */
174+
MAG_OFFSET_X_LSB_ADDR = 0X5B,
175+
MAG_OFFSET_X_MSB_ADDR = 0X5C,
176+
MAG_OFFSET_Y_LSB_ADDR = 0X5D,
177+
MAG_OFFSET_Y_MSB_ADDR = 0X5E,
178+
MAG_OFFSET_Z_LSB_ADDR = 0X5F,
179+
MAG_OFFSET_Z_MSB_ADDR = 0X60,
180+
181+
/* Gyroscope Offset register s*/
182+
GYRO_OFFSET_X_LSB_ADDR = 0X61,
183+
GYRO_OFFSET_X_MSB_ADDR = 0X62,
184+
GYRO_OFFSET_Y_LSB_ADDR = 0X63,
185+
GYRO_OFFSET_Y_MSB_ADDR = 0X64,
186+
GYRO_OFFSET_Z_LSB_ADDR = 0X65,
187+
GYRO_OFFSET_Z_MSB_ADDR = 0X66,
188+
189+
/* Radius registers */
190+
ACCEL_RADIUS_LSB_ADDR = 0X67,
191+
ACCEL_RADIUS_MSB_ADDR = 0X68,
192+
MAG_RADIUS_LSB_ADDR = 0X69,
193+
MAG_RADIUS_MSB_ADDR = 0X6A
194+
} bno055_reg_t;
195+
196+
/** BNO055 power settings */
197+
typedef enum {
198+
POWER_MODE_NORMAL = 0X00,
199+
POWER_MODE_LOWPOWER = 0X01,
200+
POWER_MODE_SUSPEND = 0X02
201+
} bno055_powermode_t;
202+
203+
204+
205+
/**
206+
* \ingroup IO
207+
* \brief Class inherit InputDevice to handle an I2C based BNO 055 IMU.
208+
*/
209+
class I2CBNO55IMU: public InputDevice, public I2CDevice {
210+
private:
211+
IMUData data;
212+
213+
/**
214+
* \brief Update quat values from IMU.
215+
*/
216+
void readQuat();
217+
/**
218+
* \brief Update linear accel. from IMU.
219+
*/
220+
void readLinAcc();
221+
/**
222+
* \brief Update gravity vector from IMU.
223+
*/
224+
void readGrav();
225+
226+
public:
227+
I2CBNO55IMU(unsigned char _DeviceAddress, int _BusId=1);
228+
229+
void setDeviceAddress( unsigned char _DeviceAddress ){ DeviceAddress = _DeviceAddress; };
230+
void setBusId( int _BusId ) { BusId = _BusId; };
231+
232+
/**
233+
* \brief Update all relevant IMU values: linear acceleration, gravity vector and quaternion.
234+
*/
235+
void updateInput();
236+
237+
/**
238+
* \brief Select (set) IMU working mode.
239+
* \param Default is NDOF: full 9DoF fusion.
240+
*/
241+
void setMode(bno055_opmode_t mode = OPERATION_MODE_NDOF) { writeReg(BNO055_OPR_MODE_ADDR, mode); };
242+
/**
243+
* \brief Set IMU in SI units ( radians, rps, m/s2, celsius).
244+
*/
245+
void setSIUnits() { writeReg(BNO055_UNIT_SEL_ADDR, 0x06); };
246+
247+
void setFastAccOnly() { setMode(OPERATION_MODE_ACCONLY); writeReg(0x08, 0x1D); }
248+
249+
/**
250+
* \brief Read and return temperature of IMU.
251+
*/
252+
int getTemp() { return readRegisterRdwr(BNO055_TEMP_ADDR); };
253+
254+
/**
255+
* \brief Get the linear acceleration readings from IMU
256+
*
257+
* \return Eigen::VectorXd x,y,z axis of linear acceleration
258+
*/
259+
Eigen::VectorXd& getLinAcc() { return data.linAcc; };
260+
261+
/**
262+
* \brief Get the quaternion readings from the IMU
263+
*
264+
* \return Eigen::VectorXd w,x,y,z quaternion
265+
*/
266+
Eigen::VectorXd& getQuat() { return data.quat; };
267+
268+
/**
269+
* \brief Get the gravity vector from the IMU
270+
*
271+
* \return Eigen::VectorXd 3D gravity direction vector
272+
*/
273+
Eigen::VectorXd& getGravityVec() { return data.grav; };
274+
275+
};
276+
#endif

0 commit comments

Comments
 (0)