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