Revision 55e2f6d1 Adafruit_BNO055.cpp
Adafruit_BNO055.cpp | ||
---|---|---|
18 | 18 |
***************************************************************************/ |
19 | 19 |
|
20 | 20 |
#if ARDUINO >= 100 |
21 |
#include "Arduino.h"
|
|
21 |
#include "Arduino.h" |
|
22 | 22 |
#else |
23 |
#include "WProgram.h"
|
|
23 |
#include "WProgram.h" |
|
24 | 24 |
#endif |
25 | 25 |
|
26 |
#include <math.h> |
|
27 | 26 |
#include <limits.h> |
27 |
#include <math.h> |
|
28 | 28 |
|
29 | 29 |
#include "Adafruit_BNO055.h" |
30 | 30 |
|
... | ... | |
35 | 35 |
/**************************************************************************/ |
36 | 36 |
/*! |
37 | 37 |
@brief Instantiates a new Adafruit_BNO055 class |
38 |
@param sensorID |
|
39 |
@param address |
|
38 | 40 |
*/ |
39 | 41 |
/**************************************************************************/ |
40 |
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) |
|
41 |
{ |
|
42 |
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) { |
|
42 | 43 |
_sensorID = sensorID; |
43 | 44 |
_address = address; |
44 | 45 |
} |
... | ... | |
50 | 51 |
/**************************************************************************/ |
51 | 52 |
/*! |
52 | 53 |
@brief Sets up the HW |
54 |
@param mode |
|
55 |
@return true if process is sucessfull |
|
53 | 56 |
*/ |
54 | 57 |
/**************************************************************************/ |
55 |
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) |
|
56 |
{ |
|
58 |
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) { |
|
57 | 59 |
/* Enable I2C */ |
58 | 60 |
Wire.begin(); |
59 | 61 |
|
... | ... | |
64 | 66 |
|
65 | 67 |
/* Make sure we have the right device */ |
66 | 68 |
uint8_t id = read8(BNO055_CHIP_ID_ADDR); |
67 |
if(id != BNO055_ID) |
|
68 |
{ |
|
69 |
if (id != BNO055_ID) { |
|
69 | 70 |
delay(1000); // hold on for boot |
70 | 71 |
id = read8(BNO055_CHIP_ID_ADDR); |
71 |
if(id != BNO055_ID) { |
|
72 |
return false; // still not? ok bail
|
|
72 |
if (id != BNO055_ID) {
|
|
73 |
return false; // still not? ok bail |
|
73 | 74 |
} |
74 | 75 |
} |
75 | 76 |
|
... | ... | |
78 | 79 |
|
79 | 80 |
/* Reset */ |
80 | 81 |
write8(BNO055_SYS_TRIGGER_ADDR, 0x20); |
81 |
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) |
|
82 |
{ |
|
82 |
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) { |
|
83 | 83 |
delay(10); |
84 | 84 |
} |
85 | 85 |
delay(50); |
... | ... | |
120 | 120 |
/**************************************************************************/ |
121 | 121 |
/*! |
122 | 122 |
@brief Puts the chip in the specified operating mode |
123 |
@param mode |
|
123 | 124 |
*/ |
124 | 125 |
/**************************************************************************/ |
125 |
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) |
|
126 |
{ |
|
126 |
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) { |
|
127 | 127 |
_mode = mode; |
128 | 128 |
write8(BNO055_OPR_MODE_ADDR, _mode); |
129 | 129 |
delay(30); |
... | ... | |
132 | 132 |
/**************************************************************************/ |
133 | 133 |
/*! |
134 | 134 |
@brief Changes the chip's axis remap |
135 |
@param remapcode |
|
135 | 136 |
*/ |
136 | 137 |
/**************************************************************************/ |
137 |
void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode )
|
|
138 |
{ |
|
138 |
void Adafruit_BNO055::setAxisRemap( |
|
139 |
adafruit_bno055_axis_remap_config_t remapcode) {
|
|
139 | 140 |
adafruit_bno055_opmode_t modeback = _mode; |
140 | 141 |
|
141 | 142 |
setMode(OPERATION_MODE_CONFIG); |
... | ... | |
150 | 151 |
/**************************************************************************/ |
151 | 152 |
/*! |
152 | 153 |
@brief Changes the chip's axis signs |
154 |
@param remapsign |
|
153 | 155 |
*/ |
154 | 156 |
/**************************************************************************/ |
155 |
void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign ) |
|
156 |
{ |
|
157 |
void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) { |
|
157 | 158 |
adafruit_bno055_opmode_t modeback = _mode; |
158 | 159 |
|
159 | 160 |
setMode(OPERATION_MODE_CONFIG); |
... | ... | |
165 | 166 |
delay(20); |
166 | 167 |
} |
167 | 168 |
|
168 |
|
|
169 | 169 |
/**************************************************************************/ |
170 | 170 |
/*! |
171 | 171 |
@brief Use the external 32.768KHz crystal |
172 |
@param usextal boolean |
|
172 | 173 |
*/ |
173 | 174 |
/**************************************************************************/ |
174 |
void Adafruit_BNO055::setExtCrystalUse(boolean usextal) |
|
175 |
{ |
|
175 |
void Adafruit_BNO055::setExtCrystalUse(boolean usextal) { |
|
176 | 176 |
adafruit_bno055_opmode_t modeback = _mode; |
177 | 177 |
|
178 | 178 |
/* Switch to config mode (just in case since this is the default) */ |
... | ... | |
190 | 190 |
delay(20); |
191 | 191 |
} |
192 | 192 |
|
193 |
|
|
194 | 193 |
/**************************************************************************/ |
195 | 194 |
/*! |
196 | 195 |
@brief Gets the latest system status info |
196 |
@param system_status |
|
197 |
@param self_test_result |
|
198 |
@param system_error |
|
197 | 199 |
*/ |
198 | 200 |
/**************************************************************************/ |
199 |
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) |
|
200 |
{ |
|
201 |
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, |
|
202 |
uint8_t *self_test_result, |
|
203 |
uint8_t *system_error) { |
|
201 | 204 |
write8(BNO055_PAGE_ID_ADDR, 0); |
202 | 205 |
|
203 | 206 |
/* System Status (see section 4.3.58) |
... | ... | |
211 | 214 |
6 = System running without fusion algorithms */ |
212 | 215 |
|
213 | 216 |
if (system_status != 0) |
214 |
*system_status = read8(BNO055_SYS_STAT_ADDR);
|
|
217 |
*system_status = read8(BNO055_SYS_STAT_ADDR); |
|
215 | 218 |
|
216 | 219 |
/* Self Test Results (see section ) |
217 | 220 |
-------------------------------- |
... | ... | |
242 | 245 |
A = Sensor configuration error */ |
243 | 246 |
|
244 | 247 |
if (system_error != 0) |
245 |
*system_error = read8(BNO055_SYS_ERR_ADDR);
|
|
248 |
*system_error = read8(BNO055_SYS_ERR_ADDR); |
|
246 | 249 |
|
247 | 250 |
delay(200); |
248 | 251 |
} |
... | ... | |
252 | 255 |
@brief Gets the chip revision numbers |
253 | 256 |
*/ |
254 | 257 |
/**************************************************************************/ |
255 |
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info) |
|
256 |
{ |
|
258 |
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) { |
|
257 | 259 |
uint8_t a, b; |
258 | 260 |
|
259 | 261 |
memset(info, 0, sizeof(adafruit_bno055_rev_info_t)); |
... | ... | |
262 | 264 |
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR); |
263 | 265 |
|
264 | 266 |
/* Check the magnetometer revision */ |
265 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
|
|
267 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR); |
|
266 | 268 |
|
267 | 269 |
/* Check the gyroscope revision */ |
268 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
|
|
270 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR); |
|
269 | 271 |
|
270 | 272 |
/* Check the SW revision */ |
271 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
|
|
273 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR); |
|
272 | 274 |
|
273 | 275 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR); |
274 | 276 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR); |
... | ... | |
280 | 282 |
@brief Gets current calibration state. Each value should be a uint8_t |
281 | 283 |
pointer and it will be set to 0 if not calibrated and 3 if |
282 | 284 |
fully calibrated. |
285 |
@param sys |
|
286 |
@param gyro |
|
287 |
@param accel |
|
288 |
@param mag |
|
283 | 289 |
*/ |
284 | 290 |
/**************************************************************************/ |
285 |
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) { |
|
291 |
void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro, |
|
292 |
uint8_t *accel, uint8_t *mag) { |
|
286 | 293 |
uint8_t calData = read8(BNO055_CALIB_STAT_ADDR); |
287 | 294 |
if (sys != NULL) { |
288 | 295 |
*sys = (calData >> 6) & 0x03; |
... | ... | |
301 | 308 |
/**************************************************************************/ |
302 | 309 |
/*! |
303 | 310 |
@brief Gets the temperature in degrees celsius |
311 |
@return temperature in degrees celsius |
|
304 | 312 |
*/ |
305 | 313 |
/**************************************************************************/ |
306 |
int8_t Adafruit_BNO055::getTemp(void) |
|
307 |
{ |
|
314 |
int8_t Adafruit_BNO055::getTemp() { |
|
308 | 315 |
int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR)); |
309 | 316 |
return temp; |
310 | 317 |
} |
311 | 318 |
|
312 | 319 |
/**************************************************************************/ |
313 | 320 |
/*! |
314 |
@brief Gets a vector reading from the specified source |
|
321 |
@brief Gets a vector reading from the specified source |
|
322 |
@param vector_type |
|
323 |
@return vector from specified source |
|
315 | 324 |
*/ |
316 | 325 |
/**************************************************************************/ |
317 |
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) |
|
318 |
{ |
|
326 |
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) { |
|
319 | 327 |
imu::Vector<3> xyz; |
320 | 328 |
uint8_t buffer[6]; |
321 |
memset (buffer, 0, 6);
|
|
329 |
memset(buffer, 0, 6); |
|
322 | 330 |
|
323 | 331 |
int16_t x, y, z; |
324 | 332 |
x = y = z = 0; |
... | ... | |
332 | 340 |
|
333 | 341 |
/* Convert the value to an appropriate range (section 3.6.4) */ |
334 | 342 |
/* and assign the value to the Vector type */ |
335 |
switch(vector_type) |
|
336 |
{ |
|
337 |
case VECTOR_MAGNETOMETER: |
|
338 |
/* 1uT = 16 LSB */ |
|
339 |
xyz[0] = ((double)x)/16.0; |
|
340 |
xyz[1] = ((double)y)/16.0; |
|
341 |
xyz[2] = ((double)z)/16.0; |
|
342 |
break; |
|
343 |
case VECTOR_GYROSCOPE: |
|
344 |
/* 1dps = 16 LSB */ |
|
345 |
xyz[0] = ((double)x)/16.0; |
|
346 |
xyz[1] = ((double)y)/16.0; |
|
347 |
xyz[2] = ((double)z)/16.0; |
|
348 |
break; |
|
349 |
case VECTOR_EULER: |
|
350 |
/* 1 degree = 16 LSB */ |
|
351 |
xyz[0] = ((double)x)/16.0; |
|
352 |
xyz[1] = ((double)y)/16.0; |
|
353 |
xyz[2] = ((double)z)/16.0; |
|
354 |
break; |
|
355 |
case VECTOR_ACCELEROMETER: |
|
356 |
case VECTOR_LINEARACCEL: |
|
357 |
case VECTOR_GRAVITY: |
|
358 |
/* 1m/s^2 = 100 LSB */ |
|
359 |
xyz[0] = ((double)x)/100.0; |
|
360 |
xyz[1] = ((double)y)/100.0; |
|
361 |
xyz[2] = ((double)z)/100.0; |
|
362 |
break; |
|
343 |
switch (vector_type) { |
|
344 |
case VECTOR_MAGNETOMETER: |
|
345 |
/* 1uT = 16 LSB */ |
|
346 |
xyz[0] = ((double)x) / 16.0; |
|
347 |
xyz[1] = ((double)y) / 16.0; |
|
348 |
xyz[2] = ((double)z) / 16.0; |
|
349 |
break; |
|
350 |
case VECTOR_GYROSCOPE: |
|
351 |
/* 1dps = 16 LSB */ |
|
352 |
xyz[0] = ((double)x) / 16.0; |
|
353 |
xyz[1] = ((double)y) / 16.0; |
|
354 |
xyz[2] = ((double)z) / 16.0; |
|
355 |
break; |
|
356 |
case VECTOR_EULER: |
|
357 |
/* 1 degree = 16 LSB */ |
|
358 |
xyz[0] = ((double)x) / 16.0; |
|
359 |
xyz[1] = ((double)y) / 16.0; |
|
360 |
xyz[2] = ((double)z) / 16.0; |
|
361 |
break; |
|
362 |
case VECTOR_ACCELEROMETER: |
|
363 |
case VECTOR_LINEARACCEL: |
|
364 |
case VECTOR_GRAVITY: |
|
365 |
/* 1m/s^2 = 100 LSB */ |
|
366 |
xyz[0] = ((double)x) / 100.0; |
|
367 |
xyz[1] = ((double)y) / 100.0; |
|
368 |
xyz[2] = ((double)z) / 100.0; |
|
369 |
break; |
|
363 | 370 |
} |
364 | 371 |
|
365 | 372 |
return xyz; |
... | ... | |
368 | 375 |
/**************************************************************************/ |
369 | 376 |
/*! |
370 | 377 |
@brief Gets a quaternion reading from the specified source |
378 |
@return quaternion reading |
|
371 | 379 |
*/ |
372 | 380 |
/**************************************************************************/ |
373 |
imu::Quaternion Adafruit_BNO055::getQuat(void) |
|
374 |
{ |
|
381 |
imu::Quaternion Adafruit_BNO055::getQuat() { |
|
375 | 382 |
uint8_t buffer[8]; |
376 |
memset (buffer, 0, 8);
|
|
383 |
memset(buffer, 0, 8); |
|
377 | 384 |
|
378 | 385 |
int16_t x, y, z, w; |
379 | 386 |
x = y = z = w = 0; |
... | ... | |
386 | 393 |
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); |
387 | 394 |
|
388 | 395 |
/* Assign to Quaternion */ |
389 |
/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf |
|
396 |
/* See |
|
397 |
http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf |
|
390 | 398 |
3.6.5.5 Orientation (Quaternion) */ |
391 |
const double scale = (1.0 / (1<<14));
|
|
399 |
const double scale = (1.0 / (1 << 14));
|
|
392 | 400 |
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z); |
393 | 401 |
return quat; |
394 | 402 |
} |
... | ... | |
396 | 404 |
/**************************************************************************/ |
397 | 405 |
/*! |
398 | 406 |
@brief Provides the sensor_t data for this sensor |
407 |
@param sensor |
|
399 | 408 |
*/ |
400 | 409 |
/**************************************************************************/ |
401 |
void Adafruit_BNO055::getSensor(sensor_t *sensor) |
|
402 |
{ |
|
410 |
void Adafruit_BNO055::getSensor(sensor_t *sensor) { |
|
403 | 411 |
/* Clear the sensor_t object */ |
404 | 412 |
memset(sensor, 0, sizeof(sensor_t)); |
405 | 413 |
|
406 | 414 |
/* Insert the sensor name in the fixed length char array */ |
407 |
strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
|
|
408 |
sensor->name[sizeof(sensor->name)- 1] = 0; |
|
409 |
sensor->version = 1;
|
|
410 |
sensor->sensor_id = _sensorID;
|
|
411 |
sensor->type = SENSOR_TYPE_ORIENTATION;
|
|
412 |
sensor->min_delay = 0;
|
|
413 |
sensor->max_value = 0.0F;
|
|
414 |
sensor->min_value = 0.0F;
|
|
415 |
sensor->resolution = 0.01F;
|
|
415 |
strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1); |
|
416 |
sensor->name[sizeof(sensor->name) - 1] = 0;
|
|
417 |
sensor->version = 1; |
|
418 |
sensor->sensor_id = _sensorID; |
|
419 |
sensor->type = SENSOR_TYPE_ORIENTATION; |
|
420 |
sensor->min_delay = 0; |
|
421 |
sensor->max_value = 0.0F; |
|
422 |
sensor->min_value = 0.0F; |
|
423 |
sensor->resolution = 0.01F; |
|
416 | 424 |
} |
417 | 425 |
|
418 | 426 |
/**************************************************************************/ |
419 | 427 |
/*! |
420 | 428 |
@brief Reads the sensor and returns the data as a sensors_event_t |
429 |
@param event |
|
430 |
@return always returns true |
|
421 | 431 |
*/ |
422 | 432 |
/**************************************************************************/ |
423 |
bool Adafruit_BNO055::getEvent(sensors_event_t *event) |
|
424 |
{ |
|
433 |
bool Adafruit_BNO055::getEvent(sensors_event_t *event) { |
|
425 | 434 |
/* Clear the event */ |
426 | 435 |
memset(event, 0, sizeof(sensors_event_t)); |
427 | 436 |
|
428 |
event->version = sizeof(sensors_event_t);
|
|
437 |
event->version = sizeof(sensors_event_t); |
|
429 | 438 |
event->sensor_id = _sensorID; |
430 |
event->type = SENSOR_TYPE_ORIENTATION;
|
|
439 |
event->type = SENSOR_TYPE_ORIENTATION; |
|
431 | 440 |
event->timestamp = millis(); |
432 | 441 |
|
433 | 442 |
/* Get a Euler angle sample for orientation */ |
... | ... | |
441 | 450 |
|
442 | 451 |
/**************************************************************************/ |
443 | 452 |
/*! |
444 |
@brief Reads the sensor's offset registers into a byte array |
|
453 |
@brief Reads the sensor's offset registers into a byte array |
|
454 |
@param calibData |
|
455 |
@return true if read is successful |
|
445 | 456 |
*/ |
446 | 457 |
/**************************************************************************/ |
447 |
bool Adafruit_BNO055::getSensorOffsets(uint8_t* calibData) |
|
448 |
{ |
|
449 |
if (isFullyCalibrated()) |
|
450 |
{ |
|
451 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
452 |
setMode(OPERATION_MODE_CONFIG); |
|
458 |
bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) { |
|
459 |
if (isFullyCalibrated()) { |
|
460 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
461 |
setMode(OPERATION_MODE_CONFIG); |
|
453 | 462 |
|
454 |
readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
|
|
463 |
readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS); |
|
455 | 464 |
|
456 |
setMode(lastMode);
|
|
457 |
return true;
|
|
458 |
}
|
|
459 |
return false;
|
|
465 |
setMode(lastMode); |
|
466 |
return true; |
|
467 |
} |
|
468 |
return false; |
|
460 | 469 |
} |
461 | 470 |
|
462 | 471 |
/**************************************************************************/ |
463 | 472 |
/*! |
464 |
@brief Reads the sensor's offset registers into an offset struct |
|
473 |
@brief Reads the sensor's offset registers into an offset struct |
|
474 |
@param offsets_type |
|
475 |
@return true if read is successful |
|
465 | 476 |
*/ |
466 | 477 |
/**************************************************************************/ |
467 |
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) |
|
468 |
{ |
|
469 |
if (isFullyCalibrated()) |
|
470 |
{ |
|
471 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
472 |
setMode(OPERATION_MODE_CONFIG); |
|
473 |
delay(25); |
|
474 |
|
|
475 |
/* Accel offset range depends on the G-range: |
|
476 |
+/-2g = +/- 2000 mg |
|
477 |
+/-4g = +/- 4000 mg |
|
478 |
+/-8g = +/- 8000 mg |
|
479 |
+/-1§g = +/- 16000 mg */ |
|
480 |
offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR)); |
|
481 |
offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR)); |
|
482 |
offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR)); |
|
483 |
|
|
484 |
/* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */ |
|
485 |
offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR)); |
|
486 |
offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR)); |
|
487 |
offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR)); |
|
488 |
|
|
489 |
/* Gyro offset range depends on the DPS range: |
|
490 |
2000 dps = +/- 32000 LSB |
|
491 |
1000 dps = +/- 16000 LSB |
|
492 |
500 dps = +/- 8000 LSB |
|
493 |
250 dps = +/- 4000 LSB |
|
494 |
125 dps = +/- 2000 LSB |
|
495 |
... where 1 DPS = 16 LSB */ |
|
496 |
offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR)); |
|
497 |
offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR)); |
|
498 |
offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR)); |
|
499 |
|
|
500 |
/* Accelerometer radius = +/- 1000 LSB */ |
|
501 |
offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR)); |
|
502 |
|
|
503 |
/* Magnetometer radius = +/- 960 LSB */ |
|
504 |
offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR)); |
|
505 |
|
|
506 |
setMode(lastMode); |
|
507 |
return true; |
|
508 |
} |
|
509 |
return false; |
|
510 |
} |
|
478 |
bool Adafruit_BNO055::getSensorOffsets( |
|
479 |
adafruit_bno055_offsets_t &offsets_type) { |
|
480 |
if (isFullyCalibrated()) { |
|
481 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
482 |
setMode(OPERATION_MODE_CONFIG); |
|
483 |
delay(25); |
|
484 |
|
|
485 |
/* Accel offset range depends on the G-range: |
|
486 |
+/-2g = +/- 2000 mg |
|
487 |
+/-4g = +/- 4000 mg |
|
488 |
+/-8g = +/- 8000 mg |
|
489 |
+/-1§g = +/- 16000 mg */ |
|
490 |
offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | |
|
491 |
(read8(ACCEL_OFFSET_X_LSB_ADDR)); |
|
492 |
offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | |
|
493 |
(read8(ACCEL_OFFSET_Y_LSB_ADDR)); |
|
494 |
offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | |
|
495 |
(read8(ACCEL_OFFSET_Z_LSB_ADDR)); |
|
496 |
|
|
497 |
/* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */ |
|
498 |
offsets_type.mag_offset_x = |
|
499 |
(read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR)); |
|
500 |
offsets_type.mag_offset_y = |
|
501 |
(read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR)); |
|
502 |
offsets_type.mag_offset_z = |
|
503 |
(read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR)); |
|
504 |
|
|
505 |
/* Gyro offset range depends on the DPS range: |
|
506 |
2000 dps = +/- 32000 LSB |
|
507 |
1000 dps = +/- 16000 LSB |
|
508 |
500 dps = +/- 8000 LSB |
|
509 |
250 dps = +/- 4000 LSB |
|
510 |
125 dps = +/- 2000 LSB |
|
511 |
... where 1 DPS = 16 LSB */ |
|
512 |
offsets_type.gyro_offset_x = |
|
513 |
(read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR)); |
|
514 |
offsets_type.gyro_offset_y = |
|
515 |
(read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR)); |
|
516 |
offsets_type.gyro_offset_z = |
|
517 |
(read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR)); |
|
518 |
|
|
519 |
/* Accelerometer radius = +/- 1000 LSB */ |
|
520 |
offsets_type.accel_radius = |
|
521 |
(read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR)); |
|
522 |
|
|
523 |
/* Magnetometer radius = +/- 960 LSB */ |
|
524 |
offsets_type.mag_radius = |
|
525 |
(read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR)); |
|
511 | 526 |
|
527 |
setMode(lastMode); |
|
528 |
return true; |
|
529 |
} |
|
530 |
return false; |
|
531 |
} |
|
512 | 532 |
|
513 | 533 |
/**************************************************************************/ |
514 | 534 |
/*! |
515 |
@brief Writes an array of calibration values to the sensor's offset registers |
|
535 |
@brief Writes an array of calibration values to the sensor's offset registers |
|
536 |
@param calibData |
|
516 | 537 |
*/ |
517 | 538 |
/**************************************************************************/ |
518 |
void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData) |
|
519 |
{ |
|
520 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
521 |
setMode(OPERATION_MODE_CONFIG); |
|
522 |
delay(25); |
|
523 |
|
|
524 |
/* Note: Configuration will take place only when user writes to the last |
|
525 |
byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). |
|
526 |
Therefore the last byte must be written whenever the user wants to |
|
527 |
changes the configuration. */ |
|
528 |
|
|
529 |
/* A writeLen() would make this much cleaner */ |
|
530 |
write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]); |
|
531 |
write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]); |
|
532 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]); |
|
533 |
write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]); |
|
534 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]); |
|
535 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]); |
|
536 |
|
|
537 |
write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]); |
|
538 |
write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]); |
|
539 |
write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]); |
|
540 |
write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]); |
|
541 |
write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]); |
|
542 |
write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]); |
|
543 |
|
|
544 |
write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]); |
|
545 |
write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]); |
|
546 |
write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]); |
|
547 |
write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]); |
|
548 |
write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]); |
|
549 |
write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]); |
|
550 |
|
|
551 |
write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]); |
|
552 |
write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]); |
|
553 |
|
|
554 |
write8(MAG_RADIUS_LSB_ADDR, calibData[20]); |
|
555 |
write8(MAG_RADIUS_MSB_ADDR, calibData[21]); |
|
539 |
void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) { |
|
540 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
541 |
setMode(OPERATION_MODE_CONFIG); |
|
542 |
delay(25); |
|
556 | 543 |
|
557 |
setMode(lastMode); |
|
544 |
/* Note: Configuration will take place only when user writes to the last |
|
545 |
byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). |
|
546 |
Therefore the last byte must be written whenever the user wants to |
|
547 |
changes the configuration. */ |
|
548 |
|
|
549 |
/* A writeLen() would make this much cleaner */ |
|
550 |
write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]); |
|
551 |
write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]); |
|
552 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]); |
|
553 |
write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]); |
|
554 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]); |
|
555 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]); |
|
556 |
|
|
557 |
write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]); |
|
558 |
write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]); |
|
559 |
write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]); |
|
560 |
write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]); |
|
561 |
write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]); |
|
562 |
write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]); |
|
563 |
|
|
564 |
write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]); |
|
565 |
write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]); |
|
566 |
write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]); |
|
567 |
write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]); |
|
568 |
write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]); |
|
569 |
write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]); |
|
570 |
|
|
571 |
write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]); |
|
572 |
write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]); |
|
573 |
|
|
574 |
write8(MAG_RADIUS_LSB_ADDR, calibData[20]); |
|
575 |
write8(MAG_RADIUS_MSB_ADDR, calibData[21]); |
|
576 |
|
|
577 |
setMode(lastMode); |
|
558 | 578 |
} |
559 | 579 |
|
560 | 580 |
/**************************************************************************/ |
561 | 581 |
/*! |
562 | 582 |
@brief Writes to the sensor's offset registers from an offset struct |
583 |
@param offsets_type |
|
563 | 584 |
*/ |
564 | 585 |
/**************************************************************************/ |
565 |
void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) |
|
566 |
{ |
|
567 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
568 |
setMode(OPERATION_MODE_CONFIG); |
|
569 |
delay(25); |
|
570 |
|
|
571 |
/* Note: Configuration will take place only when user writes to the last |
|
572 |
byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). |
|
573 |
Therefore the last byte must be written whenever the user wants to |
|
574 |
changes the configuration. */ |
|
575 |
|
|
576 |
write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF); |
|
577 |
write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); |
|
578 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF); |
|
579 |
write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); |
|
580 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF); |
|
581 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); |
|
582 |
|
|
583 |
write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF); |
|
584 |
write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); |
|
585 |
write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF); |
|
586 |
write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); |
|
587 |
write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF); |
|
588 |
write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); |
|
589 |
|
|
590 |
write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF); |
|
591 |
write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); |
|
592 |
write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF); |
|
593 |
write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); |
|
594 |
write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF); |
|
595 |
write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); |
|
596 |
|
|
597 |
write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF); |
|
598 |
write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); |
|
599 |
|
|
600 |
write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF); |
|
601 |
write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); |
|
586 |
void Adafruit_BNO055::setSensorOffsets( |
|
587 |
const adafruit_bno055_offsets_t &offsets_type) { |
|
588 |
adafruit_bno055_opmode_t lastMode = _mode; |
|
589 |
setMode(OPERATION_MODE_CONFIG); |
|
590 |
delay(25); |
|
602 | 591 |
|
603 |
setMode(lastMode); |
|
592 |
/* Note: Configuration will take place only when user writes to the last |
|
593 |
byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). |
|
594 |
Therefore the last byte must be written whenever the user wants to |
|
595 |
changes the configuration. */ |
|
596 |
|
|
597 |
write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF); |
|
598 |
write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); |
|
599 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF); |
|
600 |
write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); |
|
601 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF); |
|
602 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); |
|
603 |
|
|
604 |
write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF); |
|
605 |
write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); |
|
606 |
write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF); |
|
607 |
write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); |
|
608 |
write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF); |
|
609 |
write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); |
|
610 |
|
|
611 |
write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF); |
|
612 |
write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); |
|
613 |
write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF); |
|
614 |
write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); |
|
615 |
write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF); |
|
616 |
write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); |
|
617 |
|
|
618 |
write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF); |
|
619 |
write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); |
|
620 |
|
|
621 |
write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF); |
|
622 |
write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); |
|
623 |
|
|
624 |
setMode(lastMode); |
|
604 | 625 |
} |
605 | 626 |
|
606 | 627 |
/**************************************************************************/ |
607 | 628 |
/*! |
608 | 629 |
@brief Checks of all cal status values are set to 3 (fully calibrated) |
630 |
@return status of calibration |
|
609 | 631 |
*/ |
610 | 632 |
/**************************************************************************/ |
611 |
bool Adafruit_BNO055::isFullyCalibrated(void) |
|
612 |
{ |
|
613 |
uint8_t system, gyro, accel, mag; |
|
614 |
getCalibration(&system, &gyro, &accel, &mag); |
|
615 |
if (system < 3 || gyro < 3 || accel < 3 || mag < 3) |
|
616 |
return false; |
|
617 |
return true; |
|
633 |
bool Adafruit_BNO055::isFullyCalibrated() { |
|
634 |
uint8_t system, gyro, accel, mag; |
|
635 |
getCalibration(&system, &gyro, &accel, &mag); |
|
636 |
if (system < 3 || gyro < 3 || accel < 3 || mag < 3) |
|
637 |
return false; |
|
638 |
return true; |
|
618 | 639 |
} |
619 | 640 |
|
620 |
|
|
621 | 641 |
/*************************************************************************** |
622 | 642 |
PRIVATE FUNCTIONS |
623 | 643 |
***************************************************************************/ |
... | ... | |
627 | 647 |
@brief Writes an 8 bit value over I2C |
628 | 648 |
*/ |
629 | 649 |
/**************************************************************************/ |
630 |
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) |
|
631 |
{ |
|
650 |
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) { |
|
632 | 651 |
Wire.beginTransmission(_address); |
633 |
#if ARDUINO >= 100
|
|
634 |
Wire.write((uint8_t)reg);
|
|
635 |
Wire.write((uint8_t)value);
|
|
636 |
#else
|
|
637 |
Wire.send(reg);
|
|
638 |
Wire.send(value);
|
|
639 |
#endif
|
|
652 |
#if ARDUINO >= 100 |
|
653 |
Wire.write((uint8_t)reg); |
|
654 |
Wire.write((uint8_t)value); |
|
655 |
#else |
|
656 |
Wire.send(reg); |
|
657 |
Wire.send(value); |
|
658 |
#endif |
|
640 | 659 |
Wire.endTransmission(); |
641 | 660 |
|
642 | 661 |
/* ToDo: Check for error! */ |
... | ... | |
648 | 667 |
@brief Reads an 8 bit value over I2C |
649 | 668 |
*/ |
650 | 669 |
/**************************************************************************/ |
651 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg ) |
|
652 |
{ |
|
670 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) { |
|
653 | 671 |
byte value = 0; |
654 | 672 |
|
655 | 673 |
Wire.beginTransmission(_address); |
656 |
#if ARDUINO >= 100
|
|
657 |
Wire.write((uint8_t)reg);
|
|
658 |
#else
|
|
659 |
Wire.send(reg);
|
|
660 |
#endif
|
|
674 |
#if ARDUINO >= 100 |
|
675 |
Wire.write((uint8_t)reg); |
|
676 |
#else |
|
677 |
Wire.send(reg); |
|
678 |
#endif |
|
661 | 679 |
Wire.endTransmission(); |
662 | 680 |
Wire.requestFrom(_address, (byte)1); |
663 |
#if ARDUINO >= 100
|
|
664 |
value = Wire.read();
|
|
665 |
#else
|
|
666 |
value = Wire.receive();
|
|
667 |
#endif
|
|
681 |
#if ARDUINO >= 100 |
|
682 |
value = Wire.read(); |
|
683 |
#else |
|
684 |
value = Wire.receive(); |
|
685 |
#endif |
|
668 | 686 |
|
669 | 687 |
return value; |
670 | 688 |
} |
... | ... | |
674 | 692 |
@brief Reads the specified number of bytes over I2C |
675 | 693 |
*/ |
676 | 694 |
/**************************************************************************/ |
677 |
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
|
|
678 |
{ |
|
695 |
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte *buffer,
|
|
696 |
uint8_t len) {
|
|
679 | 697 |
Wire.beginTransmission(_address); |
680 |
#if ARDUINO >= 100
|
|
681 |
Wire.write((uint8_t)reg);
|
|
682 |
#else
|
|
683 |
Wire.send(reg);
|
|
684 |
#endif
|
|
698 |
#if ARDUINO >= 100 |
|
699 |
Wire.write((uint8_t)reg); |
|
700 |
#else |
|
701 |
Wire.send(reg); |
|
702 |
#endif |
|
685 | 703 |
Wire.endTransmission(); |
686 | 704 |
Wire.requestFrom(_address, (byte)len); |
687 | 705 |
|
688 |
for (uint8_t i = 0; i < len; i++) |
|
689 |
{ |
|
690 |
#if ARDUINO >= 100 |
|
691 |
buffer[i] = Wire.read(); |
|
692 |
#else |
|
693 |
buffer[i] = Wire.receive(); |
|
694 |
#endif |
|
706 |
for (uint8_t i = 0; i < len; i++) { |
|
707 |
#if ARDUINO >= 100 |
|
708 |
buffer[i] = Wire.read(); |
|
709 |
#else |
|
710 |
buffer[i] = Wire.receive(); |
|
711 |
#endif |
|
695 | 712 |
} |
696 | 713 |
|
697 | 714 |
/* ToDo: Check for errors! */ |
Also available in: Unified diff