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