adafruit_bno055 / Adafruit_BNO055.cpp @ 60d68bed
History | View | Annotate | Download (26.248 KB)
1 | 5d8d461e | Jan Hoffmann | /*!
|
---|---|---|---|
2 | * @file Adafruit_BNO055.cpp
|
||
3 | *
|
||
4 | * @mainpage Adafruit BNO055 Orientation Sensor
|
||
5 | 75f03d2e | Szymon Kaliski | *
|
6 | 5d8d461e | Jan Hoffmann | * @section intro_sec Introduction
|
7 | *
|
||
8 | 75f03d2e | Szymon Kaliski | * This is a library for the BNO055 orientation sensor
|
9 | 5d8d461e | Jan Hoffmann | *
|
10 | 75f03d2e | Szymon Kaliski | * Designed specifically to work with the Adafruit BNO055 Breakout.
|
11 | 5d8d461e | Jan Hoffmann | *
|
12 | 75f03d2e | Szymon Kaliski | * Pick one up today in the adafruit shop!
|
13 | * ------> https://www.adafruit.com/product/2472
|
||
14 | 5d8d461e | Jan Hoffmann | *
|
15 | 75f03d2e | Szymon Kaliski | * These sensors use I2C to communicate, 2 pins are required to interface.
|
16 | 5d8d461e | Jan Hoffmann | *
|
17 | 75f03d2e | Szymon Kaliski | * Adafruit invests time and resources providing this open source code,
|
18 | * please support Adafruit andopen-source hardware by purchasing products
|
||
19 | * from Adafruit!
|
||
20 | 5d8d461e | Jan Hoffmann | *
|
21 | * @section author Author
|
||
22 | *
|
||
23 | * K.Townsend (Adafruit Industries)
|
||
24 | *
|
||
25 | 75f03d2e | Szymon Kaliski | * @section license License
|
26 | *
|
||
27 | * MIT license, all text above must be included in any redistribution
|
||
28 | 5d8d461e | Jan Hoffmann | */
|
29 | 4bc1c0c1 | Kevin Townsend | |
30 | 55e2f6d1 | Jan Hoffmann | #include "Arduino.h" |
31 | 4bc1c0c1 | Kevin Townsend | |
32 | #include <limits.h> |
||
33 | 55e2f6d1 | Jan Hoffmann | #include <math.h> |
34 | 4bc1c0c1 | Kevin Townsend | |
35 | #include "Adafruit_BNO055.h" |
||
36 | |||
37 | /*!
|
||
38 | 5d8d461e | Jan Hoffmann | * @brief Instantiates a new Adafruit_BNO055 class
|
39 | 9a8e8b26 | Jan Hoffmann | * @param sensorID
|
40 | * sensor ID
|
||
41 | * @param address
|
||
42 | * i2c address
|
||
43 | dbc34b4d | Jan Hoffmann | * @param *theWire
|
44 | * Wire object
|
||
45 | 5d8d461e | Jan Hoffmann | */
|
46 | 9a8e8b26 | Jan Hoffmann | Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address, |
47 | TwoWire *theWire) { |
||
48 | 463eabf7 | Wetmelon | _sensorID = sensorID; |
49 | _address = address; |
||
50 | 83911fa6 | Jan Hoffmann | _wire = theWire; |
51 | 4bc1c0c1 | Kevin Townsend | } |
52 | |||
53 | /*!
|
||
54 | f7556b0d | Jan Hoffmann | * @brief Sets up the HW
|
55 | * @param mode
|
||
56 | * mode values
|
||
57 | * [OPERATION_MODE_CONFIG,
|
||
58 | * OPERATION_MODE_ACCONLY,
|
||
59 | * OPERATION_MODE_MAGONLY,
|
||
60 | * OPERATION_MODE_GYRONLY,
|
||
61 | * OPERATION_MODE_ACCMAG,
|
||
62 | * OPERATION_MODE_ACCGYRO,
|
||
63 | * OPERATION_MODE_MAGGYRO,
|
||
64 | * OPERATION_MODE_AMG,
|
||
65 | * OPERATION_MODE_IMUPLUS,
|
||
66 | * OPERATION_MODE_COMPASS,
|
||
67 | * OPERATION_MODE_M4G,
|
||
68 | * OPERATION_MODE_NDOF_FMC_OFF,
|
||
69 | * OPERATION_MODE_NDOF]
|
||
70 | * @return true if process is successful
|
||
71 | */
|
||
72 | 9a8e8b26 | Jan Hoffmann | bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) {
|
73 | #if defined(ARDUINO_SAMD_ZERO) && (_address == BNO055_ADDRESS_A)
|
||
74 | f7556b0d | Jan Hoffmann | #error \
|
75 | "On an arduino Zero, BNO055's ADR pin must be high. Fix that, then delete this line."
|
||
76 | _address = BNO055_ADDRESS_B; |
||
77 | #endif
|
||
78 | |||
79 | 463eabf7 | Wetmelon | /* Enable I2C */
|
80 | f7556b0d | Jan Hoffmann | _wire->begin(); |
81 | 463eabf7 | Wetmelon | |
82 | 78cc710f | ladyada | // BNO055 clock stretches for 500us or more!
|
83 | #ifdef ESP8266
|
||
84 | 9a8e8b26 | Jan Hoffmann | _wire->setClockStretchLimit(1000); // Allow for 1000us of clock stretching |
85 | 78cc710f | ladyada | #endif
|
86 | |||
87 | 463eabf7 | Wetmelon | /* Make sure we have the right device */
|
88 | uint8_t id = read8(BNO055_CHIP_ID_ADDR); |
||
89 | 55e2f6d1 | Jan Hoffmann | if (id != BNO055_ID) {
|
90 | 463eabf7 | Wetmelon | delay(1000); // hold on for boot |
91 | id = read8(BNO055_CHIP_ID_ADDR); |
||
92 | 55e2f6d1 | Jan Hoffmann | if (id != BNO055_ID) {
|
93 | 9a8e8b26 | Jan Hoffmann | return false; // still not? ok bail |
94 | 463eabf7 | Wetmelon | } |
95 | } |
||
96 | |||
97 | /* Switch to config mode (just in case since this is the default) */
|
||
98 | setMode(OPERATION_MODE_CONFIG); |
||
99 | |||
100 | /* Reset */
|
||
101 | write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
|
||
102 | 55e2f6d1 | Jan Hoffmann | while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) {
|
103 | 463eabf7 | Wetmelon | delay(10);
|
104 | } |
||
105 | delay(50);
|
||
106 | |||
107 | /* Set to normal power mode */
|
||
108 | write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); |
||
109 | delay(10);
|
||
110 | |||
111 | write8(BNO055_PAGE_ID_ADDR, 0);
|
||
112 | |||
113 | /* Set the output units */
|
||
114 | /*
|
||
115 | uint8_t unitsel = (0 << 7) | // Orientation = Android
|
||
116 | (0 << 4) | // Temperature = Celsius
|
||
117 | (0 << 2) | // Euler = Degrees
|
||
118 | (1 << 1) | // Gyro = Rads
|
||
119 | (0 << 0); // Accelerometer = m/s^2
|
||
120 | write8(BNO055_UNIT_SEL_ADDR, unitsel);
|
||
121 | */
|
||
122 | |||
123 | 8d5b341f | Jan | // Select BNO055 gyro temperature source - REDUCES DRIFT SIGNIFICANTLY!
|
124 | write8(BNO055_TEMP_SOURCE_ADDR, 0x01);
|
||
125 | |||
126 | 378858ec | Shunya Sato | /* Configure axis mapping (see section 3.4) */
|
127 | /*
|
||
128 | write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1
|
||
129 | delay(10);
|
||
130 | write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1
|
||
131 | delay(10);
|
||
132 | */
|
||
133 | a97e0fe1 | kA®0šhî | |
134 | 463eabf7 | Wetmelon | write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
|
135 | delay(10);
|
||
136 | /* Set the requested operating mode (see section 3.3) */
|
||
137 | setMode(mode); |
||
138 | delay(20);
|
||
139 | |||
140 | return true; |
||
141 | 4bc1c0c1 | Kevin Townsend | } |
142 | |||
143 | /*!
|
||
144 | 5d8d461e | Jan Hoffmann | * @brief Puts the chip in the specified operating mode
|
145 | * @param mode
|
||
146 | f7556b0d | Jan Hoffmann | * mode values
|
147 | * [OPERATION_MODE_CONFIG,
|
||
148 | * OPERATION_MODE_ACCONLY,
|
||
149 | * OPERATION_MODE_MAGONLY,
|
||
150 | * OPERATION_MODE_GYRONLY,
|
||
151 | * OPERATION_MODE_ACCMAG,
|
||
152 | * OPERATION_MODE_ACCGYRO,
|
||
153 | * OPERATION_MODE_MAGGYRO,
|
||
154 | * OPERATION_MODE_AMG,
|
||
155 | * OPERATION_MODE_IMUPLUS,
|
||
156 | * OPERATION_MODE_COMPASS,
|
||
157 | * OPERATION_MODE_M4G,
|
||
158 | * OPERATION_MODE_NDOF_FMC_OFF,
|
||
159 | * OPERATION_MODE_NDOF]
|
||
160 | 5d8d461e | Jan Hoffmann | */
|
161 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) {
|
162 | 463eabf7 | Wetmelon | _mode = mode; |
163 | write8(BNO055_OPR_MODE_ADDR, _mode); |
||
164 | delay(30);
|
||
165 | 4bc1c0c1 | Kevin Townsend | } |
166 | |||
167 | /*!
|
||
168 | 5d8d461e | Jan Hoffmann | * @brief Changes the chip's axis remap
|
169 | * @param remapcode
|
||
170 | f7556b0d | Jan Hoffmann | * remap code possible values
|
171 | * [REMAP_CONFIG_P0
|
||
172 | * REMAP_CONFIG_P1 (default)
|
||
173 | * REMAP_CONFIG_P2
|
||
174 | * REMAP_CONFIG_P3
|
||
175 | * REMAP_CONFIG_P4
|
||
176 | * REMAP_CONFIG_P5
|
||
177 | * REMAP_CONFIG_P6
|
||
178 | * REMAP_CONFIG_P7]
|
||
179 | 5d8d461e | Jan Hoffmann | */
|
180 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::setAxisRemap(
|
181 | adafruit_bno055_axis_remap_config_t remapcode) { |
||
182 | a97e0fe1 | kA®0šhî | adafruit_bno055_opmode_t modeback = _mode; |
183 | |||
184 | setMode(OPERATION_MODE_CONFIG); |
||
185 | delay(25);
|
||
186 | write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode); |
||
187 | delay(10);
|
||
188 | /* Set the requested operating mode (see section 3.3) */
|
||
189 | setMode(modeback); |
||
190 | delay(20);
|
||
191 | } |
||
192 | |||
193 | /*!
|
||
194 | 5d8d461e | Jan Hoffmann | * @brief Changes the chip's axis signs
|
195 | * @param remapsign
|
||
196 | f7556b0d | Jan Hoffmann | * remap sign possible values
|
197 | * [REMAP_SIGN_P0
|
||
198 | * REMAP_SIGN_P1 (default)
|
||
199 | * REMAP_SIGN_P2
|
||
200 | * REMAP_SIGN_P3
|
||
201 | * REMAP_SIGN_P4
|
||
202 | * REMAP_SIGN_P5
|
||
203 | * REMAP_SIGN_P6
|
||
204 | * REMAP_SIGN_P7]
|
||
205 | 5d8d461e | Jan Hoffmann | */
|
206 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
|
207 | a97e0fe1 | kA®0šhî | adafruit_bno055_opmode_t modeback = _mode; |
208 | |||
209 | setMode(OPERATION_MODE_CONFIG); |
||
210 | delay(25);
|
||
211 | write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign); |
||
212 | delay(10);
|
||
213 | /* Set the requested operating mode (see section 3.3) */
|
||
214 | setMode(modeback); |
||
215 | delay(20);
|
||
216 | } |
||
217 | |||
218 | /*!
|
||
219 | 5d8d461e | Jan Hoffmann | * @brief Use the external 32.768KHz crystal
|
220 | f7556b0d | Jan Hoffmann | * @param usextal
|
221 | * use external crystal boolean
|
||
222 | 5d8d461e | Jan Hoffmann | */
|
223 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::setExtCrystalUse(boolean usextal) {
|
224 | 463eabf7 | Wetmelon | adafruit_bno055_opmode_t modeback = _mode; |
225 | |||
226 | /* Switch to config mode (just in case since this is the default) */
|
||
227 | setMode(OPERATION_MODE_CONFIG); |
||
228 | delay(25);
|
||
229 | write8(BNO055_PAGE_ID_ADDR, 0);
|
||
230 | if (usextal) {
|
||
231 | write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
|
||
232 | } else {
|
||
233 | write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
|
||
234 | } |
||
235 | delay(10);
|
||
236 | /* Set the requested operating mode (see section 3.3) */
|
||
237 | setMode(modeback); |
||
238 | delay(20);
|
||
239 | c4f272e1 | ladyada | } |
240 | |||
241 | /*!
|
||
242 | 5d8d461e | Jan Hoffmann | * @brief Gets the latest system status info
|
243 | * @param system_status
|
||
244 | f7556b0d | Jan Hoffmann | * system status info
|
245 | 5d8d461e | Jan Hoffmann | * @param self_test_result
|
246 | f7556b0d | Jan Hoffmann | * self test result
|
247 | 5d8d461e | Jan Hoffmann | * @param system_error
|
248 | f7556b0d | Jan Hoffmann | * system error info
|
249 | 5d8d461e | Jan Hoffmann | */
|
250 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::getSystemStatus(uint8_t *system_status,
|
251 | uint8_t *self_test_result, |
||
252 | uint8_t *system_error) { |
||
253 | 463eabf7 | Wetmelon | write8(BNO055_PAGE_ID_ADDR, 0);
|
254 | |||
255 | /* System Status (see section 4.3.58)
|
||
256 | 0 = Idle
|
||
257 | 1 = System Error
|
||
258 | 2 = Initializing Peripherals
|
||
259 | 3 = System Iniitalization
|
||
260 | 4 = Executing Self-Test
|
||
261 | 5 = Sensor fusio algorithm running
|
||
262 | 75f03d2e | Szymon Kaliski | 6 = System running without fusion algorithms
|
263 | 5d8d461e | Jan Hoffmann | */
|
264 | 463eabf7 | Wetmelon | |
265 | if (system_status != 0) |
||
266 | 55e2f6d1 | Jan Hoffmann | *system_status = read8(BNO055_SYS_STAT_ADDR); |
267 | 463eabf7 | Wetmelon | |
268 | 5d8d461e | Jan Hoffmann | /* Self Test Results
|
269 | 463eabf7 | Wetmelon | 1 = test passed, 0 = test failed
|
270 | |||
271 | Bit 0 = Accelerometer self test
|
||
272 | Bit 1 = Magnetometer self test
|
||
273 | Bit 2 = Gyroscope self test
|
||
274 | Bit 3 = MCU self test
|
||
275 | |||
276 | 75f03d2e | Szymon Kaliski | 0x0F = all good!
|
277 | 5d8d461e | Jan Hoffmann | */
|
278 | 463eabf7 | Wetmelon | |
279 | if (self_test_result != 0) |
||
280 | *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR); |
||
281 | |||
282 | /* System Error (see section 4.3.59)
|
||
283 | 0 = No error
|
||
284 | 1 = Peripheral initialization error
|
||
285 | 2 = System initialization error
|
||
286 | 3 = Self test result failed
|
||
287 | 4 = Register map value out of range
|
||
288 | 5 = Register map address out of range
|
||
289 | 6 = Register map write error
|
||
290 | 7 = BNO low power mode not available for selected operat ion mode
|
||
291 | 8 = Accelerometer power mode not available
|
||
292 | 9 = Fusion algorithm configuration error
|
||
293 | 75f03d2e | Szymon Kaliski | A = Sensor configuration error
|
294 | 5d8d461e | Jan Hoffmann | */
|
295 | 463eabf7 | Wetmelon | |
296 | if (system_error != 0) |
||
297 | 55e2f6d1 | Jan Hoffmann | *system_error = read8(BNO055_SYS_ERR_ADDR); |
298 | 463eabf7 | Wetmelon | |
299 | delay(200);
|
||
300 | 4bc1c0c1 | Kevin Townsend | } |
301 | |||
302 | /*!
|
||
303 | 5d8d461e | Jan Hoffmann | * @brief Gets the chip revision numbers
|
304 | f7556b0d | Jan Hoffmann | * @param info
|
305 | * revision info
|
||
306 | 5d8d461e | Jan Hoffmann | */
|
307 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) {
|
308 | 463eabf7 | Wetmelon | uint8_t a, b; |
309 | 4bc1c0c1 | Kevin Townsend | |
310 | 463eabf7 | Wetmelon | memset(info, 0, sizeof(adafruit_bno055_rev_info_t)); |
311 | 4bc1c0c1 | Kevin Townsend | |
312 | 463eabf7 | Wetmelon | /* Check the accelerometer revision */
|
313 | info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR); |
||
314 | 67f3cff5 | Kevin Townsend | |
315 | 463eabf7 | Wetmelon | /* Check the magnetometer revision */
|
316 | 55e2f6d1 | Jan Hoffmann | info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR); |
317 | 67f3cff5 | Kevin Townsend | |
318 | 463eabf7 | Wetmelon | /* Check the gyroscope revision */
|
319 | 55e2f6d1 | Jan Hoffmann | info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR); |
320 | 67f3cff5 | Kevin Townsend | |
321 | 463eabf7 | Wetmelon | /* Check the SW revision */
|
322 | 55e2f6d1 | Jan Hoffmann | info->bl_rev = read8(BNO055_BL_REV_ID_ADDR); |
323 | 40f91f6f | Tony DiCola | |
324 | 463eabf7 | Wetmelon | a = read8(BNO055_SW_REV_ID_LSB_ADDR); |
325 | b = read8(BNO055_SW_REV_ID_MSB_ADDR); |
||
326 | info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
|
||
327 | 4bc1c0c1 | Kevin Townsend | } |
328 | |||
329 | /*!
|
||
330 | 5d8d461e | Jan Hoffmann | * @brief Gets current calibration state. Each value should be a uint8_t
|
331 | * pointer and it will be set to 0 if not calibrated and 3 if
|
||
332 | * fully calibrated.
|
||
333 | f7556b0d | Jan Hoffmann | * See section 34.3.54
|
334 | 5d8d461e | Jan Hoffmann | * @param sys
|
335 | f7556b0d | Jan Hoffmann | * Current system calibration status, depends on status of all sensors,
|
336 | * read-only
|
||
337 | 5d8d461e | Jan Hoffmann | * @param gyro
|
338 | f7556b0d | Jan Hoffmann | * Current calibration status of Gyroscope, read-only
|
339 | 5d8d461e | Jan Hoffmann | * @param accel
|
340 | f7556b0d | Jan Hoffmann | * Current calibration status of Accelerometer, read-only
|
341 | 5d8d461e | Jan Hoffmann | * @param mag
|
342 | f7556b0d | Jan Hoffmann | * Current calibration status of Magnetometer, read-only
|
343 | 5d8d461e | Jan Hoffmann | */
|
344 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro,
|
345 | uint8_t *accel, uint8_t *mag) { |
||
346 | 463eabf7 | Wetmelon | uint8_t calData = read8(BNO055_CALIB_STAT_ADDR); |
347 | if (sys != NULL) { |
||
348 | *sys = (calData >> 6) & 0x03; |
||
349 | } |
||
350 | if (gyro != NULL) { |
||
351 | *gyro = (calData >> 4) & 0x03; |
||
352 | } |
||
353 | if (accel != NULL) { |
||
354 | *accel = (calData >> 2) & 0x03; |
||
355 | } |
||
356 | if (mag != NULL) { |
||
357 | *mag = calData & 0x03;
|
||
358 | } |
||
359 | 40f91f6f | Tony DiCola | } |
360 | |||
361 | /*!
|
||
362 | 5d8d461e | Jan Hoffmann | * @brief Gets the temperature in degrees celsius
|
363 | * @return temperature in degrees celsius
|
||
364 | */
|
||
365 | 55e2f6d1 | Jan Hoffmann | int8_t Adafruit_BNO055::getTemp() { |
366 | 463eabf7 | Wetmelon | int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR)); |
367 | return temp;
|
||
368 | 0e2e2723 | Kevin Townsend | } |
369 | |||
370 | /*!
|
||
371 | 5d8d461e | Jan Hoffmann | * @brief Gets a vector reading from the specified source
|
372 | * @param vector_type
|
||
373 | f7556b0d | Jan Hoffmann | * possible vector type values
|
374 | * [VECTOR_ACCELEROMETER
|
||
375 | * VECTOR_MAGNETOMETER
|
||
376 | * VECTOR_GYROSCOPE
|
||
377 | * VECTOR_EULER
|
||
378 | * VECTOR_LINEARACCEL
|
||
379 | * VECTOR_GRAVITY]
|
||
380 | 5d8d461e | Jan Hoffmann | * @return vector from specified source
|
381 | */
|
||
382 | 55e2f6d1 | Jan Hoffmann | imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) {
|
383 | 463eabf7 | Wetmelon | imu::Vector<3> xyz;
|
384 | uint8_t buffer[6];
|
||
385 | 55e2f6d1 | Jan Hoffmann | memset(buffer, 0, 6); |
386 | 463eabf7 | Wetmelon | |
387 | int16_t x, y, z; |
||
388 | x = y = z = 0;
|
||
389 | |||
390 | /* Read vector data (6 bytes) */
|
||
391 | readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
|
||
392 | |||
393 | x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8); |
||
394 | y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8); |
||
395 | z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8); |
||
396 | |||
397 | 75f03d2e | Szymon Kaliski | /*!
|
398 | 5d8d461e | Jan Hoffmann | * Convert the value to an appropriate range (section 3.6.4)
|
399 | 75f03d2e | Szymon Kaliski | * and assign the value to the Vector type
|
400 | 5d8d461e | Jan Hoffmann | */
|
401 | 55e2f6d1 | Jan Hoffmann | switch (vector_type) {
|
402 | case VECTOR_MAGNETOMETER:
|
||
403 | /* 1uT = 16 LSB */
|
||
404 | xyz[0] = ((double)x) / 16.0; |
||
405 | xyz[1] = ((double)y) / 16.0; |
||
406 | xyz[2] = ((double)z) / 16.0; |
||
407 | break;
|
||
408 | case VECTOR_GYROSCOPE:
|
||
409 | /* 1dps = 16 LSB */
|
||
410 | xyz[0] = ((double)x) / 16.0; |
||
411 | xyz[1] = ((double)y) / 16.0; |
||
412 | xyz[2] = ((double)z) / 16.0; |
||
413 | break;
|
||
414 | case VECTOR_EULER:
|
||
415 | /* 1 degree = 16 LSB */
|
||
416 | xyz[0] = ((double)x) / 16.0; |
||
417 | xyz[1] = ((double)y) / 16.0; |
||
418 | xyz[2] = ((double)z) / 16.0; |
||
419 | break;
|
||
420 | case VECTOR_ACCELEROMETER:
|
||
421 | e223568a | Jan Hoffmann | /* 1m/s^2 = 100 LSB */
|
422 | xyz[0] = ((double)x) / 100.0; |
||
423 | xyz[1] = ((double)y) / 100.0; |
||
424 | xyz[2] = ((double)z) / 100.0; |
||
425 | break;
|
||
426 | 55e2f6d1 | Jan Hoffmann | case VECTOR_LINEARACCEL:
|
427 | e223568a | Jan Hoffmann | /* 1m/s^2 = 100 LSB */
|
428 | xyz[0] = ((double)x) / 100.0; |
||
429 | xyz[1] = ((double)y) / 100.0; |
||
430 | xyz[2] = ((double)z) / 100.0; |
||
431 | break;
|
||
432 | 55e2f6d1 | Jan Hoffmann | case VECTOR_GRAVITY:
|
433 | /* 1m/s^2 = 100 LSB */
|
||
434 | xyz[0] = ((double)x) / 100.0; |
||
435 | xyz[1] = ((double)y) / 100.0; |
||
436 | xyz[2] = ((double)z) / 100.0; |
||
437 | break;
|
||
438 | 463eabf7 | Wetmelon | } |
439 | |||
440 | return xyz;
|
||
441 | 4bc1c0c1 | Kevin Townsend | } |
442 | |||
443 | /*!
|
||
444 | 5d8d461e | Jan Hoffmann | * @brief Gets a quaternion reading from the specified source
|
445 | * @return quaternion reading
|
||
446 | */
|
||
447 | 55e2f6d1 | Jan Hoffmann | imu::Quaternion Adafruit_BNO055::getQuat() { |
448 | 463eabf7 | Wetmelon | uint8_t buffer[8];
|
449 | 55e2f6d1 | Jan Hoffmann | memset(buffer, 0, 8); |
450 | 463eabf7 | Wetmelon | |
451 | int16_t x, y, z, w; |
||
452 | x = y = z = w = 0;
|
||
453 | |||
454 | /* Read quat data (8 bytes) */
|
||
455 | readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
|
||
456 | w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); |
||
457 | x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); |
||
458 | y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); |
||
459 | z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); |
||
460 | |||
461 | 5d8d461e | Jan Hoffmann | /*!
|
462 | * Assign to Quaternion
|
||
463 | * See
|
||
464 | * http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
||
465 | 75f03d2e | Szymon Kaliski | * 3.6.5.5 Orientation (Quaternion)
|
466 | 5d8d461e | Jan Hoffmann | */
|
467 | 55e2f6d1 | Jan Hoffmann | const double scale = (1.0 / (1 << 14)); |
468 | 463eabf7 | Wetmelon | imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z); |
469 | return quat;
|
||
470 | 48741e1f | Kevin Townsend | } |
471 | |||
472 | /*!
|
||
473 | 5d8d461e | Jan Hoffmann | * @brief Provides the sensor_t data for this sensor
|
474 | * @param sensor
|
||
475 | 75f03d2e | Szymon Kaliski | */
|
476 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::getSensor(sensor_t *sensor) {
|
477 | 463eabf7 | Wetmelon | /* Clear the sensor_t object */
|
478 | memset(sensor, 0, sizeof(sensor_t)); |
||
479 | |||
480 | /* Insert the sensor name in the fixed length char array */
|
||
481 | 55e2f6d1 | Jan Hoffmann | strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1); |
482 | sensor->name[sizeof(sensor->name) - 1] = 0; |
||
483 | sensor->version = 1;
|
||
484 | sensor->sensor_id = _sensorID; |
||
485 | sensor->type = SENSOR_TYPE_ORIENTATION; |
||
486 | sensor->min_delay = 0;
|
||
487 | sensor->max_value = 0.0F; |
||
488 | sensor->min_value = 0.0F; |
||
489 | sensor->resolution = 0.01F; |
||
490 | 4bc1c0c1 | Kevin Townsend | } |
491 | |||
492 | /*!
|
||
493 | 5d8d461e | Jan Hoffmann | * @brief Reads the sensor and returns the data as a sensors_event_t
|
494 | * @param event
|
||
495 | * @return always returns true
|
||
496 | */
|
||
497 | 55e2f6d1 | Jan Hoffmann | bool Adafruit_BNO055::getEvent(sensors_event_t *event) {
|
498 | 463eabf7 | Wetmelon | /* Clear the event */
|
499 | memset(event, 0, sizeof(sensors_event_t)); |
||
500 | 4bc1c0c1 | Kevin Townsend | |
501 | 55e2f6d1 | Jan Hoffmann | event->version = sizeof(sensors_event_t);
|
502 | 463eabf7 | Wetmelon | event->sensor_id = _sensorID; |
503 | 55e2f6d1 | Jan Hoffmann | event->type = SENSOR_TYPE_ORIENTATION; |
504 | 463eabf7 | Wetmelon | event->timestamp = millis(); |
505 | fcd68623 | Kevin Townsend | |
506 | 463eabf7 | Wetmelon | /* Get a Euler angle sample for orientation */
|
507 | imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
|
||
508 | event->orientation.x = euler.x(); |
||
509 | event->orientation.y = euler.y(); |
||
510 | event->orientation.z = euler.z(); |
||
511 | 312a5b9e | Wetmelon | |
512 | 463eabf7 | Wetmelon | return true; |
513 | 312a5b9e | Wetmelon | } |
514 | fcd68623 | Kevin Townsend | |
515 | 312a5b9e | Wetmelon | /*!
|
516 | f0ebdf46 | Jan Hoffmann | * @brief Reads the sensor and returns the data as a sensors_event_t
|
517 | * @param event
|
||
518 | * @param vec_type
|
||
519 | * specify the type of reading
|
||
520 | * @return always returns true
|
||
521 | */
|
||
522 | bool Adafruit_BNO055::getEvent(sensors_event_t *event, adafruit_vector_type_t vec_type)
|
||
523 | 312a5b9e | Wetmelon | { |
524 | f0ebdf46 | Jan Hoffmann | /* Clear the event */
|
525 | memset(event, 0, sizeof(sensors_event_t)); |
||
526 | 312a5b9e | Wetmelon | |
527 | f0ebdf46 | Jan Hoffmann | event->version = sizeof(sensors_event_t);
|
528 | event->sensor_id = _sensorID; |
||
529 | event->timestamp = millis(); |
||
530 | 312a5b9e | Wetmelon | |
531 | f0ebdf46 | Jan Hoffmann | //read the data according to vec_type
|
532 | imu::Vector<3> vec;
|
||
533 | if (vec_type == Adafruit_BNO055::VECTOR_LINEARACCEL)
|
||
534 | { |
||
535 | event->type = SENSOR_TYPE_ACCELEROMETER; |
||
536 | vec = getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); |
||
537 | 312a5b9e | Wetmelon | |
538 | f0ebdf46 | Jan Hoffmann | event->acceleration.x = vec.x(); |
539 | event->acceleration.y = vec.y(); |
||
540 | event->acceleration.z = vec.z(); |
||
541 | } |
||
542 | else if (vec_type == Adafruit_BNO055::VECTOR_ACCELEROMETER) |
||
543 | { |
||
544 | event->type = SENSOR_TYPE_ACCELEROMETER; |
||
545 | vec = getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); |
||
546 | |||
547 | event->acceleration.x = vec.x(); |
||
548 | event->acceleration.y = vec.y(); |
||
549 | event->acceleration.z = vec.z(); |
||
550 | } |
||
551 | else if (vec_type == Adafruit_BNO055::VECTOR_GRAVITY) |
||
552 | { |
||
553 | event->type = SENSOR_TYPE_ACCELEROMETER; |
||
554 | vec = getVector(Adafruit_BNO055::VECTOR_GRAVITY); |
||
555 | |||
556 | event->acceleration.x = vec.x(); |
||
557 | event->acceleration.y = vec.y(); |
||
558 | event->acceleration.z = vec.z(); |
||
559 | } |
||
560 | else if (vec_type == Adafruit_BNO055::VECTOR_EULER) |
||
561 | { |
||
562 | event->type = SENSOR_TYPE_ORIENTATION; |
||
563 | vec = getVector(Adafruit_BNO055::VECTOR_EULER); |
||
564 | |||
565 | event->orientation.x = vec.x(); |
||
566 | event->orientation.y = vec.y(); |
||
567 | event->orientation.z = vec.z(); |
||
568 | } |
||
569 | else if (vec_type == Adafruit_BNO055::VECTOR_GYROSCOPE) |
||
570 | { |
||
571 | event->type = SENSOR_TYPE_ROTATION_VECTOR; |
||
572 | vec = getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); |
||
573 | |||
574 | event->gyro.x = vec.x(); |
||
575 | event->gyro.y = vec.y(); |
||
576 | event->gyro.z = vec.z(); |
||
577 | } |
||
578 | else if (vec_type == Adafruit_BNO055::VECTOR_MAGNETOMETER) |
||
579 | { |
||
580 | event->type = SENSOR_TYPE_MAGNETIC_FIELD; |
||
581 | vec = getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); |
||
582 | |||
583 | event->magnetic.x = vec.x(); |
||
584 | event->magnetic.y = vec.y(); |
||
585 | event->magnetic.z = vec.z(); |
||
586 | } |
||
587 | |||
588 | |||
589 | return true; |
||
590 | 312a5b9e | Wetmelon | } |
591 | |||
592 | |||
593 | /*!
|
||
594 | 5d8d461e | Jan Hoffmann | * @brief Reads the sensor's offset registers into a byte array
|
595 | * @param calibData
|
||
596 | * @return true if read is successful
|
||
597 | */
|
||
598 | 55e2f6d1 | Jan Hoffmann | bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) {
|
599 | if (isFullyCalibrated()) {
|
||
600 | 463eabf7 | Wetmelon | adafruit_bno055_opmode_t lastMode = _mode; |
601 | setMode(OPERATION_MODE_CONFIG); |
||
602 | |||
603 | 55e2f6d1 | Jan Hoffmann | readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS); |
604 | 463eabf7 | Wetmelon | |
605 | setMode(lastMode); |
||
606 | 55e2f6d1 | Jan Hoffmann | return true; |
607 | } |
||
608 | return false; |
||
609 | 4bc1c0c1 | Kevin Townsend | } |
610 | |||
611 | 312a5b9e | Wetmelon | /*!
|
612 | 5d8d461e | Jan Hoffmann | * @brief Reads the sensor's offset registers into an offset struct
|
613 | * @param offsets_type
|
||
614 | dbc34b4d | Jan Hoffmann | * type of offsets
|
615 | 5d8d461e | Jan Hoffmann | * @return true if read is successful
|
616 | */
|
||
617 | 55e2f6d1 | Jan Hoffmann | bool Adafruit_BNO055::getSensorOffsets(
|
618 | adafruit_bno055_offsets_t &offsets_type) { |
||
619 | if (isFullyCalibrated()) {
|
||
620 | 463eabf7 | Wetmelon | adafruit_bno055_opmode_t lastMode = _mode; |
621 | setMode(OPERATION_MODE_CONFIG); |
||
622 | delay(25);
|
||
623 | |||
624 | 55e2f6d1 | Jan Hoffmann | /* Accel offset range depends on the G-range:
|
625 | +/-2g = +/- 2000 mg
|
||
626 | +/-4g = +/- 4000 mg
|
||
627 | +/-8g = +/- 8000 mg
|
||
628 | +/-1§g = +/- 16000 mg */
|
||
629 | offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) |
|
||
630 | (read8(ACCEL_OFFSET_X_LSB_ADDR)); |
||
631 | offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) |
|
||
632 | (read8(ACCEL_OFFSET_Y_LSB_ADDR)); |
||
633 | offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) |
|
||
634 | (read8(ACCEL_OFFSET_Z_LSB_ADDR)); |
||
635 | |||
636 | /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
|
||
637 | offsets_type.mag_offset_x = |
||
638 | (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
|
||
639 | offsets_type.mag_offset_y = |
||
640 | (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
|
||
641 | offsets_type.mag_offset_z = |
||
642 | (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
|
||
643 | |||
644 | /* Gyro offset range depends on the DPS range:
|
||
645 | 2000 dps = +/- 32000 LSB
|
||
646 | 1000 dps = +/- 16000 LSB
|
||
647 | 500 dps = +/- 8000 LSB
|
||
648 | 250 dps = +/- 4000 LSB
|
||
649 | 125 dps = +/- 2000 LSB
|
||
650 | ... where 1 DPS = 16 LSB */
|
||
651 | offsets_type.gyro_offset_x = |
||
652 | (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
|
||
653 | offsets_type.gyro_offset_y = |
||
654 | (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
|
||
655 | offsets_type.gyro_offset_z = |
||
656 | (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
|
||
657 | |||
658 | /* Accelerometer radius = +/- 1000 LSB */
|
||
659 | offsets_type.accel_radius = |
||
660 | (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
|
||
661 | |||
662 | /* Magnetometer radius = +/- 960 LSB */
|
||
663 | offsets_type.mag_radius = |
||
664 | (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
|
||
665 | 463eabf7 | Wetmelon | |
666 | setMode(lastMode); |
||
667 | 55e2f6d1 | Jan Hoffmann | return true; |
668 | } |
||
669 | return false; |
||
670 | 312a5b9e | Wetmelon | } |
671 | |||
672 | 741a95a7 | Kevin Townsend | /*!
|
673 | 75f03d2e | Szymon Kaliski | * @brief Writes an array of calibration values to the sensor's offset
|
674 | dbc34b4d | Jan Hoffmann | * @param *calibData
|
675 | f7556b0d | Jan Hoffmann | * calibration data
|
676 | 5d8d461e | Jan Hoffmann | */
|
677 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) { |
678 | adafruit_bno055_opmode_t lastMode = _mode; |
||
679 | setMode(OPERATION_MODE_CONFIG); |
||
680 | delay(25);
|
||
681 | 463eabf7 | Wetmelon | |
682 | 55e2f6d1 | Jan Hoffmann | /* Note: Configuration will take place only when user writes to the last
|
683 | byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
|
||
684 | Therefore the last byte must be written whenever the user wants to
|
||
685 | changes the configuration. */
|
||
686 | |||
687 | /* A writeLen() would make this much cleaner */
|
||
688 | write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
|
||
689 | write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
|
||
690 | write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
|
||
691 | write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
|
||
692 | write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
|
||
693 | write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
|
||
694 | |||
695 | write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
|
||
696 | write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
|
||
697 | write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
|
||
698 | write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
|
||
699 | write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
|
||
700 | write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
|
||
701 | |||
702 | write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
|
||
703 | write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
|
||
704 | write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
|
||
705 | write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
|
||
706 | write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
|
||
707 | write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
|
||
708 | |||
709 | write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
|
||
710 | write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
|
||
711 | |||
712 | write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
|
||
713 | write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
|
||
714 | |||
715 | setMode(lastMode); |
||
716 | 312a5b9e | Wetmelon | } |
717 | |||
718 | /*!
|
||
719 | 5d8d461e | Jan Hoffmann | * @brief Writes to the sensor's offset registers from an offset struct
|
720 | * @param offsets_type
|
||
721 | f7556b0d | Jan Hoffmann | * accel_offset_x = acceleration offset x
|
722 | * accel_offset_y = acceleration offset y
|
||
723 | * accel_offset_z = acceleration offset z
|
||
724 | *
|
||
725 | * mag_offset_x = magnetometer offset x
|
||
726 | * mag_offset_y = magnetometer offset y
|
||
727 | * mag_offset_z = magnetometer offset z
|
||
728 | *
|
||
729 | * gyro_offset_x = gyroscrope offset x
|
||
730 | * gyro_offset_y = gyroscrope offset y
|
||
731 | * gyro_offset_z = gyroscrope offset z
|
||
732 | 75f03d2e | Szymon Kaliski | */
|
733 | 55e2f6d1 | Jan Hoffmann | void Adafruit_BNO055::setSensorOffsets(
|
734 | const adafruit_bno055_offsets_t &offsets_type) {
|
||
735 | adafruit_bno055_opmode_t lastMode = _mode; |
||
736 | setMode(OPERATION_MODE_CONFIG); |
||
737 | delay(25);
|
||
738 | 312a5b9e | Wetmelon | |
739 | 55e2f6d1 | Jan Hoffmann | /* Note: Configuration will take place only when user writes to the last
|
740 | byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
|
||
741 | Therefore the last byte must be written whenever the user wants to
|
||
742 | changes the configuration. */
|
||
743 | |||
744 | write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
|
||
745 | write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); |
||
746 | write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
|
||
747 | write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); |
||
748 | write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
|
||
749 | write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); |
||
750 | |||
751 | write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
|
||
752 | write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); |
||
753 | write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
|
||
754 | write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); |
||
755 | write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
|
||
756 | write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); |
||
757 | |||
758 | write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
|
||
759 | write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); |
||
760 | write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
|
||
761 | write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); |
||
762 | write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
|
||
763 | write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); |
||
764 | |||
765 | write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
|
||
766 | write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); |
||
767 | |||
768 | write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
|
||
769 | write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); |
||
770 | |||
771 | setMode(lastMode); |
||
772 | 312a5b9e | Wetmelon | } |
773 | 4bc1c0c1 | Kevin Townsend | |
774 | /*!
|
||
775 | 5d8d461e | Jan Hoffmann | * @brief Checks of all cal status values are set to 3 (fully calibrated)
|
776 | * @return status of calibration
|
||
777 | */
|
||
778 | 55e2f6d1 | Jan Hoffmann | bool Adafruit_BNO055::isFullyCalibrated() {
|
779 | uint8_t system, gyro, accel, mag; |
||
780 | getCalibration(&system, &gyro, &accel, &mag); |
||
781 | 510d0f10 | Jan Hoffmann | |
782 | switch (_mode) {
|
||
783 | case OPERATION_MODE_ACCONLY:
|
||
784 | return (accel == 3); |
||
785 | case OPERATION_MODE_MAGONLY:
|
||
786 | return (mag == 3); |
||
787 | case OPERATION_MODE_GYRONLY:
|
||
788 | case OPERATION_MODE_M4G: /* No magnetometer calibration required. */ |
||
789 | return (gyro == 3); |
||
790 | case OPERATION_MODE_ACCMAG:
|
||
791 | case OPERATION_MODE_COMPASS:
|
||
792 | return (accel == 3 && mag == 3); |
||
793 | case OPERATION_MODE_ACCGYRO:
|
||
794 | case OPERATION_MODE_IMUPLUS:
|
||
795 | return (accel == 3 && gyro == 3); |
||
796 | case OPERATION_MODE_MAGGYRO:
|
||
797 | return (mag == 3 && gyro == 3); |
||
798 | default:
|
||
799 | return (system == 3 && gyro == 3 && accel == 3 && mag == 3); |
||
800 | } |
||
801 | 312a5b9e | Wetmelon | } |
802 | |||
803 | 4bc1c0c1 | Kevin Townsend | /*!
|
804 | a7b2a1c9 | Jan Hoffmann | * @brief Enter Suspend mode (i.e., sleep)
|
805 | */
|
||
806 | void Adafruit_BNO055::enterSuspendMode() {
|
||
807 | adafruit_bno055_opmode_t modeback = _mode; |
||
808 | |||
809 | /* Switch to config mode (just in case since this is the default) */
|
||
810 | setMode(OPERATION_MODE_CONFIG); |
||
811 | delay(25);
|
||
812 | write8(BNO055_PWR_MODE_ADDR, 0x02);
|
||
813 | /* Set the requested operating mode (see section 3.3) */
|
||
814 | setMode(modeback); |
||
815 | delay(20);
|
||
816 | } |
||
817 | |||
818 | /*!
|
||
819 | * @brief Enter Normal mode (i.e., wake)
|
||
820 | */
|
||
821 | void Adafruit_BNO055::enterNormalMode() {
|
||
822 | adafruit_bno055_opmode_t modeback = _mode; |
||
823 | |||
824 | /* Switch to config mode (just in case since this is the default) */
|
||
825 | setMode(OPERATION_MODE_CONFIG); |
||
826 | delay(25);
|
||
827 | write8(BNO055_PWR_MODE_ADDR, 0x00);
|
||
828 | /* Set the requested operating mode (see section 3.3) */
|
||
829 | setMode(modeback); |
||
830 | delay(20);
|
||
831 | } |
||
832 | |||
833 | /*!
|
||
834 | 5d8d461e | Jan Hoffmann | * @brief Writes an 8 bit value over I2C
|
835 | */
|
||
836 | 55e2f6d1 | Jan Hoffmann | bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) {
|
837 | f7556b0d | Jan Hoffmann | _wire->beginTransmission(_address); |
838 | 55e2f6d1 | Jan Hoffmann | #if ARDUINO >= 100 |
839 | f7556b0d | Jan Hoffmann | _wire->write((uint8_t)reg); |
840 | _wire->write((uint8_t)value); |
||
841 | 55e2f6d1 | Jan Hoffmann | #else
|
842 | f7556b0d | Jan Hoffmann | _wire->send(reg); |
843 | _wire->send(value); |
||
844 | 55e2f6d1 | Jan Hoffmann | #endif
|
845 | f7556b0d | Jan Hoffmann | _wire->endTransmission(); |
846 | 463eabf7 | Wetmelon | |
847 | /* ToDo: Check for error! */
|
||
848 | return true; |
||
849 | 4bc1c0c1 | Kevin Townsend | } |
850 | |||
851 | /*!
|
||
852 | 5d8d461e | Jan Hoffmann | * @brief Reads an 8 bit value over I2C
|
853 | */
|
||
854 | 55e2f6d1 | Jan Hoffmann | byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) { |
855 | 463eabf7 | Wetmelon | byte value = 0;
|
856 | |||
857 | f7556b0d | Jan Hoffmann | _wire->beginTransmission(_address); |
858 | 55e2f6d1 | Jan Hoffmann | #if ARDUINO >= 100 |
859 | f7556b0d | Jan Hoffmann | _wire->write((uint8_t)reg); |
860 | 55e2f6d1 | Jan Hoffmann | #else
|
861 | f7556b0d | Jan Hoffmann | _wire->send(reg); |
862 | 55e2f6d1 | Jan Hoffmann | #endif
|
863 | f7556b0d | Jan Hoffmann | _wire->endTransmission(); |
864 | _wire->requestFrom(_address, (byte)1);
|
||
865 | 55e2f6d1 | Jan Hoffmann | #if ARDUINO >= 100 |
866 | f7556b0d | Jan Hoffmann | value = _wire->read(); |
867 | 55e2f6d1 | Jan Hoffmann | #else
|
868 | f7556b0d | Jan Hoffmann | value = _wire->receive(); |
869 | 55e2f6d1 | Jan Hoffmann | #endif
|
870 | 463eabf7 | Wetmelon | |
871 | return value;
|
||
872 | 4bc1c0c1 | Kevin Townsend | } |
873 | |||
874 | /*!
|
||
875 | 5d8d461e | Jan Hoffmann | * @brief Reads the specified number of bytes over I2C
|
876 | */
|
||
877 | 55e2f6d1 | Jan Hoffmann | bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte *buffer,
|
878 | uint8_t len) { |
||
879 | f7556b0d | Jan Hoffmann | _wire->beginTransmission(_address); |
880 | 55e2f6d1 | Jan Hoffmann | #if ARDUINO >= 100 |
881 | f7556b0d | Jan Hoffmann | _wire->write((uint8_t)reg); |
882 | 55e2f6d1 | Jan Hoffmann | #else
|
883 | f7556b0d | Jan Hoffmann | _wire->send(reg); |
884 | 55e2f6d1 | Jan Hoffmann | #endif
|
885 | f7556b0d | Jan Hoffmann | _wire->endTransmission(); |
886 | _wire->requestFrom(_address, (byte)len); |
||
887 | 463eabf7 | Wetmelon | |
888 | 55e2f6d1 | Jan Hoffmann | for (uint8_t i = 0; i < len; i++) { |
889 | #if ARDUINO >= 100 |
||
890 | f7556b0d | Jan Hoffmann | buffer[i] = _wire->read(); |
891 | 55e2f6d1 | Jan Hoffmann | #else
|
892 | f7556b0d | Jan Hoffmann | buffer[i] = _wire->receive(); |
893 | 55e2f6d1 | Jan Hoffmann | #endif
|
894 | 463eabf7 | Wetmelon | } |
895 | |||
896 | 60d68bed | Jan | /*
|
897 | * @brief Explicit reset for the chip
|
||
898 | */
|
||
899 | bool Adafruit_BNO055::reset() {
|
||
900 | _wire->beginTransmission(_address); |
||
901 | setMode(OPERATION_MODE_CONFIG); |
||
902 | |||
903 | /* Reset */
|
||
904 | write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
|
||
905 | while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) {
|
||
906 | delay(10);
|
||
907 | } |
||
908 | delay(50);
|
||
909 | } |
||
910 | |||
911 | 463eabf7 | Wetmelon | /* ToDo: Check for errors! */
|
912 | return true; |
||
913 | } |