| 2 |
2 |
* @file Adafruit_BNO055.cpp
|
| 3 |
3 |
*
|
| 4 |
4 |
* @mainpage Adafruit BNO055 Orientation Sensor
|
| 5 |
|
*
|
|
5 |
*
|
| 6 |
6 |
* @section intro_sec Introduction
|
| 7 |
7 |
*
|
| 8 |
|
* This is a library for the BNO055 orientation sensor
|
|
8 |
* This is a library for the BNO055 orientation sensor
|
| 9 |
9 |
*
|
| 10 |
|
* Designed specifically to work with the Adafruit BNO055 Breakout.
|
|
10 |
* Designed specifically to work with the Adafruit BNO055 Breakout.
|
| 11 |
11 |
*
|
| 12 |
|
* Pick one up today in the adafruit shop!
|
| 13 |
|
* ------> https://www.adafruit.com/product/2472
|
|
12 |
* Pick one up today in the adafruit shop!
|
|
13 |
* ------> https://www.adafruit.com/product/2472
|
| 14 |
14 |
*
|
| 15 |
|
* These sensors use I2C to communicate, 2 pins are required to interface.
|
|
15 |
* These sensors use I2C to communicate, 2 pins are required to interface.
|
| 16 |
16 |
*
|
| 17 |
|
* Adafruit invests time and resources providing this open source code,
|
| 18 |
|
* please support Adafruit andopen-source hardware by purchasing products
|
| 19 |
|
* from Adafruit!
|
|
17 |
* Adafruit invests time and resources providing this open source code,
|
|
18 |
* please support Adafruit andopen-source hardware by purchasing products
|
|
19 |
* from Adafruit!
|
| 20 |
20 |
*
|
| 21 |
21 |
* @section author Author
|
| 22 |
22 |
*
|
| 23 |
23 |
* K.Townsend (Adafruit Industries)
|
| 24 |
24 |
*
|
| 25 |
|
* @section license License
|
| 26 |
|
* MIT license, all text above must be included in any redistribution
|
|
25 |
* @section license License
|
|
26 |
*
|
|
27 |
* MIT license, all text above must be included in any redistribution
|
| 27 |
28 |
*/
|
| 28 |
29 |
|
| 29 |
30 |
#if ARDUINO >= 100
|
| ... | ... | |
| 59 |
60 |
/* BNO055 clock stretches for 500us or more! */
|
| 60 |
61 |
#ifdef ESP8266
|
| 61 |
62 |
/* Allow for 1000us of clock stretching */
|
| 62 |
|
Wire.setClockStretchLimit(1000);
|
|
63 |
Wire.setClockStretchLimit(1000);
|
| 63 |
64 |
#endif
|
| 64 |
65 |
|
| 65 |
66 |
/* Make sure we have the right device */
|
| 66 |
67 |
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
|
| 67 |
68 |
if (id != BNO055_ID) {
|
| 68 |
69 |
/* hold on for boot */
|
| 69 |
|
delay(1000);
|
|
70 |
delay(1000);
|
| 70 |
71 |
id = read8(BNO055_CHIP_ID_ADDR);
|
| 71 |
72 |
if (id != BNO055_ID) {
|
| 72 |
73 |
/* still not? ok bail */
|
| ... | ... | |
| 90 |
91 |
|
| 91 |
92 |
write8(BNO055_PAGE_ID_ADDR, 0);
|
| 92 |
93 |
|
| 93 |
|
/* Set the output units
|
|
94 |
/* Set the output units
|
| 94 |
95 |
uint8_t unitsel = (0 << 7) | // Orientation = Android
|
| 95 |
96 |
(0 << 4) | // Temperature = Celsius
|
| 96 |
97 |
(0 << 2) | // Euler = Degrees
|
| ... | ... | |
| 192 |
193 |
3 = System Iniitalization
|
| 193 |
194 |
4 = Executing Self-Test
|
| 194 |
195 |
5 = Sensor fusio algorithm running
|
| 195 |
|
6 = System running without fusion algorithms
|
|
196 |
6 = System running without fusion algorithms
|
| 196 |
197 |
*/
|
| 197 |
198 |
|
| 198 |
199 |
if (system_status != 0)
|
| ... | ... | |
| 200 |
201 |
|
| 201 |
202 |
/* Self Test Results
|
| 202 |
203 |
1 = test passed, 0 = test failed
|
| 203 |
|
|
|
204 |
|
| 204 |
205 |
Bit 0 = Accelerometer self test
|
| 205 |
206 |
Bit 1 = Magnetometer self test
|
| 206 |
207 |
Bit 2 = Gyroscope self test
|
| 207 |
208 |
Bit 3 = MCU self test
|
| 208 |
|
|
| 209 |
|
0x0F = all good!
|
|
209 |
|
|
210 |
0x0F = all good!
|
| 210 |
211 |
*/
|
| 211 |
212 |
|
| 212 |
213 |
if (self_test_result != 0)
|
| ... | ... | |
| 223 |
224 |
7 = BNO low power mode not available for selected operat ion mode
|
| 224 |
225 |
8 = Accelerometer power mode not available
|
| 225 |
226 |
9 = Fusion algorithm configuration error
|
| 226 |
|
A = Sensor configuration error
|
|
227 |
A = Sensor configuration error
|
| 227 |
228 |
*/
|
| 228 |
229 |
|
| 229 |
230 |
if (system_error != 0)
|
| ... | ... | |
| 312 |
313 |
y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
|
| 313 |
314 |
z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
|
| 314 |
315 |
|
| 315 |
|
/*!
|
|
316 |
/*!
|
| 316 |
317 |
* Convert the value to an appropriate range (section 3.6.4)
|
| 317 |
|
* and assign the value to the Vector type
|
|
318 |
* and assign the value to the Vector type
|
| 318 |
319 |
*/
|
| 319 |
320 |
switch (vector_type) {
|
| 320 |
321 |
case VECTOR_MAGNETOMETER:
|
| ... | ... | |
| 370 |
371 |
* Assign to Quaternion
|
| 371 |
372 |
* See
|
| 372 |
373 |
* http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
| 373 |
|
* 3.6.5.5 Orientation (Quaternion)
|
|
374 |
* 3.6.5.5 Orientation (Quaternion)
|
| 374 |
375 |
*/
|
| 375 |
376 |
const double scale = (1.0 / (1 << 14));
|
| 376 |
377 |
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
|
| ... | ... | |
| 380 |
381 |
/*!
|
| 381 |
382 |
* @brief Provides the sensor_t data for this sensor
|
| 382 |
383 |
* @param sensor
|
| 383 |
|
*/
|
|
384 |
*/
|
| 384 |
385 |
void Adafruit_BNO055::getSensor(sensor_t *sensor) {
|
| 385 |
386 |
/* Clear the sensor_t object */
|
| 386 |
387 |
memset(sensor, 0, sizeof(sensor_t));
|
| ... | ... | |
| 499 |
500 |
}
|
| 500 |
501 |
|
| 501 |
502 |
/*!
|
| 502 |
|
* @brief Writes an array of calibration values to the sensor's offset registers
|
|
503 |
* @brief Writes an array of calibration values to the sensor's offset
|
|
504 |
* registers
|
| 503 |
505 |
* @param calibData
|
| 504 |
506 |
*/
|
| 505 |
507 |
void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) {
|
| ... | ... | |
| 546 |
548 |
/*!
|
| 547 |
549 |
* @brief Writes to the sensor's offset registers from an offset struct
|
| 548 |
550 |
* @param offsets_type
|
| 549 |
|
*/
|
|
551 |
*/
|
| 550 |
552 |
void Adafruit_BNO055::setSensorOffsets(
|
| 551 |
553 |
const adafruit_bno055_offsets_t &offsets_type) {
|
| 552 |
554 |
adafruit_bno055_opmode_t lastMode = _mode;
|