1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
|
#include "bno055.hpp"
/// @link [Pico BNO055 Example](https://learnembeddedsystems.co.uk/bno005-i2c-example-code)
BNO055::BNO055() {
bno055_address = BNO055_ADDRESS_A;
_sensorID = BNO055_ID;
default_mode = OPERATION_MODE_NDOF;
}
void BNO055::reset_bno055() {
uint8_t data[2];
data[0] = BNO055_SYS_TRIGGER_ADDR;
data[1] = 0x20; // Reset system
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
sleep_ms(1000); // Wait 650ms for the sensor to reset
}
void BNO055::init() {
sleep_ms(1000); // Wait 650ms for the sensor to reset
uint8_t chip_id_addr = BNO055_CHIP_ID_ADDR;
uint8_t id[1];
i2c_write_blocking(i2c_default, bno055_address, &chip_id_addr, 1, false);
i2c_read_blocking(i2c_default, bno055_address, id, 1, false);
if (!id[0] == _sensorID) {
printf("BNO055 not detected\n");
}
// Use internal oscillator
uint8_t data[2];
data[0] = BNO055_SYS_TRIGGER_ADDR;
data[1] = 0x40; // Set to use internal oscillator
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
// Reset all interrupt status bits
data[0] = BNO055_SYS_TRIGGER_ADDR;
data[1] = 0x01; // Reset interrupt status
// 0x05 = Reset system
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
// Set to normal power mode
data[0] = BNO055_PWR_MODE_ADDR;
data[1] = 0x00; // Normal power mode
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
sleep_ms(50); // Wait 50ms for the sensor to switch to normal power mode
// Page 25 of the datasheet
// Default Axis Config
data[0] = BNO055_AXIS_MAP_CONFIG_ADDR;
data[1] = 0x24; // P1=Z, P2=Y, P3=X
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
// Default Axis Sign
data[0] = BNO055_AXIS_MAP_SIGN_ADDR;
data[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
// Set units to m/s^2
data[0] = BNO055_UNIT_SEL_ADDR;
data[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2
i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
sleep_ms(30);
//The default operation mode after power-on is CONFIGMODE
// Set mode to NDOF
// Takes 7ms to switch from CONFIG mode; see page 21 on datasheet (3.3)
data[0] = BNO055_OPR_MODE_ADDR;
data[1] = default_mode; // NDOF
i2c_write_blocking(i2c_default, bno055_address, data, 2, false);
sleep_ms(100);
}
void BNO055::read_calib_status() {
uint8_t calib_stat_reg = BNO055_CALIB_STAT_ADDR;
uint8_t calib_stat[1];
i2c_write_blocking(i2c_default, bno055_address, &calib_stat_reg, 1, true);
i2c_read_blocking(i2c_default, bno055_address, calib_stat, 1, false);
calib_status.mag = ((calib_stat[0] & 0b00000011) >> 0);
calib_status.accel = ((calib_stat[0] & 0b00001100) >> 2);
calib_status.gyro = ((calib_stat[0] & 0b00110000) >> 4);
calib_status.sys = ((calib_stat[0] & 0b11000000) >> 6);
}
void BNO055::read_lin_accel() {
uint8_t lin_accel_reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
i2c_write_blocking(i2c_default, bno055_address, &lin_accel_reg, 1, true);
i2c_read_blocking(i2c_default, bno055_address, accel, 6, false);
int16_t x, y, z;
x = y = z = 0;
x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8);
y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8);
z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8);
linear_acceleration.x = ((float)x) / 100.0;
linear_acceleration.y = ((float)y) / 100.0;
linear_acceleration.z = ((float)z) / 100.0;
}
void BNO055::clamp_close_zero(volatile float &val) {
if (val < 0.01 && val > -0.01) {
val = 0.0;
}
}
void BNO055::accel_to_gravity() {
accel_gravity.x = abs_lin_accel.x / 9.81;
accel_gravity.y = abs_lin_accel.y / 9.81;
accel_gravity.z = abs_lin_accel.z / 9.81;
}
void BNO055::read_abs_quaternion() {
uint8_t quat_reg = BNO055_QUATERNION_DATA_W_LSB_ADDR;
i2c_write_blocking(i2c_default, bno055_address, &quat_reg, 1, true);
i2c_read_blocking(i2c_default, bno055_address, quat, 8, false);
int16_t w, x, y, z;
w = x = y = z = 0;
w = ((int16_t)quat[0]) | (((int16_t)quat[1]) << 8);
x = ((int16_t)quat[2]) | (((int16_t)quat[3]) << 8);
y = ((int16_t)quat[4]) | (((int16_t)quat[5]) << 8);
z = ((int16_t)quat[6]) | (((int16_t)quat[7]) << 8);
abs_quaternion.w = ((float)w) / 16384.0; // 2^14 LSB
abs_quaternion.x = ((float)x) / 16384.0;
abs_quaternion.y = ((float)y) / 16384.0;
abs_quaternion.z = ((float)z) / 16384.0;
}
void BNO055::read_euler_angles() {
uint8_t euler[6];
uint8_t euler_reg = BNO055_EULER_H_LSB_ADDR;
i2c_write_blocking(i2c_default, bno055_address, &euler_reg, 1, true);
i2c_read_blocking(i2c_default, bno055_address, euler, 6, false);
/// @note heading = yaw
int16_t heading, roll, pitch;
heading = roll = pitch = 0;
heading = ((int16_t)euler[0]) | (((int16_t)euler[1]) << 8);
roll = ((int16_t)euler[2]) | (((int16_t)euler[3]) << 8);
pitch = ((int16_t)euler[4]) | (((int16_t)euler[5]) << 8);
euler_angles.x = ((float)roll) / 16.0;
euler_angles.y = ((float)pitch) / 16.0;
euler_angles.z = ((float)heading) / 16.0;
}
void BNO055::read_accel() {
uint8_t accel[6];
uint8_t accel_reg = BNO055_ACCEL_DATA_X_LSB_ADDR;
i2c_write_blocking(i2c_default, bno055_address, &accel_reg, 1, true);
i2c_read_blocking(i2c_default, bno055_address, accel, 6, false);
int16_t x, y, z;
x = y = z = 0;
x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8);
y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8);
z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8);
acceleration.x = ((float)x) / 100.0;
acceleration.y = ((float)y) / 100.0;
acceleration.z = ((float)z) / 100.0;
}
void BNO055::quaternion_to_euler() {
Eigen::Quaternion<float> q;
q.w() = abs_quaternion.w;
q.x() = abs_quaternion.x;
q.y() = abs_quaternion.y;
q.z() = abs_quaternion.z;
q.normalize();
Eigen::Matrix3f m = q.toRotationMatrix();
euler_angles.x = atan2f(m(2,1), m(2,2));
euler_angles.y = asinf(-m(2,0));
euler_angles.z = atan2f(m(1,0), m(0,0));
}
void BNO055::calculate_abs_linear_acceleration() {
Eigen::Quaternion<float> q;
q.w() = abs_quaternion.w;
q.x() = abs_quaternion.x;
q.y() = abs_quaternion.y;
q.z() = abs_quaternion.z;
// q.normalize();
Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
Eigen::Vector3f lin_accel;
lin_accel.x() = linear_acceleration.x;
lin_accel.y() = linear_acceleration.y;
lin_accel.z() = linear_acceleration.z;
abs_lin_accel.x = lin_accel.x() * rotation_matrix(0, 0) + lin_accel.y() * rotation_matrix(0, 1) + lin_accel.z() * rotation_matrix(0, 2);
abs_lin_accel.y = lin_accel.x() * rotation_matrix(1, 0) + lin_accel.y() * rotation_matrix(1, 1) + lin_accel.z() * rotation_matrix(1, 2);
abs_lin_accel.z = -1.0f * (lin_accel.x() * rotation_matrix(2, 0) + lin_accel.y() * rotation_matrix(2, 1) + lin_accel.z() * rotation_matrix(2, 2));
}
void BNO055::get_rotation_vector() {
Eigen::Quaternion<float> q;
q.w() = abs_quaternion.w;
q.x() = abs_quaternion.x;
q.y() = abs_quaternion.y;
q.z() = abs_quaternion.z;
q.normalize();
Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
rot_y_vec.x = rotation_matrix(1, 0);
rot_y_vec.y = rotation_matrix(1, 1);
rot_y_vec.z = rotation_matrix(1, 2);
}
|
