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