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