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