| 472 |
472 |
setMode(OPERATION_MODE_CONFIG);
|
| 473 |
473 |
delay(25);
|
| 474 |
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 */
|
| 475 |
480 |
offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
|
| 476 |
481 |
offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
|
| 477 |
482 |
offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
|
| 478 |
483 |
|
| 479 |
|
offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
|
| 480 |
|
offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
|
| 481 |
|
offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
|
| 482 |
|
|
|
484 |
/* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
|
| 483 |
485 |
offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
|
| 484 |
486 |
offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
|
| 485 |
487 |
offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
|
| 486 |
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 */
|
| 487 |
501 |
offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
|
|
502 |
|
|
503 |
/* Magnetometer radius = +/- 960 LSB */
|
| 488 |
504 |
offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
|
| 489 |
505 |
|
| 490 |
506 |
setMode(lastMode);
|
| ... | ... | |
| 505 |
521 |
setMode(OPERATION_MODE_CONFIG);
|
| 506 |
522 |
delay(25);
|
| 507 |
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 |
|
| 508 |
529 |
/* A writeLen() would make this much cleaner */
|
| 509 |
530 |
write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
|
| 510 |
531 |
write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
|
| ... | ... | |
| 547 |
568 |
setMode(OPERATION_MODE_CONFIG);
|
| 548 |
569 |
delay(25);
|
| 549 |
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 |
|
| 550 |
576 |
write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
|
| 551 |
577 |
write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
|
| 552 |
578 |
write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
|
| ... | ... | |
| 554 |
580 |
write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
|
| 555 |
581 |
write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
|
| 556 |
582 |
|
| 557 |
|
write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
|
| 558 |
|
write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
|
| 559 |
|
write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
|
| 560 |
|
write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
|
| 561 |
|
write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
|
| 562 |
|
write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
|
| 563 |
|
|
| 564 |
583 |
write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
|
| 565 |
584 |
write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
|
| 566 |
585 |
write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
|
| ... | ... | |
| 568 |
587 |
write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
|
| 569 |
588 |
write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
|
| 570 |
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 |
|
| 571 |
597 |
write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
|
| 572 |
598 |
write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
|
| 573 |
599 |
|
| ... | ... | |
| 577 |
603 |
setMode(lastMode);
|
| 578 |
604 |
}
|
| 579 |
605 |
|
|
606 |
/**************************************************************************/
|
|
607 |
/*!
|
|
608 |
@brief Checks of all cal status values are set to 3 (fully calibrated)
|
|
609 |
*/
|
|
610 |
/**************************************************************************/
|
| 580 |
611 |
bool Adafruit_BNO055::isFullyCalibrated(void)
|
| 581 |
612 |
{
|
| 582 |
613 |
uint8_t system, gyro, accel, mag;
|