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