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