Revision 312a5b9e
Adafruit_BNO055.cpp | ||
---|---|---|
15 | 15 |
Written by KTOWN for Adafruit Industries. |
16 | 16 |
|
17 | 17 |
MIT license, all text above must be included in any redistribution |
18 |
***************************************************************************/ |
|
18 |
***************************************************************************/
|
|
19 | 19 |
|
20 | 20 |
#if ARDUINO >= 100 |
21 |
#include "Arduino.h"
|
|
21 |
#include "Arduino.h" |
|
22 | 22 |
#else |
23 |
#include "WProgram.h"
|
|
23 |
#include "WProgram.h" |
|
24 | 24 |
#endif |
25 | 25 |
|
26 | 26 |
#include <math.h> |
... | ... | |
34 | 34 |
|
35 | 35 |
/**************************************************************************/ |
36 | 36 |
/*! |
37 |
@brief Instantiates a new Adafruit_BNO055 class
|
|
38 |
*/ |
|
37 |
@brief Instantiates a new Adafruit_BNO055 class
|
|
38 |
*/
|
|
39 | 39 |
/**************************************************************************/ |
40 | 40 |
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) |
41 | 41 |
{ |
42 |
_sensorID = sensorID;
|
|
43 |
_address = address;
|
|
42 |
_sensorID = sensorID;
|
|
43 |
_address = address;
|
|
44 | 44 |
} |
45 | 45 |
|
46 | 46 |
/*************************************************************************** |
... | ... | |
49 | 49 |
|
50 | 50 |
/**************************************************************************/ |
51 | 51 |
/*! |
52 |
@brief Sets up the HW
|
|
53 |
*/ |
|
52 |
@brief Sets up the HW
|
|
53 |
*/
|
|
54 | 54 |
/**************************************************************************/ |
55 | 55 |
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) |
56 | 56 |
{ |
57 |
/* Enable I2C */
|
|
58 |
Wire.begin();
|
|
59 |
|
|
60 |
/* Make sure we have the right device */
|
|
61 |
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
|
|
62 |
if(id != BNO055_ID)
|
|
63 |
{
|
|
64 |
delay(1000); // hold on for boot
|
|
65 |
id = read8(BNO055_CHIP_ID_ADDR);
|
|
66 |
if(id != BNO055_ID) {
|
|
67 |
return false; // still not? ok bail
|
|
68 |
}
|
|
69 |
}
|
|
70 |
|
|
71 |
/* Switch to config mode (just in case since this is the default) */
|
|
72 |
setMode(OPERATION_MODE_CONFIG);
|
|
73 |
|
|
74 |
/* Reset */
|
|
75 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
|
|
76 |
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
|
|
77 |
{
|
|
78 |
delay(10);
|
|
79 |
}
|
|
80 |
delay(50);
|
|
81 |
|
|
82 |
/* Set to normal power mode */
|
|
83 |
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
|
|
84 |
delay(10);
|
|
85 |
|
|
86 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
|
87 |
|
|
88 |
/* Set the output units */
|
|
89 |
/*
|
|
90 |
uint8_t unitsel = (0 << 7) | // Orientation = Android
|
|
91 |
(0 << 4) | // Temperature = Celsius
|
|
92 |
(0 << 2) | // Euler = Degrees
|
|
93 |
(1 << 1) | // Gyro = Rads
|
|
94 |
(0 << 0); // Accelerometer = m/s^2
|
|
95 |
write8(BNO055_UNIT_SEL_ADDR, unitsel);
|
|
96 |
*/
|
|
97 |
|
|
98 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
|
|
99 |
delay(10);
|
|
100 |
/* Set the requested operating mode (see section 3.3) */
|
|
101 |
setMode(mode);
|
|
102 |
delay(20);
|
|
103 |
|
|
104 |
return true;
|
|
57 |
/* Enable I2C */
|
|
58 |
Wire.begin();
|
|
59 |
|
|
60 |
/* Make sure we have the right device */
|
|
61 |
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
|
|
62 |
if (id != BNO055_ID)
|
|
63 |
{
|
|
64 |
delay(1000); // hold on for boot
|
|
65 |
id = read8(BNO055_CHIP_ID_ADDR);
|
|
66 |
if (id != BNO055_ID) {
|
|
67 |
return false; // still not? ok bail
|
|
68 |
}
|
|
69 |
}
|
|
70 |
|
|
71 |
/* Switch to config mode (just in case since this is the default) */
|
|
72 |
setMode(OPERATION_MODE_CONFIG);
|
|
73 |
|
|
74 |
/* Reset */
|
|
75 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
|
|
76 |
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
|
|
77 |
{
|
|
78 |
delay(10);
|
|
79 |
}
|
|
80 |
delay(50);
|
|
81 |
|
|
82 |
/* Set to normal power mode */
|
|
83 |
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
|
|
84 |
delay(10);
|
|
85 |
|
|
86 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
|
87 |
|
|
88 |
/* Set the output units */
|
|
89 |
/*
|
|
90 |
uint8_t unitsel = (0 << 7) | // Orientation = Android
|
|
91 |
(0 << 4) | // Temperature = Celsius
|
|
92 |
(0 << 2) | // Euler = Degrees
|
|
93 |
(1 << 1) | // Gyro = Rads
|
|
94 |
(0 << 0); // Accelerometer = m/s^2
|
|
95 |
write8(BNO055_UNIT_SEL_ADDR, unitsel);
|
|
96 |
*/
|
|
97 |
|
|
98 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
|
|
99 |
delay(10);
|
|
100 |
/* Set the requested operating mode (see section 3.3) */
|
|
101 |
setMode(mode);
|
|
102 |
delay(20);
|
|
103 |
|
|
104 |
return true;
|
|
105 | 105 |
} |
106 | 106 |
|
107 | 107 |
/**************************************************************************/ |
108 | 108 |
/*! |
109 |
@brief Puts the chip in the specified operating mode
|
|
110 |
*/ |
|
109 |
@brief Puts the chip in the specified operating mode
|
|
110 |
*/
|
|
111 | 111 |
/**************************************************************************/ |
112 | 112 |
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) |
113 | 113 |
{ |
114 |
_mode = mode;
|
|
115 |
write8(BNO055_OPR_MODE_ADDR, _mode);
|
|
116 |
delay(30);
|
|
114 |
_mode = mode;
|
|
115 |
write8(BNO055_OPR_MODE_ADDR, _mode);
|
|
116 |
delay(30);
|
|
117 | 117 |
} |
118 | 118 |
|
119 | 119 |
/**************************************************************************/ |
120 | 120 |
/*! |
121 |
@brief Use the external 32.768KHz crystal
|
|
122 |
*/ |
|
121 |
@brief Use the external 32.768KHz crystal
|
|
122 |
*/
|
|
123 | 123 |
/**************************************************************************/ |
124 | 124 |
void Adafruit_BNO055::setExtCrystalUse(boolean usextal) |
125 | 125 |
{ |
126 |
adafruit_bno055_opmode_t modeback = _mode; |
|
127 |
|
|
128 |
/* Switch to config mode (just in case since this is the default) */ |
|
129 |
setMode(OPERATION_MODE_CONFIG); |
|
130 |
delay(25); |
|
131 |
write8(BNO055_PAGE_ID_ADDR, 0); |
|
132 |
if (usextal) { |
|
133 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x80); |
|
134 |
} else { |
|
135 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x00); |
|
136 |
} |
|
137 |
delay(10); |
|
138 |
/* Set the requested operating mode (see section 3.3) */ |
|
139 |
setMode(modeback); |
|
140 |
delay(20); |
|
126 |
adafruit_bno055_opmode_t modeback = _mode; |
|
127 |
|
|
128 |
/* Switch to config mode (just in case since this is the default) */ |
|
129 |
setMode(OPERATION_MODE_CONFIG); |
|
130 |
delay(25); |
|
131 |
write8(BNO055_PAGE_ID_ADDR, 0); |
|
132 |
if (usextal) { |
|
133 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x80); |
|
134 |
} |
|
135 |
else { |
|
136 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x00); |
|
137 |
} |
|
138 |
delay(10); |
|
139 |
/* Set the requested operating mode (see section 3.3) */ |
|
140 |
setMode(modeback); |
|
141 |
delay(20); |
|
141 | 142 |
} |
142 | 143 |
|
143 | 144 |
|
144 | 145 |
/**************************************************************************/ |
145 | 146 |
/*! |
146 |
@brief Gets the latest system status info
|
|
147 |
*/ |
|
147 |
@brief Gets the latest system status info
|
|
148 |
*/
|
|
148 | 149 |
/**************************************************************************/ |
149 | 150 |
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) |
150 | 151 |
{ |
151 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
|
152 |
|
|
153 |
/* System Status (see section 4.3.58)
|
|
154 |
---------------------------------
|
|
155 |
0 = Idle
|
|
156 |
1 = System Error
|
|
157 |
2 = Initializing Peripherals
|
|
158 |
3 = System Iniitalization
|
|
159 |
4 = Executing Self-Test
|
|
160 |
5 = Sensor fusio algorithm running
|
|
161 |
6 = System running without fusion algorithms */
|
|
162 |
|
|
163 |
if (system_status != 0)
|
|
164 |
*system_status = read8(BNO055_SYS_STAT_ADDR);
|
|
165 |
|
|
166 |
/* Self Test Results (see section )
|
|
167 |
--------------------------------
|
|
168 |
1 = test passed, 0 = test failed
|
|
169 |
|
|
170 |
Bit 0 = Accelerometer self test
|
|
171 |
Bit 1 = Magnetometer self test
|
|
172 |
Bit 2 = Gyroscope self test
|
|
173 |
Bit 3 = MCU self test
|
|
174 |
|
|
175 |
0x0F = all good! */
|
|
176 |
|
|
177 |
if (self_test_result != 0)
|
|
178 |
*self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
|
|
179 |
|
|
180 |
/* System Error (see section 4.3.59)
|
|
181 |
---------------------------------
|
|
182 |
0 = No error
|
|
183 |
1 = Peripheral initialization error
|
|
184 |
2 = System initialization error
|
|
185 |
3 = Self test result failed
|
|
186 |
4 = Register map value out of range
|
|
187 |
5 = Register map address out of range
|
|
188 |
6 = Register map write error
|
|
189 |
7 = BNO low power mode not available for selected operat ion mode
|
|
190 |
8 = Accelerometer power mode not available
|
|
191 |
9 = Fusion algorithm configuration error
|
|
192 |
A = Sensor configuration error */
|
|
193 |
|
|
194 |
if (system_error != 0)
|
|
195 |
*system_error = read8(BNO055_SYS_ERR_ADDR);
|
|
196 |
|
|
197 |
delay(200);
|
|
152 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
|
153 |
|
|
154 |
/* System Status (see section 4.3.58)
|
|
155 |
---------------------------------
|
|
156 |
0 = Idle
|
|
157 |
1 = System Error
|
|
158 |
2 = Initializing Peripherals
|
|
159 |
3 = System Iniitalization
|
|
160 |
4 = Executing Self-Test
|
|
161 |
5 = Sensor fusio algorithm running
|
|
162 |
6 = System running without fusion algorithms */
|
|
163 |
|
|
164 |
if (system_status != 0)
|
|
165 |
*system_status = read8(BNO055_SYS_STAT_ADDR);
|
|
166 |
|
|
167 |
/* Self Test Results (see section )
|
|
168 |
--------------------------------
|
|
169 |
1 = test passed, 0 = test failed
|
|
170 |
|
|
171 |
Bit 0 = Accelerometer self test
|
|
172 |
Bit 1 = Magnetometer self test
|
|
173 |
Bit 2 = Gyroscope self test
|
|
174 |
Bit 3 = MCU self test
|
|
175 |
|
|
176 |
0x0F = all good! */
|
|
177 |
|
|
178 |
if (self_test_result != 0)
|
|
179 |
*self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
|
|
180 |
|
|
181 |
/* System Error (see section 4.3.59)
|
|
182 |
---------------------------------
|
|
183 |
0 = No error
|
|
184 |
1 = Peripheral initialization error
|
|
185 |
2 = System initialization error
|
|
186 |
3 = Self test result failed
|
|
187 |
4 = Register map value out of range
|
|
188 |
5 = Register map address out of range
|
|
189 |
6 = Register map write error
|
|
190 |
7 = BNO low power mode not available for selected operat ion mode
|
|
191 |
8 = Accelerometer power mode not available
|
|
192 |
9 = Fusion algorithm configuration error
|
|
193 |
A = Sensor configuration error */
|
|
194 |
|
|
195 |
if (system_error != 0)
|
|
196 |
*system_error = read8(BNO055_SYS_ERR_ADDR);
|
|
197 |
|
|
198 |
delay(200);
|
|
198 | 199 |
} |
199 | 200 |
|
200 | 201 |
/**************************************************************************/ |
201 | 202 |
/*! |
202 |
@brief Gets the chip revision numbers
|
|
203 |
*/ |
|
203 |
@brief Gets the chip revision numbers
|
|
204 |
*/
|
|
204 | 205 |
/**************************************************************************/ |
205 | 206 |
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info) |
206 | 207 |
{ |
207 |
uint8_t a, b;
|
|
208 |
uint8_t a, b;
|
|
208 | 209 |
|
209 |
memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
|
|
210 |
memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
|
|
210 | 211 |
|
211 |
/* Check the accelerometer revision */
|
|
212 |
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
|
|
212 |
/* Check the accelerometer revision */
|
|
213 |
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
|
|
213 | 214 |
|
214 |
/* Check the magnetometer revision */
|
|
215 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
|
|
215 |
/* Check the magnetometer revision */
|
|
216 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
|
|
216 | 217 |
|
217 |
/* Check the gyroscope revision */
|
|
218 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
|
|
218 |
/* Check the gyroscope revision */
|
|
219 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
|
|
219 | 220 |
|
220 |
/* Check the SW revision */
|
|
221 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
|
221 |
/* Check the SW revision */
|
|
222 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
|
222 | 223 |
|
223 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
|
|
224 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
|
|
225 |
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
|
|
224 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
|
|
225 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
|
|
226 |
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
|
|
226 | 227 |
} |
227 | 228 |
|
228 | 229 |
/**************************************************************************/ |
229 | 230 |
/*! |
230 |
@brief Gets current calibration state. Each value should be a uint8_t
|
|
231 |
pointer and it will be set to 0 if not calibrated and 3 if
|
|
232 |
fully calibrated.
|
|
233 |
*/ |
|
231 |
@brief Gets current calibration state. Each value should be a uint8_t
|
|
232 |
pointer and it will be set to 0 if not calibrated and 3 if
|
|
233 |
fully calibrated.
|
|
234 |
*/
|
|
234 | 235 |
/**************************************************************************/ |
235 | 236 |
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) { |
236 |
uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
|
|
237 |
if (sys != NULL) {
|
|
238 |
*sys = (calData >> 6) & 0x03;
|
|
239 |
}
|
|
240 |
if (gyro != NULL) {
|
|
241 |
*gyro = (calData >> 4) & 0x03;
|
|
242 |
}
|
|
243 |
if (accel != NULL) {
|
|
244 |
*accel = (calData >> 2) & 0x03;
|
|
245 |
}
|
|
246 |
if (mag != NULL) {
|
|
247 |
*mag = calData & 0x03;
|
|
248 |
}
|
|
237 |
uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
|
|
238 |
if (sys != NULL) {
|
|
239 |
*sys = (calData >> 6) & 0x03;
|
|
240 |
}
|
|
241 |
if (gyro != NULL) {
|
|
242 |
*gyro = (calData >> 4) & 0x03;
|
|
243 |
}
|
|
244 |
if (accel != NULL) {
|
|
245 |
*accel = (calData >> 2) & 0x03;
|
|
246 |
}
|
|
247 |
if (mag != NULL) {
|
|
248 |
*mag = calData & 0x03;
|
|
249 |
}
|
|
249 | 250 |
} |
250 | 251 |
|
251 | 252 |
/**************************************************************************/ |
252 | 253 |
/*! |
253 |
@brief Gets the temperature in degrees celsius
|
|
254 |
*/ |
|
254 |
@brief Gets the temperature in degrees celsius
|
|
255 |
*/
|
|
255 | 256 |
/**************************************************************************/ |
256 | 257 |
int8_t Adafruit_BNO055::getTemp(void) |
257 | 258 |
{ |
258 |
int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
|
|
259 |
return temp;
|
|
259 |
int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
|
|
260 |
return temp;
|
|
260 | 261 |
} |
261 | 262 |
|
262 | 263 |
/**************************************************************************/ |
263 | 264 |
/*! |
264 |
@brief Gets a vector reading from the specified source
|
|
265 |
*/ |
|
265 |
@brief Gets a vector reading from the specified source
|
|
266 |
*/
|
|
266 | 267 |
/**************************************************************************/ |
267 | 268 |
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) |
268 | 269 |
{ |
269 |
imu::Vector<3> xyz;
|
|
270 |
uint8_t buffer[6];
|
|
271 |
memset (buffer, 0, 6);
|
|
272 |
|
|
273 |
int16_t x, y, z;
|
|
274 |
x = y = z = 0;
|
|
275 |
|
|
276 |
/* Read vector data (6 bytes) */
|
|
277 |
readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
|
|
278 |
|
|
279 |
x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
|
|
280 |
y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
|
|
281 |
z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
|
|
282 |
|
|
283 |
/* Convert the value to an appropriate range (section 3.6.4) */
|
|
284 |
/* and assign the value to the Vector type */
|
|
285 |
switch(vector_type)
|
|
286 |
{
|
|
287 |
case VECTOR_MAGNETOMETER:
|
|
288 |
/* 1uT = 16 LSB */
|
|
289 |
xyz[0] = ((double)x)/16.0;
|
|
290 |
xyz[1] = ((double)y)/16.0;
|
|
291 |
xyz[2] = ((double)z)/16.0;
|
|
292 |
break;
|
|
293 |
case VECTOR_GYROSCOPE:
|
|
294 |
/* 1rps = 900 LSB */
|
|
295 |
xyz[0] = ((double)x)/900.0;
|
|
296 |
xyz[1] = ((double)y)/900.0;
|
|
297 |
xyz[2] = ((double)z)/900.0;
|
|
298 |
break;
|
|
299 |
case VECTOR_EULER:
|
|
300 |
/* 1 degree = 16 LSB */
|
|
301 |
xyz[0] = ((double)x)/16.0;
|
|
302 |
xyz[1] = ((double)y)/16.0;
|
|
303 |
xyz[2] = ((double)z)/16.0;
|
|
304 |
break;
|
|
305 |
case VECTOR_ACCELEROMETER:
|
|
306 |
case VECTOR_LINEARACCEL:
|
|
307 |
case VECTOR_GRAVITY:
|
|
308 |
/* 1m/s^2 = 100 LSB */
|
|
309 |
xyz[0] = ((double)x)/100.0;
|
|
310 |
xyz[1] = ((double)y)/100.0;
|
|
311 |
xyz[2] = ((double)z)/100.0;
|
|
312 |
break;
|
|
313 |
}
|
|
314 |
|
|
315 |
return xyz;
|
|
270 |
imu::Vector<3> xyz;
|
|
271 |
uint8_t buffer[6];
|
|
272 |
memset(buffer, 0, 6);
|
|
273 |
|
|
274 |
int16_t x, y, z;
|
|
275 |
x = y = z = 0;
|
|
276 |
|
|
277 |
/* Read vector data (6 bytes) */
|
|
278 |
readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
|
|
279 |
|
|
280 |
x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
|
|
281 |
y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
|
|
282 |
z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
|
|
283 |
|
|
284 |
/* Convert the value to an appropriate range (section 3.6.4) */
|
|
285 |
/* and assign the value to the Vector type */
|
|
286 |
switch (vector_type)
|
|
287 |
{
|
|
288 |
case VECTOR_MAGNETOMETER:
|
|
289 |
/* 1uT = 16 LSB */
|
|
290 |
xyz[0] = ((double)x) / 16.0;
|
|
291 |
xyz[1] = ((double)y) / 16.0;
|
|
292 |
xyz[2] = ((double)z) / 16.0;
|
|
293 |
break;
|
|
294 |
case VECTOR_GYROSCOPE:
|
|
295 |
/* 1rps = 900 LSB */
|
|
296 |
xyz[0] = ((double)x) / 900.0;
|
|
297 |
xyz[1] = ((double)y) / 900.0;
|
|
298 |
xyz[2] = ((double)z) / 900.0;
|
|
299 |
break;
|
|
300 |
case VECTOR_EULER:
|
|
301 |
/* 1 degree = 16 LSB */
|
|
302 |
xyz[0] = ((double)x) / 16.0;
|
|
303 |
xyz[1] = ((double)y) / 16.0;
|
|
304 |
xyz[2] = ((double)z) / 16.0;
|
|
305 |
break;
|
|
306 |
case VECTOR_ACCELEROMETER:
|
|
307 |
case VECTOR_LINEARACCEL:
|
|
308 |
case VECTOR_GRAVITY:
|
|
309 |
/* 1m/s^2 = 100 LSB */
|
|
310 |
xyz[0] = ((double)x) / 100.0;
|
|
311 |
xyz[1] = ((double)y) / 100.0;
|
|
312 |
xyz[2] = ((double)z) / 100.0;
|
|
313 |
break;
|
|
314 |
}
|
|
315 |
|
|
316 |
return xyz;
|
|
316 | 317 |
} |
317 | 318 |
|
318 | 319 |
/**************************************************************************/ |
319 | 320 |
/*! |
320 |
@brief Gets a quaternion reading from the specified source
|
|
321 |
*/ |
|
321 |
@brief Gets a quaternion reading from the specified source
|
|
322 |
*/
|
|
322 | 323 |
/**************************************************************************/ |
323 | 324 |
imu::Quaternion Adafruit_BNO055::getQuat(void) |
324 | 325 |
{ |
325 |
uint8_t buffer[8];
|
|
326 |
memset (buffer, 0, 8);
|
|
327 |
|
|
328 |
int16_t x, y, z, w;
|
|
329 |
x = y = z = w = 0;
|
|
330 |
|
|
331 |
/* Read quat data (8 bytes) */
|
|
332 |
readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
|
|
333 |
w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
|
|
334 |
x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
|
|
335 |
y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
|
|
336 |
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
|
|
337 |
|
|
338 |
/* Assign to Quaternion */
|
|
339 |
/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
|
340 |
3.6.5.5 Orientation (Quaternion) */
|
|
341 |
const double scale = (1.0 / (1<<14));
|
|
342 |
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
|
|
343 |
return quat;
|
|
326 |
uint8_t buffer[8];
|
|
327 |
memset(buffer, 0, 8);
|
|
328 |
|
|
329 |
int16_t x, y, z, w;
|
|
330 |
x = y = z = w = 0;
|
|
331 |
|
|
332 |
/* Read quat data (8 bytes) */
|
|
333 |
readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
|
|
334 |
w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
|
|
335 |
x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
|
|
336 |
y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
|
|
337 |
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
|
|
338 |
|
|
339 |
/* Assign to Quaternion */
|
|
340 |
/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
|
341 |
3.6.5.5 Orientation (Quaternion) */
|
|
342 |
const double scale = (1.0 / (1 << 14));
|
|
343 |
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
|
|
344 |
return quat;
|
|
344 | 345 |
} |
345 | 346 |
|
346 | 347 |
/**************************************************************************/ |
347 | 348 |
/*! |
348 |
@brief Provides the sensor_t data for this sensor
|
|
349 |
*/ |
|
349 |
@brief Provides the sensor_t data for this sensor
|
|
350 |
*/
|
|
350 | 351 |
/**************************************************************************/ |
351 | 352 |
void Adafruit_BNO055::getSensor(sensor_t *sensor) |
352 | 353 |
{ |
353 |
/* Clear the sensor_t object */
|
|
354 |
memset(sensor, 0, sizeof(sensor_t));
|
|
355 |
|
|
356 |
/* Insert the sensor name in the fixed length char array */
|
|
357 |
strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
|
|
358 |
sensor->name[sizeof(sensor->name)- 1] = 0;
|
|
359 |
sensor->version = 1;
|
|
360 |
sensor->sensor_id = _sensorID;
|
|
361 |
sensor->type = SENSOR_TYPE_ORIENTATION;
|
|
362 |
sensor->min_delay = 0;
|
|
363 |
sensor->max_value = 0.0F;
|
|
364 |
sensor->min_value = 0.0F;
|
|
365 |
sensor->resolution = 0.01F;
|
|
354 |
/* Clear the sensor_t object */
|
|
355 |
memset(sensor, 0, sizeof(sensor_t));
|
|
356 |
|
|
357 |
/* Insert the sensor name in the fixed length char array */
|
|
358 |
strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1);
|
|
359 |
sensor->name[sizeof(sensor->name) - 1] = 0;
|
|
360 |
sensor->version = 1;
|
|
361 |
sensor->sensor_id = _sensorID;
|
|
362 |
sensor->type = SENSOR_TYPE_ORIENTATION;
|
|
363 |
sensor->min_delay = 0;
|
|
364 |
sensor->max_value = 0.0F;
|
|
365 |
sensor->min_value = 0.0F;
|
|
366 |
sensor->resolution = 0.01F;
|
|
366 | 367 |
} |
367 | 368 |
|
368 | 369 |
/**************************************************************************/ |
369 | 370 |
/*! |
370 |
@brief Reads the sensor and returns the data as a sensors_event_t
|
|
371 |
*/ |
|
371 |
@brief Reads the sensor and returns the data as a sensors_event_t
|
|
372 |
*/
|
|
372 | 373 |
/**************************************************************************/ |
373 | 374 |
bool Adafruit_BNO055::getEvent(sensors_event_t *event) |
374 | 375 |
{ |
375 |
/* Clear the event */
|
|
376 |
memset(event, 0, sizeof(sensors_event_t));
|
|
376 |
/* Clear the event */
|
|
377 |
memset(event, 0, sizeof(sensors_event_t));
|
|
377 | 378 |
|
378 |
event->version = sizeof(sensors_event_t);
|
|
379 |
event->sensor_id = _sensorID;
|
|
380 |
event->type = SENSOR_TYPE_ORIENTATION;
|
|
381 |
event->timestamp = millis();
|
|
379 |
event->version = sizeof(sensors_event_t);
|
|
380 |
event->sensor_id = _sensorID;
|
|
381 |
event->type = SENSOR_TYPE_ORIENTATION;
|
|
382 |
event->timestamp = millis();
|
|
382 | 383 |
|
383 |
/* Get a Euler angle sample for orientation */ |
|
384 |
imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER); |
|
385 |
event->orientation.x = euler.x(); |
|
386 |
event->orientation.y = euler.y(); |
|
387 |
event->orientation.z = euler.z(); |
|
384 |
/* Get a Euler angle sample for orientation */ |
|
385 |
imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER); |
|
386 |
event->orientation.x = euler.x(); |
|
387 |
event->orientation.y = euler.y(); |
|
388 |
event->orientation.z = euler.z(); |
|
389 |
|
|
390 |
return true; |
|
391 |
} |
|
388 | 392 |
|
389 |
return true; |
|
393 |
/**************************************************************************/ |
|
394 |
/*! |
|
395 |
@brief Reads the sensor's offset registers into a byte array |
|
396 |
*/ |
|
397 |
/**************************************************************************/ |
|
398 |
bool Adafruit_BNO055::getSensorOffsets(int8_t* calibData) |
|
399 |
{ |
|
400 |
if (isFullyCalibrated()) |
|
401 |
{ |
|
402 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
403 |
setMode(OPERATION_MODE_CONFIG); |
|
404 |
|
|
405 |
readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS); |
|
406 |
|
|
407 |
setMode(lastMode); |
|
408 |
return true; |
|
409 |
} |
|
410 |
return false; |
|
411 |
} |
|
412 |
|
|
413 |
/**************************************************************************/ |
|
414 |
/*! |
|
415 |
@brief Reads the sensor's offset registers into an offset struct |
|
416 |
*/ |
|
417 |
/**************************************************************************/ |
|
418 |
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) |
|
419 |
{ |
|
420 |
if (isFullyCalibrated()) |
|
421 |
{ |
|
422 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
423 |
setMode(OPERATION_MODE_CONFIG); |
|
424 |
delay(25); |
|
425 |
|
|
426 |
offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR)); |
|
427 |
offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR)); |
|
428 |
offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR)); |
|
429 |
|
|
430 |
offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR)); |
|
431 |
offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR)); |
|
432 |
offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR)); |
|
433 |
|
|
434 |
offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR)); |
|
435 |
offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR)); |
|
436 |
offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR)); |
|
437 |
|
|
438 |
offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR)); |
|
439 |
offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR)); |
|
440 |
|
|
441 |
setMode(lastMode); |
|
442 |
return true; |
|
443 |
} |
|
444 |
return false; |
|
445 |
} |
|
446 |
|
|
447 |
|
|
448 |
/**************************************************************************/ |
|
449 |
/*! |
|
450 |
@brief Writes an array of calibration values to the sensor's offset registers |
|
451 |
*/ |
|
452 |
/**************************************************************************/ |
|
453 |
void Adafruit_BNO055::setSensorOffsets(const int8_t* calibData) |
|
454 |
{ |
|
455 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
456 |
setMode(OPERATION_MODE_CONFIG); |
|
457 |
delay(25); |
|
458 |
|
|
459 |
/* A writeLen() would make this much cleaner */ |
|
460 |
write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]); |
|
461 |
write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]); |
|
462 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]); |
|
463 |
write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]); |
|
464 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]); |
|
465 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]); |
|
466 |
|
|
467 |
write8(GYRO_OFFSET_X_LSB_ADDR, calibData[6]); |
|
468 |
write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]); |
|
469 |
write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]); |
|
470 |
write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]); |
|
471 |
write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]); |
|
472 |
write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]); |
|
473 |
|
|
474 |
write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]); |
|
475 |
write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]); |
|
476 |
write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]); |
|
477 |
write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]); |
|
478 |
write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]); |
|
479 |
write8(MAG_OFFSET_Z_MSB_ADDR, calibData[17]); |
|
480 |
|
|
481 |
write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]); |
|
482 |
write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]); |
|
483 |
|
|
484 |
write8(MAG_RADIUS_LSB_ADDR, calibData[20]); |
|
485 |
write8(MAG_RADIUS_MSB_ADDR, calibData[21]); |
|
486 |
|
|
487 |
setMode(lastMode); |
|
390 | 488 |
} |
391 | 489 |
|
490 |
/**************************************************************************/ |
|
491 |
/*! |
|
492 |
@brief Writes to the sensor's offset registers from an offset struct |
|
493 |
*/ |
|
494 |
/**************************************************************************/ |
|
495 |
void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) |
|
496 |
{ |
|
497 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
498 |
setMode(OPERATION_MODE_CONFIG); |
|
499 |
delay(25); |
|
500 |
|
|
501 |
write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF); |
|
502 |
write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); |
|
503 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF); |
|
504 |
write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); |
|
505 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF); |
|
506 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); |
|
507 |
|
|
508 |
write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF); |
|
509 |
write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); |
|
510 |
write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF); |
|
511 |
write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); |
|
512 |
write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF); |
|
513 |
write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); |
|
514 |
|
|
515 |
write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF); |
|
516 |
write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); |
|
517 |
write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF); |
|
518 |
write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); |
|
519 |
write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF); |
|
520 |
write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); |
|
521 |
|
|
522 |
write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF); |
|
523 |
write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); |
|
524 |
|
|
525 |
write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF); |
|
526 |
write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); |
|
527 |
|
|
528 |
setMode(lastMode); |
|
529 |
} |
|
530 |
|
|
531 |
bool Adafruit_BNO055::isFullyCalibrated(void) |
|
532 |
{ |
|
533 |
uint8_t system, gyro, accel, mag; |
|
534 |
getCalibration(&system, &gyro, &accel, &mag); |
|
535 |
if (system < 3 || gyro < 3 || accel < 3 || mag < 3) |
|
536 |
return false; |
|
537 |
return true; |
|
538 |
} |
|
539 |
|
|
540 |
|
|
541 |
|
|
392 | 542 |
/*************************************************************************** |
393 | 543 |
PRIVATE FUNCTIONS |
394 | 544 |
***************************************************************************/ |
395 | 545 |
|
396 | 546 |
/**************************************************************************/ |
397 | 547 |
/*! |
398 |
@brief Writes an 8 bit value over I2C
|
|
399 |
*/ |
|
548 |
@brief Writes an 8 bit value over I2C
|
|
549 |
*/
|
|
400 | 550 |
/**************************************************************************/ |
401 | 551 |
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) |
402 | 552 |
{ |
403 |
Wire.beginTransmission(_address);
|
|
404 |
#if ARDUINO >= 100
|
|
405 |
Wire.write((uint8_t)reg);
|
|
406 |
Wire.write((uint8_t)value);
|
|
407 |
#else
|
|
408 |
Wire.send(reg);
|
|
409 |
Wire.send(value);
|
|
410 |
#endif
|
|
411 |
Wire.endTransmission();
|
|
412 |
|
|
413 |
/* ToDo: Check for error! */
|
|
414 |
return true;
|
|
553 |
Wire.beginTransmission(_address);
|
|
554 |
#if ARDUINO >= 100 |
|
555 |
Wire.write((uint8_t)reg);
|
|
556 |
Wire.write((uint8_t)value);
|
|
557 |
#else |
|
558 |
Wire.send(reg);
|
|
559 |
Wire.send(value);
|
|
560 |
#endif |
|
561 |
Wire.endTransmission();
|
|
562 |
|
|
563 |
/* ToDo: Check for error! */
|
|
564 |
return true;
|
|
415 | 565 |
} |
416 | 566 |
|
417 | 567 |
/**************************************************************************/ |
418 | 568 |
/*! |
419 |
@brief Reads an 8 bit value over I2C
|
|
420 |
*/ |
|
569 |
@brief Reads an 8 bit value over I2C
|
|
570 |
*/
|
|
421 | 571 |
/**************************************************************************/ |
422 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
|
|
572 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) |
|
423 | 573 |
{ |
424 |
byte value = 0;
|
|
425 |
|
|
426 |
Wire.beginTransmission(_address);
|
|
427 |
#if ARDUINO >= 100
|
|
428 |
Wire.write((uint8_t)reg);
|
|
429 |
#else
|
|
430 |
Wire.send(reg);
|
|
431 |
#endif
|
|
432 |
Wire.endTransmission();
|
|
433 |
Wire.requestFrom(_address, (byte)1);
|
|
434 |
#if ARDUINO >= 100
|
|
435 |
value = Wire.read();
|
|
436 |
#else
|
|
437 |
value = Wire.receive();
|
|
438 |
#endif
|
|
439 |
|
|
440 |
return value;
|
|
574 |
byte value = 0;
|
|
575 |
|
|
576 |
Wire.beginTransmission(_address);
|
|
577 |
#if ARDUINO >= 100 |
|
578 |
Wire.write((uint8_t)reg);
|
|
579 |
#else |
|
580 |
Wire.send(reg);
|
|
581 |
#endif |
|
582 |
Wire.endTransmission();
|
|
583 |
Wire.requestFrom(_address, (byte)1);
|
|
584 |
#if ARDUINO >= 100 |
|
585 |
value = Wire.read();
|
|
586 |
#else |
|
587 |
value = Wire.receive();
|
|
588 |
#endif |
|
589 |
|
|
590 |
return value;
|
|
441 | 591 |
} |
442 | 592 |
|
443 | 593 |
/**************************************************************************/ |
444 | 594 |
/*! |
445 |
@brief Reads the specified number of bytes over I2C
|
|
446 |
*/ |
|
595 |
@brief Reads the specified number of bytes over I2C
|
|
596 |
*/
|
|
447 | 597 |
/**************************************************************************/ |
448 | 598 |
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len) |
449 | 599 |
{ |
450 |
Wire.beginTransmission(_address);
|
|
451 |
#if ARDUINO >= 100
|
|
452 |
Wire.write((uint8_t)reg);
|
|
453 |
#else
|
|
454 |
Wire.send(reg);
|
|
455 |
#endif
|
|
456 |
Wire.endTransmission();
|
|
457 |
Wire.requestFrom(_address, (byte)len);
|
|
458 |
|
|
459 |
for (uint8_t i = 0; i < len; i++)
|
|
460 |
{
|
|
461 |
#if ARDUINO >= 100
|
|
462 |
buffer[i] = Wire.read();
|
|
463 |
#else
|
|
464 |
buffer[i] = Wire.receive();
|
|
465 |
#endif
|
|
466 |
}
|
|
467 |
|
|
468 |
/* ToDo: Check for errors! */
|
|
469 |
return true;
|
|
470 |
} |
|
600 |
Wire.beginTransmission(_address);
|
|
601 |
#if ARDUINO >= 100 |
|
602 |
Wire.write((uint8_t)reg);
|
|
603 |
#else |
|
604 |
Wire.send(reg);
|
|
605 |
#endif |
|
606 |
Wire.endTransmission();
|
|
607 |
Wire.requestFrom(_address, (byte)len);
|
|
608 |
|
|
609 |
for (uint8_t i = 0; i < len; i++)
|
|
610 |
{
|
|
611 |
#if ARDUINO >= 100 |
|
612 |
buffer[i] = Wire.read();
|
|
613 |
#else |
|
614 |
buffer[i] = Wire.receive();
|
|
615 |
#endif |
|
616 |
}
|
|
617 |
|
|
618 |
/* ToDo: Check for errors! */
|
|
619 |
return true;
|
|
620 |
} |
Adafruit_BNO055.h | ||
---|---|---|
15 | 15 |
Written by KTOWN for Adafruit Industries. |
16 | 16 |
|
17 | 17 |
MIT license, all text above must be included in any redistribution |
18 |
***************************************************************************/ |
|
18 |
***************************************************************************/
|
|
19 | 19 |
|
20 | 20 |
#ifndef __ADAFRUIT_BNO055_H__ |
21 | 21 |
#define __ADAFRUIT_BNO055_H__ |
22 | 22 |
|
23 | 23 |
#if (ARDUINO >= 100) |
24 |
#include "Arduino.h"
|
|
24 |
#include "Arduino.h" |
|
25 | 25 |
#else |
26 |
#include "WProgram.h"
|
|
26 |
#include "WProgram.h" |
|
27 | 27 |
#endif |
28 | 28 |
|
29 | 29 |
#ifdef __AVR_ATtiny85__ |
30 |
#include <TinyWireM.h>
|
|
31 |
#define Wire TinyWireM
|
|
30 |
#include <TinyWireM.h> |
|
31 |
#define Wire TinyWireM |
|
32 | 32 |
#else |
33 |
#include <Wire.h>
|
|
33 |
#include <Wire.h> |
|
34 | 34 |
#endif |
35 | 35 |
|
36 | 36 |
#include <Adafruit_Sensor.h> |
... | ... | |
40 | 40 |
#define BNO055_ADDRESS_B (0x29) |
41 | 41 |
#define BNO055_ID (0xA0) |
42 | 42 |
|
43 |
#define NUM_BNO055_OFFSET_REGISTERS (22) |
|
44 |
|
|
45 |
typedef struct |
|
46 |
{ |
|
47 |
int16_t accel_offset_x; |
|
48 |
int16_t accel_offset_y; |
|
49 |
int16_t accel_offset_z; |
|
50 |
int16_t gyro_offset_x; |
|
51 |
int16_t gyro_offset_y; |
|
52 |
int16_t gyro_offset_z; |
|
53 |
int16_t mag_offset_x; |
|
54 |
int16_t mag_offset_y; |
|
55 |
int16_t mag_offset_z; |
|
56 |
|
|
57 |
int16_t accel_radius; |
|
58 |
int16_t mag_radius; |
|
59 |
} adafruit_bno055_offsets_t; |
|
60 |
|
|
43 | 61 |
class Adafruit_BNO055 : public Adafruit_Sensor |
44 | 62 |
{ |
45 |
public: |
|
46 |
typedef enum |
|
47 |
{ |
|
48 |
/* Page id register definition */ |
|
49 |
BNO055_PAGE_ID_ADDR = 0X07, |
|
50 |
|
|
51 |
/* PAGE0 REGISTER DEFINITION START*/ |
|
52 |
BNO055_CHIP_ID_ADDR = 0x00, |
|
53 |
BNO055_ACCEL_REV_ID_ADDR = 0x01, |
|
54 |
BNO055_MAG_REV_ID_ADDR = 0x02, |
|
55 |
BNO055_GYRO_REV_ID_ADDR = 0x03, |
|
56 |
BNO055_SW_REV_ID_LSB_ADDR = 0x04, |
|
57 |
BNO055_SW_REV_ID_MSB_ADDR = 0x05, |
|
58 |
BNO055_BL_REV_ID_ADDR = 0X06, |
|
59 |
|
|
60 |
/* Accel data register */ |
|
61 |
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, |
|
62 |
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, |
|
63 |
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, |
|
64 |
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, |
|
65 |
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, |
|
66 |
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, |
|
67 |
|
|
68 |
/* Mag data register */ |
|
69 |
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, |
|
70 |
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, |
|
71 |
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, |
|
72 |
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, |
|
73 |
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, |
|
74 |
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, |
|
75 |
|
|
76 |
/* Gyro data registers */ |
|
77 |
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, |
|
78 |
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, |
|
79 |
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, |
|
80 |
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, |
|
81 |
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, |
|
82 |
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, |
|
83 |
|
|
84 |
/* Euler data registers */ |
|
85 |
BNO055_EULER_H_LSB_ADDR = 0X1A, |
|
86 |
BNO055_EULER_H_MSB_ADDR = 0X1B, |
|
87 |
BNO055_EULER_R_LSB_ADDR = 0X1C, |
|
88 |
BNO055_EULER_R_MSB_ADDR = 0X1D, |
|
89 |
BNO055_EULER_P_LSB_ADDR = 0X1E, |
|
90 |
BNO055_EULER_P_MSB_ADDR = 0X1F, |
|
91 |
|
|
92 |
/* Quaternion data registers */ |
|
93 |
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, |
|
94 |
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, |
|
95 |
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, |
|
96 |
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, |
|
97 |
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, |
|
98 |
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, |
|
99 |
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, |
|
100 |
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, |
|
101 |
|
|
102 |
/* Linear acceleration data registers */ |
|
103 |
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, |
|
104 |
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, |
|
105 |
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, |
|
106 |
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, |
|
107 |
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, |
|
108 |
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, |
|
109 |
|
|
110 |
/* Gravity data registers */ |
|
111 |
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, |
|
112 |
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, |
|
113 |
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, |
|
114 |
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, |
|
115 |
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, |
|
116 |
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, |
|
117 |
|
|
118 |
/* Temperature data register */ |
|
119 |
BNO055_TEMP_ADDR = 0X34, |
|
120 |
|
|
121 |
/* Status registers */ |
|
122 |
BNO055_CALIB_STAT_ADDR = 0X35, |
|
123 |
BNO055_SELFTEST_RESULT_ADDR = 0X36, |
|
124 |
BNO055_INTR_STAT_ADDR = 0X37, |
|
125 |
|
|
126 |
BNO055_SYS_CLK_STAT_ADDR = 0X38, |
|
127 |
BNO055_SYS_STAT_ADDR = 0X39, |
|
128 |
BNO055_SYS_ERR_ADDR = 0X3A, |
|
129 |
|
|
130 |
/* Unit selection register */ |
|
131 |
BNO055_UNIT_SEL_ADDR = 0X3B, |
|
132 |
BNO055_DATA_SELECT_ADDR = 0X3C, |
|
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 |
} adafruit_bno055_reg_t; |
|
195 |
|
|
196 |
typedef enum |
|
197 |
{ |
|
198 |
POWER_MODE_NORMAL = 0X00, |
|
199 |
POWER_MODE_LOWPOWER = 0X01, |
|
200 |
POWER_MODE_SUSPEND = 0X02 |
|
201 |
} adafruit_bno055_powermode_t; |
|
202 |
|
|
203 |
typedef enum |
|
204 |
{ |
|
205 |
/* Operation mode settings*/ |
|
206 |
OPERATION_MODE_CONFIG = 0X00, |
|
207 |
OPERATION_MODE_ACCONLY = 0X01, |
|
208 |
OPERATION_MODE_MAGONLY = 0X02, |
|
209 |
OPERATION_MODE_GYRONLY = 0X03, |
|
210 |
OPERATION_MODE_ACCMAG = 0X04, |
|
211 |
OPERATION_MODE_ACCGYRO = 0X05, |
|
212 |
OPERATION_MODE_MAGGYRO = 0X06, |
|
213 |
OPERATION_MODE_AMG = 0X07, |
|
214 |
OPERATION_MODE_IMUPLUS = 0X08, |
|
215 |
OPERATION_MODE_COMPASS = 0X09, |
|
216 |
OPERATION_MODE_M4G = 0X0A, |
|
217 |
OPERATION_MODE_NDOF_FMC_OFF = 0X0B, |
|
218 |
OPERATION_MODE_NDOF = 0X0C |
|
219 |
} adafruit_bno055_opmode_t; |
|
220 |
|
|
221 |
typedef struct |
|
222 |
{ |
|
223 |
uint8_t accel_rev; |
|
224 |
uint8_t mag_rev; |
|
225 |
uint8_t gyro_rev; |
|
226 |
uint16_t sw_rev; |
|
227 |
uint8_t bl_rev; |
|
228 |
} adafruit_bno055_rev_info_t; |
|
229 |
|
|
230 |
typedef enum |
|
231 |
{ |
|
232 |
VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, |
|
233 |
VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, |
|
234 |
VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, |
|
235 |
VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, |
|
236 |
VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, |
|
237 |
VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR |
|
238 |
} adafruit_vector_type_t; |
|
239 |
|
|
240 |
Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A ); |
|
241 |
|
|
242 |
bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF ); |
|
243 |
void setMode ( adafruit_bno055_opmode_t mode ); |
|
244 |
void getRevInfo ( adafruit_bno055_rev_info_t* ); |
|
245 |
void displayRevInfo ( void ); |
|
246 |
void setExtCrystalUse ( boolean usextal ); |
|
247 |
void getSystemStatus ( uint8_t *system_status, |
|
248 |
uint8_t *self_test_result, |
|
249 |
uint8_t *system_error); |
|
250 |
void displaySystemStatus ( void ); |
|
251 |
void getCalibration ( uint8_t* system, uint8_t* gyro, uint8_t* accel, uint8_t* mag); |
|
252 |
|
|
253 |
imu::Vector<3> getVector ( adafruit_vector_type_t vector_type ); |
|
254 |
imu::Quaternion getQuat ( void ); |
|
255 |
int8_t getTemp ( void ); |
|
256 |
|
|
257 |
/* Adafruit_Sensor implementation */ |
|
258 |
bool getEvent ( sensors_event_t* ); |
|
259 |
void getSensor ( sensor_t* ); |
|
260 |
|
|
261 |
private: |
|
262 |
byte read8 ( adafruit_bno055_reg_t ); |
|
263 |
bool readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len ); |
|
264 |
bool write8 ( adafruit_bno055_reg_t, byte value ); |
|
265 |
|
|
266 |
uint8_t _address; |
|
267 |
int32_t _sensorID; |
|
268 |
adafruit_bno055_opmode_t _mode; |
|
63 |
public: |
|
64 |
typedef enum |
|
65 |
{ |
|
66 |
/* Page id register definition */ |
|
67 |
BNO055_PAGE_ID_ADDR = 0X07, |
|
68 |
|
|
69 |
/* PAGE0 REGISTER DEFINITION START*/ |
|
70 |
BNO055_CHIP_ID_ADDR = 0x00, |
|
71 |
BNO055_ACCEL_REV_ID_ADDR = 0x01, |
|
72 |
BNO055_MAG_REV_ID_ADDR = 0x02, |
|
73 |
BNO055_GYRO_REV_ID_ADDR = 0x03, |
|
74 |
BNO055_SW_REV_ID_LSB_ADDR = 0x04, |
|
75 |
BNO055_SW_REV_ID_MSB_ADDR = 0x05, |
|
76 |
BNO055_BL_REV_ID_ADDR = 0X06, |
|
77 |
|
|
78 |
/* Accel data register */ |
|
79 |
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, |
|
80 |
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, |
|
81 |
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, |
|
82 |
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, |
|
83 |
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, |
|
84 |
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, |
|
85 |
|
|
86 |
/* Mag data register */ |
|
87 |
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, |
|
88 |
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, |
|
89 |
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, |
|
90 |
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, |
|
91 |
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, |
|
92 |
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, |
|
93 |
|
|
94 |
/* Gyro data registers */ |
|
95 |
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, |
|
96 |
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, |
|
97 |
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, |
|
98 |
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, |
|
99 |
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, |
|
100 |
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, |
|
101 |
|
|
102 |
/* Euler data registers */ |
|
103 |
BNO055_EULER_H_LSB_ADDR = 0X1A, |
|
104 |
BNO055_EULER_H_MSB_ADDR = 0X1B, |
|
105 |
BNO055_EULER_R_LSB_ADDR = 0X1C, |
|
106 |
BNO055_EULER_R_MSB_ADDR = 0X1D, |
|
107 |
BNO055_EULER_P_LSB_ADDR = 0X1E, |
|
108 |
BNO055_EULER_P_MSB_ADDR = 0X1F, |
|
109 |
|
|
110 |
/* Quaternion data registers */ |
|
111 |
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, |
|
112 |
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, |
|
113 |
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, |
|
114 |
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, |
|
115 |
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, |
|
116 |
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, |
|
117 |
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, |
|
118 |
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, |
|
119 |
|
|
120 |
/* Linear acceleration data registers */ |
|
121 |
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, |
|
122 |
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, |
|
123 |
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, |
|
124 |
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, |
|
125 |
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, |
|
126 |
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, |
|
127 |
|
|
128 |
/* Gravity data registers */ |
|
129 |
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, |
|
130 |
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, |
|
131 |
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, |
|
132 |
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, |
|
133 |
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, |
|
134 |
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, |
|
135 |
|
|
136 |
/* Temperature data register */ |
|
137 |
BNO055_TEMP_ADDR = 0X34, |
|
138 |
|
|
139 |
/* Status registers */ |
|
140 |
BNO055_CALIB_STAT_ADDR = 0X35, |
|
141 |
BNO055_SELFTEST_RESULT_ADDR = 0X36, |
|
142 |
BNO055_INTR_STAT_ADDR = 0X37, |
|
143 |
|
|
144 |
BNO055_SYS_CLK_STAT_ADDR = 0X38, |
|
145 |
BNO055_SYS_STAT_ADDR = 0X39, |
|
146 |
BNO055_SYS_ERR_ADDR = 0X3A, |
|
147 |
|
|
148 |
/* Unit selection register */ |
|
149 |
BNO055_UNIT_SEL_ADDR = 0X3B, |
|
150 |
BNO055_DATA_SELECT_ADDR = 0X3C, |
|
151 |
|
|
152 |
/* Mode registers */ |
|
153 |
BNO055_OPR_MODE_ADDR = 0X3D, |
|
154 |
BNO055_PWR_MODE_ADDR = 0X3E, |
|
155 |
|
|
156 |
BNO055_SYS_TRIGGER_ADDR = 0X3F, |
|
157 |
BNO055_TEMP_SOURCE_ADDR = 0X40, |
|
158 |
|
|
159 |
/* Axis remap registers */ |
|
160 |
BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, |
|
161 |
BNO055_AXIS_MAP_SIGN_ADDR = 0X42, |
|
162 |
|
|
163 |
/* SIC registers */ |
|
164 |
BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, |
|
165 |
BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, |
|
166 |
BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, |
|
167 |
BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, |
|
168 |
BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, |
|
169 |
BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, |
|
170 |
BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, |
|
171 |
BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, |
|
172 |
BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, |
|
173 |
BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, |
|
174 |
BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, |
|
175 |
BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, |
|
176 |
BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, |
|
177 |
BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, |
|
178 |
BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, |
|
179 |
BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, |
|
180 |
BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, |
|
181 |
BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, |
|
182 |
|
|
183 |
/* Accelerometer Offset registers */ |
|
184 |
ACCEL_OFFSET_X_LSB_ADDR = 0X55, |
|
185 |
ACCEL_OFFSET_X_MSB_ADDR = 0X56, |
|
186 |
ACCEL_OFFSET_Y_LSB_ADDR = 0X57, |
|
187 |
ACCEL_OFFSET_Y_MSB_ADDR = 0X58, |
|
188 |
ACCEL_OFFSET_Z_LSB_ADDR = 0X59, |
|
189 |
ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, |
|
190 |
|
|
191 |
/* Magnetometer Offset registers */ |
|
192 |
MAG_OFFSET_X_LSB_ADDR = 0X5B, |
|
193 |
MAG_OFFSET_X_MSB_ADDR = 0X5C, |
|
194 |
MAG_OFFSET_Y_LSB_ADDR = 0X5D, |
|
195 |
MAG_OFFSET_Y_MSB_ADDR = 0X5E, |
|
196 |
MAG_OFFSET_Z_LSB_ADDR = 0X5F, |
|
197 |
MAG_OFFSET_Z_MSB_ADDR = 0X60, |
|
198 |
|
|
199 |
/* Gyroscope Offset register s*/ |
|
200 |
GYRO_OFFSET_X_LSB_ADDR = 0X61, |
|
201 |
GYRO_OFFSET_X_MSB_ADDR = 0X62, |
|
202 |
GYRO_OFFSET_Y_LSB_ADDR = 0X63, |
|
203 |
GYRO_OFFSET_Y_MSB_ADDR = 0X64, |
|
204 |
GYRO_OFFSET_Z_LSB_ADDR = 0X65, |
|
205 |
GYRO_OFFSET_Z_MSB_ADDR = 0X66, |
|
206 |
|
|
207 |
/* Radius registers */ |
|
208 |
ACCEL_RADIUS_LSB_ADDR = 0X67, |
|
209 |
ACCEL_RADIUS_MSB_ADDR = 0X68, |
|
210 |
MAG_RADIUS_LSB_ADDR = 0X69, |
|
211 |
MAG_RADIUS_MSB_ADDR = 0X6A |
|
212 |
} adafruit_bno055_reg_t; |
|
213 |
|
|
214 |
typedef enum |
|
215 |
{ |
|
216 |
POWER_MODE_NORMAL = 0X00, |
|
217 |
POWER_MODE_LOWPOWER = 0X01, |
|
218 |
POWER_MODE_SUSPEND = 0X02 |
|
219 |
} adafruit_bno055_powermode_t; |
|
220 |
|
|
221 |
typedef enum |
|
222 |
{ |
|
223 |
/* Operation mode settings*/ |
|
224 |
OPERATION_MODE_CONFIG = 0X00, |
|
225 |
OPERATION_MODE_ACCONLY = 0X01, |
|
226 |
OPERATION_MODE_MAGONLY = 0X02, |
|
227 |
OPERATION_MODE_GYRONLY = 0X03, |
|
228 |
OPERATION_MODE_ACCMAG = 0X04, |
|
229 |
OPERATION_MODE_ACCGYRO = 0X05, |
|
230 |
OPERATION_MODE_MAGGYRO = 0X06, |
|
231 |
OPERATION_MODE_AMG = 0X07, |
|
232 |
OPERATION_MODE_IMUPLUS = 0X08, |
|
233 |
OPERATION_MODE_COMPASS = 0X09, |
|
234 |
OPERATION_MODE_M4G = 0X0A, |
|
235 |
OPERATION_MODE_NDOF_FMC_OFF = 0X0B, |
|
236 |
OPERATION_MODE_NDOF = 0X0C |
|
237 |
} adafruit_bno055_opmode_t; |
|
238 |
|
|
239 |
typedef struct |
|
240 |
{ |
|
241 |
uint8_t accel_rev; |
|
242 |
uint8_t mag_rev; |
|
243 |
uint8_t gyro_rev; |
|
244 |
uint16_t sw_rev; |
|
245 |
uint8_t bl_rev; |
|
246 |
} adafruit_bno055_rev_info_t; |
|
247 |
|
|
248 |
typedef enum |
|
249 |
{ |
|
250 |
VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, |
|
251 |
VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, |
|
252 |
VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, |
|
253 |
VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, |
|
254 |
VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, |
|
255 |
VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR |
|
256 |
} adafruit_vector_type_t; |
|
257 |
|
|
258 |
|
|
259 |
|
|
260 |
Adafruit_BNO055(int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A); |
|
261 |
|
|
262 |
bool begin(adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF); |
|
263 |
void setMode(adafruit_bno055_opmode_t mode); |
|
264 |
void getRevInfo(adafruit_bno055_rev_info_t*); |
|
265 |
void displayRevInfo(void); |
|
266 |
void setExtCrystalUse(boolean usextal); |
|
267 |
void getSystemStatus(uint8_t *system_status, |
|
268 |
uint8_t *self_test_result, |
|
269 |
uint8_t *system_error); |
|
270 |
void displaySystemStatus(void); |
|
271 |
void getCalibration(uint8_t* system, uint8_t* gyro, uint8_t* accel, uint8_t* mag); |
|
272 |
|
|
273 |
imu::Vector<3> getVector(adafruit_vector_type_t vector_type); |
|
274 |
imu::Quaternion getQuat(void); |
|
275 |
int8_t getTemp(void); |
|
276 |
|
|
277 |
/* Adafruit_Sensor implementation */ |
|
278 |
bool getEvent(sensors_event_t*); |
|
279 |
void getSensor(sensor_t*); |
|
280 |
|
|
281 |
/* Functions to deal with raw calibration data */ |
|
282 |
bool getSensorOffsets(int8_t* calibData); |
|
283 |
bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type); |
|
284 |
void setSensorOffsets(const int8_t* calibData); |
|
285 |
void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); |
|
286 |
bool isFullyCalibrated(void); |
|
287 |
|
|
288 |
private: |
|
289 |
byte read8(adafruit_bno055_reg_t); |
|
290 |
bool readLen(adafruit_bno055_reg_t, byte* buffer, uint8_t len); |
|
291 |
bool write8(adafruit_bno055_reg_t, byte value); |
|
292 |
|
|
293 |
uint8_t _address; |
|
294 |
int32_t _sensorID; |
|
295 |
adafruit_bno055_opmode_t _mode; |
|
269 | 296 |
}; |
270 | 297 |
|
271 | 298 |
#endif |
examples/restore_offsets/restore_offsets.ino | ||
---|---|---|
1 |
#include <Wire.h> |
|
2 |
#include <Adafruit_Sensor.h> |
|
3 |
#include <Adafruit_BNO055.h> |
|
4 |
#include <utility/imumaths.h> |
|
5 |
#include <EEPROM.h> |
|
6 |
|
|
7 |
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), |
|
8 |
which provides a common 'type' for sensor data and some helper functions. |
|
9 |
|
|
10 |
To use this driver you will also need to download the Adafruit_Sensor |
|
11 |
library and include it in your libraries folder. |
|
12 |
|
|
13 |
You should also assign a unique ID to this sensor for use with |
|
14 |
the Adafruit Sensor API so that you can identify this particular |
|
15 |
sensor in any data logs, etc. To assign a unique ID, simply |
|
16 |
provide an appropriate value in the constructor below (12345 |
|
17 |
is used by default in this example). |
|
18 |
|
|
19 |
Connections |
|
20 |
=========== |
|
21 |
Connect SCL to analog 5 |
|
22 |
Connect SDA to analog 4 |
|
23 |
Connect VDD to 3-5V DC |
|
24 |
Connect GROUND to common ground |
|
25 |
|
|
26 |
History |
|
27 |
======= |
|
28 |
2015/MAR/03 - First release (KTOWN) |
|
29 |
2015/AUG/27 - Added calibration and system status helpers |
|
30 |
2015/NOV/13 - Added calibration save and restore |
|
31 |
*/ |
|
32 |
|
|
33 |
/* Set the delay between fresh samples */ |
|
34 |
#define BNO055_SAMPLERATE_DELAY_MS (100) |
|
35 |
|
|
36 |
Adafruit_BNO055 bno = Adafruit_BNO055(55); |
|
37 |
|
|
38 |
/**************************************************************************/ |
|
39 |
/* |
|
40 |
Displays some basic information on this sensor from the unified |
|
41 |
sensor API sensor_t type (see Adafruit_Sensor for more information) |
|
42 |
*/ |
|
43 |
/**************************************************************************/ |
|
44 |
void displaySensorDetails(void) |
|
45 |
{ |
|
46 |
sensor_t sensor; |
|
47 |
bno.getSensor(&sensor); |
Also available in: Unified diff