adafruit_bno055 / Adafruit_BNO055.cpp @ a3aea32b
History | View | Annotate | Download (20.885 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 | ------> http://www.adafruit.com/products
|
||
8 | |||
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 | 463eabf7 | Wetmelon | #include "Arduino.h" |
22 | 4bc1c0c1 | Kevin Townsend | #else
|
23 | 463eabf7 | Wetmelon | #include "WProgram.h" |
24 | 4bc1c0c1 | Kevin Townsend | #endif
|
25 | |||
26 | #include <math.h> |
||
27 | #include <limits.h> |
||
28 | |||
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 | */
|
||
39 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
40 | Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) |
||
41 | { |
||
42 | 463eabf7 | Wetmelon | _sensorID = sensorID; |
43 | _address = address; |
||
44 | 4bc1c0c1 | Kevin Townsend | } |
45 | |||
46 | /***************************************************************************
|
||
47 | PUBLIC FUNCTIONS
|
||
48 | ***************************************************************************/
|
||
49 | |||
50 | /**************************************************************************/
|
||
51 | /*!
|
||
52 | 463eabf7 | Wetmelon | @brief Sets up the HW
|
53 | */
|
||
54 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
55 | bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
|
||
56 | { |
||
57 | 463eabf7 | Wetmelon | /* Enable I2C */
|
58 | Wire.begin(); |
||
59 | |||
60 | 78cc710f | ladyada | // BNO055 clock stretches for 500us or more!
|
61 | #ifdef ESP8266
|
||
62 | Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching |
||
63 | #endif
|
||
64 | |||
65 | 463eabf7 | Wetmelon | /* Make sure we have the right device */
|
66 | uint8_t id = read8(BNO055_CHIP_ID_ADDR); |
||
67 | if(id != BNO055_ID)
|
||
68 | { |
||
69 | delay(1000); // hold on for boot |
||
70 | id = read8(BNO055_CHIP_ID_ADDR); |
||
71 | if(id != BNO055_ID) {
|
||
72 | return false; // still not? ok bail |
||
73 | } |
||
74 | } |
||
75 | |||
76 | /* Switch to config mode (just in case since this is the default) */
|
||
77 | setMode(OPERATION_MODE_CONFIG); |
||
78 | |||
79 | /* Reset */
|
||
80 | write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
|
||
81 | while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
|
||
82 | { |
||
83 | 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 | */
|
||
124 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
125 | void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
|
||
126 | { |
||
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 | */
|
||
136 | /**************************************************************************/
|
||
137 | void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode )
|
||
138 | { |
||
139 | adafruit_bno055_opmode_t modeback = _mode; |
||
140 | |||
141 | setMode(OPERATION_MODE_CONFIG); |
||
142 | delay(25);
|
||
143 | write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode); |
||
144 | delay(10);
|
||
145 | /* Set the requested operating mode (see section 3.3) */
|
||
146 | setMode(modeback); |
||
147 | delay(20);
|
||
148 | } |
||
149 | |||
150 | /**************************************************************************/
|
||
151 | /*!
|
||
152 | @brief Changes the chip's axis signs
|
||
153 | */
|
||
154 | /**************************************************************************/
|
||
155 | void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign )
|
||
156 | { |
||
157 | adafruit_bno055_opmode_t modeback = _mode; |
||
158 | |||
159 | setMode(OPERATION_MODE_CONFIG); |
||
160 | delay(25);
|
||
161 | write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign); |
||
162 | delay(10);
|
||
163 | /* Set the requested operating mode (see section 3.3) */
|
||
164 | setMode(modeback); |
||
165 | delay(20);
|
||
166 | } |
||
167 | |||
168 | |||
169 | /**************************************************************************/
|
||
170 | /*!
|
||
171 | 463eabf7 | Wetmelon | @brief Use the external 32.768KHz crystal
|
172 | */
|
||
173 | c4f272e1 | ladyada | /**************************************************************************/
|
174 | void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
|
||
175 | { |
||
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 | /*!
|
||
196 | 463eabf7 | Wetmelon | @brief Gets the latest system status info
|
197 | */
|
||
198 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
199 | 3b2655dc | ladyada | void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
|
200 | 4bc1c0c1 | Kevin Townsend | { |
201 | 463eabf7 | Wetmelon | write8(BNO055_PAGE_ID_ADDR, 0);
|
202 | |||
203 | /* System Status (see section 4.3.58)
|
||
204 | ---------------------------------
|
||
205 | 0 = Idle
|
||
206 | 1 = System Error
|
||
207 | 2 = Initializing Peripherals
|
||
208 | 3 = System Iniitalization
|
||
209 | 4 = Executing Self-Test
|
||
210 | 5 = Sensor fusio algorithm running
|
||
211 | 6 = System running without fusion algorithms */
|
||
212 | |||
213 | if (system_status != 0) |
||
214 | *system_status = read8(BNO055_SYS_STAT_ADDR); |
||
215 | |||
216 | /* Self Test Results (see section )
|
||
217 | --------------------------------
|
||
218 | 1 = test passed, 0 = test failed
|
||
219 | |||
220 | Bit 0 = Accelerometer self test
|
||
221 | Bit 1 = Magnetometer self test
|
||
222 | Bit 2 = Gyroscope self test
|
||
223 | Bit 3 = MCU self test
|
||
224 | |||
225 | 0x0F = all good! */
|
||
226 | |||
227 | if (self_test_result != 0) |
||
228 | *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR); |
||
229 | |||
230 | /* System Error (see section 4.3.59)
|
||
231 | ---------------------------------
|
||
232 | 0 = No error
|
||
233 | 1 = Peripheral initialization error
|
||
234 | 2 = System initialization error
|
||
235 | 3 = Self test result failed
|
||
236 | 4 = Register map value out of range
|
||
237 | 5 = Register map address out of range
|
||
238 | 6 = Register map write error
|
||
239 | 7 = BNO low power mode not available for selected operat ion mode
|
||
240 | 8 = Accelerometer power mode not available
|
||
241 | 9 = Fusion algorithm configuration error
|
||
242 | A = Sensor configuration error */
|
||
243 | |||
244 | if (system_error != 0) |
||
245 | *system_error = read8(BNO055_SYS_ERR_ADDR); |
||
246 | |||
247 | delay(200);
|
||
248 | 4bc1c0c1 | Kevin Townsend | } |
249 | |||
250 | /**************************************************************************/
|
||
251 | /*!
|
||
252 | 463eabf7 | Wetmelon | @brief Gets the chip revision numbers
|
253 | */
|
||
254 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
255 | void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
|
||
256 | { |
||
257 | 463eabf7 | Wetmelon | uint8_t a, b; |
258 | 4bc1c0c1 | Kevin Townsend | |
259 | 463eabf7 | Wetmelon | memset(info, 0, sizeof(adafruit_bno055_rev_info_t)); |
260 | 4bc1c0c1 | Kevin Townsend | |
261 | 463eabf7 | Wetmelon | /* Check the accelerometer revision */
|
262 | info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR); |
||
263 | 67f3cff5 | Kevin Townsend | |
264 | 463eabf7 | Wetmelon | /* Check the magnetometer revision */
|
265 | info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR); |
||
266 | 67f3cff5 | Kevin Townsend | |
267 | 463eabf7 | Wetmelon | /* Check the gyroscope revision */
|
268 | info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR); |
||
269 | 67f3cff5 | Kevin Townsend | |
270 | 463eabf7 | Wetmelon | /* Check the SW revision */
|
271 | info->bl_rev = read8(BNO055_BL_REV_ID_ADDR); |
||
272 | 40f91f6f | Tony DiCola | |
273 | 463eabf7 | Wetmelon | a = read8(BNO055_SW_REV_ID_LSB_ADDR); |
274 | b = read8(BNO055_SW_REV_ID_MSB_ADDR); |
||
275 | info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
|
||
276 | 4bc1c0c1 | Kevin Townsend | } |
277 | |||
278 | /**************************************************************************/
|
||
279 | /*!
|
||
280 | 463eabf7 | Wetmelon | @brief Gets current calibration state. Each value should be a uint8_t
|
281 | pointer and it will be set to 0 if not calibrated and 3 if
|
||
282 | fully calibrated.
|
||
283 | */
|
||
284 | 40f91f6f | Tony DiCola | /**************************************************************************/
|
285 | void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
|
||
286 | 463eabf7 | Wetmelon | uint8_t calData = read8(BNO055_CALIB_STAT_ADDR); |
287 | if (sys != NULL) { |
||
288 | *sys = (calData >> 6) & 0x03; |
||
289 | } |
||
290 | if (gyro != NULL) { |
||
291 | *gyro = (calData >> 4) & 0x03; |
||
292 | } |
||
293 | if (accel != NULL) { |
||
294 | *accel = (calData >> 2) & 0x03; |
||
295 | } |
||
296 | if (mag != NULL) { |
||
297 | *mag = calData & 0x03;
|
||
298 | } |
||
299 | 40f91f6f | Tony DiCola | } |
300 | |||
301 | /**************************************************************************/
|
||
302 | /*!
|
||
303 | 463eabf7 | Wetmelon | @brief Gets the temperature in degrees celsius
|
304 | */
|
||
305 | 0e2e2723 | Kevin Townsend | /**************************************************************************/
|
306 | int8_t Adafruit_BNO055::getTemp(void)
|
||
307 | { |
||
308 | 463eabf7 | Wetmelon | int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR)); |
309 | return temp;
|
||
310 | 0e2e2723 | Kevin Townsend | } |
311 | |||
312 | /**************************************************************************/
|
||
313 | /*!
|
||
314 | 463eabf7 | Wetmelon | @brief Gets a vector reading from the specified source
|
315 | */
|
||
316 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
317 | 48741e1f | Kevin Townsend | imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
|
318 | 4bc1c0c1 | Kevin Townsend | { |
319 | 463eabf7 | Wetmelon | imu::Vector<3> xyz;
|
320 | uint8_t buffer[6];
|
||
321 | memset (buffer, 0, 6); |
||
322 | |||
323 | int16_t x, y, z; |
||
324 | x = y = z = 0;
|
||
325 | |||
326 | /* Read vector data (6 bytes) */
|
||
327 | readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
|
||
328 | |||
329 | x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8); |
||
330 | y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8); |
||
331 | z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8); |
||
332 | |||
333 | /* Convert the value to an appropriate range (section 3.6.4) */
|
||
334 | /* and assign the value to the Vector type */
|
||
335 | switch(vector_type)
|
||
336 | { |
||
337 | case VECTOR_MAGNETOMETER:
|
||
338 | /* 1uT = 16 LSB */
|
||
339 | xyz[0] = ((double)x)/16.0; |
||
340 | xyz[1] = ((double)y)/16.0; |
||
341 | xyz[2] = ((double)z)/16.0; |
||
342 | break;
|
||
343 | case VECTOR_GYROSCOPE:
|
||
344 | /* 1rps = 900 LSB */
|
||
345 | xyz[0] = ((double)x)/900.0; |
||
346 | xyz[1] = ((double)y)/900.0; |
||
347 | xyz[2] = ((double)z)/900.0; |
||
348 | break;
|
||
349 | case VECTOR_EULER:
|
||
350 | /* 1 degree = 16 LSB */
|
||
351 | xyz[0] = ((double)x)/16.0; |
||
352 | xyz[1] = ((double)y)/16.0; |
||
353 | xyz[2] = ((double)z)/16.0; |
||
354 | break;
|
||
355 | case VECTOR_ACCELEROMETER:
|
||
356 | case VECTOR_LINEARACCEL:
|
||
357 | case VECTOR_GRAVITY:
|
||
358 | /* 1m/s^2 = 100 LSB */
|
||
359 | xyz[0] = ((double)x)/100.0; |
||
360 | xyz[1] = ((double)y)/100.0; |
||
361 | xyz[2] = ((double)z)/100.0; |
||
362 | break;
|
||
363 | } |
||
364 | |||
365 | return xyz;
|
||
366 | 4bc1c0c1 | Kevin Townsend | } |
367 | |||
368 | /**************************************************************************/
|
||
369 | /*!
|
||
370 | 463eabf7 | Wetmelon | @brief Gets a quaternion reading from the specified source
|
371 | */
|
||
372 | 48741e1f | Kevin Townsend | /**************************************************************************/
|
373 | imu::Quaternion Adafruit_BNO055::getQuat(void)
|
||
374 | { |
||
375 | 463eabf7 | Wetmelon | uint8_t buffer[8];
|
376 | memset (buffer, 0, 8); |
||
377 | |||
378 | int16_t x, y, z, w; |
||
379 | x = y = z = w = 0;
|
||
380 | |||
381 | /* Read quat data (8 bytes) */
|
||
382 | readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
|
||
383 | w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); |
||
384 | x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); |
||
385 | y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); |
||
386 | z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); |
||
387 | |||
388 | /* Assign to Quaternion */
|
||
389 | /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
|
||
390 | 3.6.5.5 Orientation (Quaternion) */
|
||
391 | const double scale = (1.0 / (1<<14)); |
||
392 | imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z); |
||
393 | return quat;
|
||
394 | 48741e1f | Kevin Townsend | } |
395 | |||
396 | /**************************************************************************/
|
||
397 | /*!
|
||
398 | 463eabf7 | Wetmelon | @brief Provides the sensor_t data for this sensor
|
399 | */
|
||
400 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
401 | void Adafruit_BNO055::getSensor(sensor_t *sensor)
|
||
402 | { |
||
403 | 463eabf7 | Wetmelon | /* Clear the sensor_t object */
|
404 | memset(sensor, 0, sizeof(sensor_t)); |
||
405 | |||
406 | /* Insert the sensor name in the fixed length char array */
|
||
407 | strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1); |
||
408 | sensor->name[sizeof(sensor->name)- 1] = 0; |
||
409 | sensor->version = 1;
|
||
410 | sensor->sensor_id = _sensorID; |
||
411 | sensor->type = SENSOR_TYPE_ORIENTATION; |
||
412 | sensor->min_delay = 0;
|
||
413 | sensor->max_value = 0.0F; |
||
414 | sensor->min_value = 0.0F; |
||
415 | sensor->resolution = 0.01F; |
||
416 | 4bc1c0c1 | Kevin Townsend | } |
417 | |||
418 | /**************************************************************************/
|
||
419 | /*!
|
||
420 | 463eabf7 | Wetmelon | @brief Reads the sensor and returns the data as a sensors_event_t
|
421 | */
|
||
422 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
423 | bool Adafruit_BNO055::getEvent(sensors_event_t *event)
|
||
424 | { |
||
425 | 463eabf7 | Wetmelon | /* Clear the event */
|
426 | memset(event, 0, sizeof(sensors_event_t)); |
||
427 | 4bc1c0c1 | Kevin Townsend | |
428 | 463eabf7 | Wetmelon | event->version = sizeof(sensors_event_t);
|
429 | event->sensor_id = _sensorID; |
||
430 | event->type = SENSOR_TYPE_ORIENTATION; |
||
431 | event->timestamp = millis(); |
||
432 | fcd68623 | Kevin Townsend | |
433 | 463eabf7 | Wetmelon | /* Get a Euler angle sample for orientation */
|
434 | imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
|
||
435 | event->orientation.x = euler.x(); |
||
436 | event->orientation.y = euler.y(); |
||
437 | event->orientation.z = euler.z(); |
||
438 | 312a5b9e | Wetmelon | |
439 | 463eabf7 | Wetmelon | return true; |
440 | 312a5b9e | Wetmelon | } |
441 | fcd68623 | Kevin Townsend | |
442 | 312a5b9e | Wetmelon | /**************************************************************************/
|
443 | /*!
|
||
444 | @brief Reads the sensor's offset registers into a byte array
|
||
445 | */
|
||
446 | /**************************************************************************/
|
||
447 | 8e095f02 | Wetmelon | bool Adafruit_BNO055::getSensorOffsets(uint8_t* calibData)
|
448 | 312a5b9e | Wetmelon | { |
449 | 463eabf7 | Wetmelon | if (isFullyCalibrated())
|
450 | { |
||
451 | adafruit_bno055_opmode_t lastMode = _mode; |
||
452 | setMode(OPERATION_MODE_CONFIG); |
||
453 | 312a5b9e | Wetmelon | |
454 | 463eabf7 | Wetmelon | readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS); |
455 | 312a5b9e | Wetmelon | |
456 | 463eabf7 | Wetmelon | setMode(lastMode); |
457 | return true; |
||
458 | } |
||
459 | return false; |
||
460 | 312a5b9e | Wetmelon | } |
461 | |||
462 | /**************************************************************************/
|
||
463 | /*!
|
||
464 | @brief Reads the sensor's offset registers into an offset struct
|
||
465 | */
|
||
466 | /**************************************************************************/
|
||
467 | bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
|
||
468 | { |
||
469 | 463eabf7 | Wetmelon | if (isFullyCalibrated())
|
470 | { |
||
471 | adafruit_bno055_opmode_t lastMode = _mode; |
||
472 | setMode(OPERATION_MODE_CONFIG); |
||
473 | delay(25);
|
||
474 | |||
475 | offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
|
||
476 | offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
|
||
477 | offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
|
||
478 | |||
479 | offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
|
||
480 | offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
|
||
481 | offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
|
||
482 | |||
483 | offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
|
||
484 | offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
|
||
485 | offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
|
||
486 | |||
487 | offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
|
||
488 | offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
|
||
489 | |||
490 | setMode(lastMode); |
||
491 | return true; |
||
492 | } |
||
493 | return false; |
||
494 | 312a5b9e | Wetmelon | } |
495 | |||
496 | |||
497 | /**************************************************************************/
|
||
498 | /*!
|
||
499 | @brief Writes an array of calibration values to the sensor's offset registers
|
||
500 | */
|
||
501 | /**************************************************************************/
|
||
502 | 8e095f02 | Wetmelon | void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData) |
503 | 312a5b9e | Wetmelon | { |
504 | 463eabf7 | Wetmelon | adafruit_bno055_opmode_t lastMode = _mode; |
505 | setMode(OPERATION_MODE_CONFIG); |
||
506 | delay(25);
|
||
507 | |||
508 | /* A writeLen() would make this much cleaner */
|
||
509 | write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
|
||
510 | write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
|
||
511 | write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
|
||
512 | write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
|
||
513 | write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
|
||
514 | write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
|
||
515 | |||
516 | write8(GYRO_OFFSET_X_LSB_ADDR, calibData[6]);
|
||
517 | write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]);
|
||
518 | write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]);
|
||
519 | write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]);
|
||
520 | write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]);
|
||
521 | write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]);
|
||
522 | |||
523 | write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]);
|
||
524 | write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]);
|
||
525 | write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]);
|
||
526 | write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]);
|
||
527 | write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]);
|
||
528 | write8(MAG_OFFSET_Z_MSB_ADDR, calibData[17]);
|
||
529 | |||
530 | write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
|
||
531 | write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
|
||
532 | |||
533 | write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
|
||
534 | write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
|
||
535 | |||
536 | setMode(lastMode); |
||
537 | 4bc1c0c1 | Kevin Townsend | } |
538 | |||
539 | 312a5b9e | Wetmelon | /**************************************************************************/
|
540 | /*!
|
||
541 | @brief Writes to the sensor's offset registers from an offset struct
|
||
542 | */
|
||
543 | /**************************************************************************/
|
||
544 | void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) |
||
545 | { |
||
546 | 463eabf7 | Wetmelon | adafruit_bno055_opmode_t lastMode = _mode; |
547 | setMode(OPERATION_MODE_CONFIG); |
||
548 | delay(25);
|
||
549 | |||
550 | write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
|
||
551 | write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); |
||
552 | write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
|
||
553 | write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); |
||
554 | write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
|
||
555 | write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); |
||
556 | |||
557 | write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
|
||
558 | write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); |
||
559 | write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
|
||
560 | write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); |
||
561 | write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
|
||
562 | write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); |
||
563 | |||
564 | write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
|
||
565 | write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); |
||
566 | write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
|
||
567 | write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); |
||
568 | write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
|
||
569 | write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); |
||
570 | |||
571 | write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
|
||
572 | write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); |
||
573 | |||
574 | write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
|
||
575 | write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); |
||
576 | |||
577 | setMode(lastMode); |
||
578 | 312a5b9e | Wetmelon | } |
579 | |||
580 | bool Adafruit_BNO055::isFullyCalibrated(void) |
||
581 | { |
||
582 | 463eabf7 | Wetmelon | uint8_t system, gyro, accel, mag; |
583 | getCalibration(&system, &gyro, &accel, &mag); |
||
584 | if (system < 3 || gyro < 3 || accel < 3 || mag < 3) |
||
585 | return false; |
||
586 | return true; |
||
587 | 312a5b9e | Wetmelon | } |
588 | |||
589 | |||
590 | 4bc1c0c1 | Kevin Townsend | /***************************************************************************
|
591 | PRIVATE FUNCTIONS
|
||
592 | ***************************************************************************/
|
||
593 | |||
594 | /**************************************************************************/
|
||
595 | /*!
|
||
596 | 463eabf7 | Wetmelon | @brief Writes an 8 bit value over I2C
|
597 | */
|
||
598 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
599 | bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
|
||
600 | { |
||
601 | 463eabf7 | Wetmelon | Wire.beginTransmission(_address); |
602 | #if ARDUINO >= 100 |
||
603 | Wire.write((uint8_t)reg); |
||
604 | Wire.write((uint8_t)value); |
||
605 | #else
|
||
606 | Wire.send(reg); |
||
607 | Wire.send(value); |
||
608 | #endif
|
||
609 | Wire.endTransmission(); |
||
610 | |||
611 | /* ToDo: Check for error! */
|
||
612 | return true; |
||
613 | 4bc1c0c1 | Kevin Townsend | } |
614 | |||
615 | /**************************************************************************/
|
||
616 | /*!
|
||
617 | 463eabf7 | Wetmelon | @brief Reads an 8 bit value over I2C
|
618 | */
|
||
619 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
620 | 463eabf7 | Wetmelon | byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg ) |
621 | 4bc1c0c1 | Kevin Townsend | { |
622 | 463eabf7 | Wetmelon | byte value = 0;
|
623 | |||
624 | Wire.beginTransmission(_address); |
||
625 | #if ARDUINO >= 100 |
||
626 | Wire.write((uint8_t)reg); |
||
627 | #else
|
||
628 | Wire.send(reg); |
||
629 | #endif
|
||
630 | Wire.endTransmission(); |
||
631 | Wire.requestFrom(_address, (byte)1);
|
||
632 | #if ARDUINO >= 100 |
||
633 | value = Wire.read(); |
||
634 | #else
|
||
635 | value = Wire.receive(); |
||
636 | #endif
|
||
637 | |||
638 | return value;
|
||
639 | 4bc1c0c1 | Kevin Townsend | } |
640 | |||
641 | /**************************************************************************/
|
||
642 | /*!
|
||
643 | 463eabf7 | Wetmelon | @brief Reads the specified number of bytes over I2C
|
644 | */
|
||
645 | 4bc1c0c1 | Kevin Townsend | /**************************************************************************/
|
646 | bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
|
||
647 | { |
||
648 | 463eabf7 | Wetmelon | Wire.beginTransmission(_address); |
649 | #if ARDUINO >= 100 |
||
650 | Wire.write((uint8_t)reg); |
||
651 | #else
|
||
652 | Wire.send(reg); |
||
653 | #endif
|
||
654 | Wire.endTransmission(); |
||
655 | Wire.requestFrom(_address, (byte)len); |
||
656 | |||
657 | for (uint8_t i = 0; i < len; i++) |
||
658 | { |
||
659 | #if ARDUINO >= 100 |
||
660 | buffer[i] = Wire.read(); |
||
661 | #else
|
||
662 | buffer[i] = Wire.receive(); |
||
663 | #endif
|
||
664 | } |
||
665 | |||
666 | /* ToDo: Check for errors! */
|
||
667 | return true; |
||
668 | } |