1 package org
.usfirst
.frc
.team3501
.robot
.utils
;
3 import java
.util
.TimerTask
;
5 import edu
.wpi
.first
.wpilibj
.I2C
;
6 import edu
.wpi
.first
.wpilibj
.Timer
;
9 * BNO055 IMU for the FIRST Robotics Competition.
10 * References throughout the code are to the following sensor documentation:
13 * To use the sensor, wire up to it over I2C on the roboRIO.
14 * Creating an instance of this class will cause communications with the sensor
15 * to being.All communications with the sensor occur in a separate thread
16 * from your robot code to avoid blocking the main robot program execution.
19 * private static BNO055 imu;
22 * imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
23 * BNO055.vector_type_t.VECTOR_EULER);
26 * You can check the status of the sensor by using the following methods:
27 * isSensorPresent(); //Checks if the code can talk to the sensor over I2C
28 * // If this returns false, check your wiring.
29 * isInitialized(); //Checks if the sensor initialization has completed.
30 * // Initialization takes about 3 seconds. You won't get
31 * // position data back from the sensor until its init'd.
32 * isCalibrated(); //The BNO055 will return position data after its init'd,
33 * // but the position data may be inaccurate until all
34 * // required sensors report they are calibrated. Some
35 * // Calibration sequences require you to move the BNO055
36 * // around. See the method comments for more info.
38 * Once the sensor calibration is complete , you can get position data by
39 * by using the getVector() method. See this method definiton for usage info.
41 * This code was originally ported from arduino source developed by Adafruit.
42 * See the original comment header below.
44 * @author james@team2168.org
47 * ORIGINAL ADAFRUIT HEADER -
48 * https://github.com/adafruit/Adafruit_BNO055/
49 * =======================================================================
50 * This is a library for the BNO055 orientation sensor
52 * Designed specifically to work with the Adafruit BNO055 Breakout.
54 * Pick one up today in the adafruit shop!
55 * ------> http://www.adafruit.com/products
57 * These sensors use I2C to communicate, 2 pins are required to
60 * Adafruit invests time and resources providing this open source code,
61 * please support Adafruit and open-source hardware by purchasing
65 * Written by KTOWN for Adafruit Industries.
67 * MIT license, all text above must be included in any redistribution
72 private java
.util
.Timer executor
;
73 private static final long THREAD_PERIOD
= 20; // ms - max poll rate on sensor.
75 public static final byte BNO055_ADDRESS_A
= 0x28;
76 public static final byte BNO055_ADDRESS_B
= 0x29;
77 public static final int BNO055_ID
= 0xA0;
79 private static BNO055 instance
;
81 private static I2C imu
;
82 private static int _mode
;
83 private static opmode_t requestedMode
; // user requested mode of operation.
84 private static vector_type_t requestedVectorType
;
86 // State machine variables
87 private volatile int state
= 0;
88 private volatile boolean sensorPresent
= false;
89 private volatile boolean initialized
= false;
90 private volatile double currentTime
; // seconds
91 private volatile double nextTime
; // seconds
92 private volatile byte[] positionVector
= new byte[6];
93 private volatile long turns
= 0;
94 private volatile double[] xyz
= new double[3];
96 private double zeroReferenceConst
= 0;
98 public class SystemStatus
{
99 public int system_status
;
100 public int self_test_result
;
101 public int system_error
;
105 /* Page id register definition */
106 BNO055_PAGE_ID_ADDR(0X07),
108 /* PAGE0 REGISTER DEFINITION START */
109 BNO055_CHIP_ID_ADDR(0x00), BNO055_ACCEL_REV_ID_ADDR(
110 0x01), BNO055_MAG_REV_ID_ADDR(0x02), BNO055_GYRO_REV_ID_ADDR(
111 0x03), BNO055_SW_REV_ID_LSB_ADDR(0x04), BNO055_SW_REV_ID_MSB_ADDR(
112 0x05), BNO055_BL_REV_ID_ADDR(0X06),
114 /* Accel data register */
115 BNO055_ACCEL_DATA_X_LSB_ADDR(0X08), BNO055_ACCEL_DATA_X_MSB_ADDR(
116 0X09), BNO055_ACCEL_DATA_Y_LSB_ADDR(0X0A), BNO055_ACCEL_DATA_Y_MSB_ADDR(
117 0X0B), BNO055_ACCEL_DATA_Z_LSB_ADDR(
118 0X0C), BNO055_ACCEL_DATA_Z_MSB_ADDR(0X0D),
120 /* Mag data register */
121 BNO055_MAG_DATA_X_LSB_ADDR(0X0E), BNO055_MAG_DATA_X_MSB_ADDR(
122 0X0F), BNO055_MAG_DATA_Y_LSB_ADDR(0X10), BNO055_MAG_DATA_Y_MSB_ADDR(
123 0X11), BNO055_MAG_DATA_Z_LSB_ADDR(
124 0X12), BNO055_MAG_DATA_Z_MSB_ADDR(0X13),
126 /* Gyro data registers */
127 BNO055_GYRO_DATA_X_LSB_ADDR(0X14), BNO055_GYRO_DATA_X_MSB_ADDR(
128 0X15), BNO055_GYRO_DATA_Y_LSB_ADDR(0X16), BNO055_GYRO_DATA_Y_MSB_ADDR(
129 0X17), BNO055_GYRO_DATA_Z_LSB_ADDR(
130 0X18), BNO055_GYRO_DATA_Z_MSB_ADDR(0X19),
132 /* Euler data registers */
133 BNO055_EULER_H_LSB_ADDR(0X1A), BNO055_EULER_H_MSB_ADDR(
134 0X1B), BNO055_EULER_R_LSB_ADDR(0X1C), BNO055_EULER_R_MSB_ADDR(
135 0X1D), BNO055_EULER_P_LSB_ADDR(0X1E), BNO055_EULER_P_MSB_ADDR(0X1F),
137 /* Quaternion data registers */
138 BNO055_QUATERNION_DATA_W_LSB_ADDR(0X20), BNO055_QUATERNION_DATA_W_MSB_ADDR(
139 0X21), BNO055_QUATERNION_DATA_X_LSB_ADDR(
140 0X22), BNO055_QUATERNION_DATA_X_MSB_ADDR(
141 0X23), BNO055_QUATERNION_DATA_Y_LSB_ADDR(
142 0X24), BNO055_QUATERNION_DATA_Y_MSB_ADDR(
143 0X25), BNO055_QUATERNION_DATA_Z_LSB_ADDR(
144 0X26), BNO055_QUATERNION_DATA_Z_MSB_ADDR(0X27),
146 /* Linear acceleration data registers */
147 BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR(
148 0X28), BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR(
149 0X29), BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR(
150 0X2A), BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR(
151 0X2B), BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR(
152 0X2C), BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR(0X2D),
154 /* Gravity data registers */
155 BNO055_GRAVITY_DATA_X_LSB_ADDR(0X2E), BNO055_GRAVITY_DATA_X_MSB_ADDR(
156 0X2F), BNO055_GRAVITY_DATA_Y_LSB_ADDR(
157 0X30), BNO055_GRAVITY_DATA_Y_MSB_ADDR(
158 0X31), BNO055_GRAVITY_DATA_Z_LSB_ADDR(
159 0X32), BNO055_GRAVITY_DATA_Z_MSB_ADDR(0X33),
161 /* Temperature data register */
162 BNO055_TEMP_ADDR(0X34),
164 /* Status registers */
165 BNO055_CALIB_STAT_ADDR(0X35), BNO055_SELFTEST_RESULT_ADDR(
166 0X36), BNO055_INTR_STAT_ADDR(0X37),
168 BNO055_SYS_CLK_STAT_ADDR(0X38), BNO055_SYS_STAT_ADDR(
169 0X39), BNO055_SYS_ERR_ADDR(0X3A),
171 /* Unit selection register */
172 BNO055_UNIT_SEL_ADDR(0X3B), BNO055_DATA_SELECT_ADDR(0X3C),
175 BNO055_OPR_MODE_ADDR(0X3D), BNO055_PWR_MODE_ADDR(0X3E),
177 BNO055_SYS_TRIGGER_ADDR(0X3F), BNO055_TEMP_SOURCE_ADDR(0X40),
179 /* Axis remap registers */
180 BNO055_AXIS_MAP_CONFIG_ADDR(0X41), BNO055_AXIS_MAP_SIGN_ADDR(0X42),
183 BNO055_SIC_MATRIX_0_LSB_ADDR(0X43), BNO055_SIC_MATRIX_0_MSB_ADDR(
184 0X44), BNO055_SIC_MATRIX_1_LSB_ADDR(0X45), BNO055_SIC_MATRIX_1_MSB_ADDR(
185 0X46), BNO055_SIC_MATRIX_2_LSB_ADDR(
186 0X47), BNO055_SIC_MATRIX_2_MSB_ADDR(
187 0X48), BNO055_SIC_MATRIX_3_LSB_ADDR(
188 0X49), BNO055_SIC_MATRIX_3_MSB_ADDR(
189 0X4A), BNO055_SIC_MATRIX_4_LSB_ADDR(
190 0X4B), BNO055_SIC_MATRIX_4_MSB_ADDR(
191 0X4C), BNO055_SIC_MATRIX_5_LSB_ADDR(
192 0X4D), BNO055_SIC_MATRIX_5_MSB_ADDR(
193 0X4E), BNO055_SIC_MATRIX_6_LSB_ADDR(
194 0X4F), BNO055_SIC_MATRIX_6_MSB_ADDR(
195 0X50), BNO055_SIC_MATRIX_7_LSB_ADDR(
196 0X51), BNO055_SIC_MATRIX_7_MSB_ADDR(
197 0X52), BNO055_SIC_MATRIX_8_LSB_ADDR(
198 0X53), BNO055_SIC_MATRIX_8_MSB_ADDR(
201 /* Accelerometer Offset registers */
202 ACCEL_OFFSET_X_LSB_ADDR(0X55), ACCEL_OFFSET_X_MSB_ADDR(
203 0X56), ACCEL_OFFSET_Y_LSB_ADDR(0X57), ACCEL_OFFSET_Y_MSB_ADDR(
204 0X58), ACCEL_OFFSET_Z_LSB_ADDR(0X59), ACCEL_OFFSET_Z_MSB_ADDR(0X5A),
206 /* Magnetometer Offset registers */
207 MAG_OFFSET_X_LSB_ADDR(0X5B), MAG_OFFSET_X_MSB_ADDR(
208 0X5C), MAG_OFFSET_Y_LSB_ADDR(0X5D), MAG_OFFSET_Y_MSB_ADDR(
209 0X5E), MAG_OFFSET_Z_LSB_ADDR(0X5F), MAG_OFFSET_Z_MSB_ADDR(0X60),
211 /* Gyroscope Offset register s */
212 GYRO_OFFSET_X_LSB_ADDR(0X61), GYRO_OFFSET_X_MSB_ADDR(
213 0X62), GYRO_OFFSET_Y_LSB_ADDR(0X63), GYRO_OFFSET_Y_MSB_ADDR(
214 0X64), GYRO_OFFSET_Z_LSB_ADDR(0X65), GYRO_OFFSET_Z_MSB_ADDR(0X66),
216 /* Radius registers */
217 ACCEL_RADIUS_LSB_ADDR(0X67), ACCEL_RADIUS_MSB_ADDR(
218 0X68), MAG_RADIUS_LSB_ADDR(0X69), MAG_RADIUS_MSB_ADDR(0X6A);
220 private final int val
;
226 public int getVal() {
231 public enum powermode_t
{
232 POWER_MODE_NORMAL(0X00), POWER_MODE_LOWPOWER(0X01), POWER_MODE_SUSPEND(
235 private final int val
;
237 powermode_t(int val
) {
241 public int getVal() {
246 public enum opmode_t
{
247 /* Operation mode settings */
248 OPERATION_MODE_CONFIG(0X00), OPERATION_MODE_ACCONLY(
249 0X01), OPERATION_MODE_MAGONLY(0X02), OPERATION_MODE_GYRONLY(
250 0X03), OPERATION_MODE_ACCMAG(0X04), OPERATION_MODE_ACCGYRO(
251 0X05), OPERATION_MODE_MAGGYRO(
252 0X06), OPERATION_MODE_AMG(0X07), OPERATION_MODE_IMUPLUS(
253 0X08), OPERATION_MODE_COMPASS(0X09), OPERATION_MODE_M4G(
254 0X0A), OPERATION_MODE_NDOF_FMC_OFF(
255 0X0B), OPERATION_MODE_NDOF(0X0C);
257 private final int val
;
263 public int getVal() {
268 public class RevInfo
{
269 public byte accel_rev
;
271 public byte gyro_rev
;
276 public class CalData
{
283 public enum vector_type_t
{
284 VECTOR_ACCELEROMETER(
285 reg_t
.BNO055_ACCEL_DATA_X_LSB_ADDR
.getVal()), VECTOR_MAGNETOMETER(
286 reg_t
.BNO055_MAG_DATA_X_LSB_ADDR
.getVal()), VECTOR_GYROSCOPE(
287 reg_t
.BNO055_GYRO_DATA_X_LSB_ADDR
.getVal()), VECTOR_EULER(
288 reg_t
.BNO055_EULER_H_LSB_ADDR
.getVal()), VECTOR_LINEARACCEL(
289 reg_t
.BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR
290 .getVal()), VECTOR_GRAVITY(
291 reg_t
.BNO055_GRAVITY_DATA_X_LSB_ADDR
.getVal());
293 private final int val
;
295 vector_type_t(int val
) {
299 public int getVal() {
305 * Instantiates a new BNO055 class.
308 * the physical port the sensor is plugged into on the roboRio
310 * the address the sensor is at (0x28 or 0x29)
312 private BNO055(I2C
.Port port
, byte address
) {
313 imu
= new I2C(port
, address
);
315 executor
= new java
.util
.Timer();
316 executor
.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD
);
320 * Get an instance of the IMU object.
323 * the operating mode to run the sensor in.
325 * the physical port the sensor is plugged into on the roboRio
327 * the address the sensor is at (0x28 or 0x29)
328 * @return the instantiated BNO055 object
330 public static BNO055
getInstance(opmode_t mode
, vector_type_t vectorType
,
331 I2C
.Port port
, byte address
) {
333 if (instance
== null) {
335 instance
= new BNO055(port
, address
);
338 requestedMode
= mode
;
339 requestedVectorType
= vectorType
;
344 * Get an instance of the IMU object plugged into the onboard I2C header.
345 * Using the default address (0x28)
348 * the operating mode to run the sensor in.
350 * the format the position vector data should be returned
351 * in (if you don't know use VECTOR_EULER).
352 * @return the instantiated BNO055 object
354 public static BNO055
getInstance(opmode_t mode
, vector_type_t vectorType
) {
355 return getInstance(mode
, vectorType
, I2C
.Port
.kOnboard
,
360 * Called periodically. Communicates with the sensor, and checks its state.
362 private void update() {
363 currentTime
= Timer
.getFPGATimestamp(); // seconds
365 // System.out.println("State: " + state + ". curr: " + currentTime
366 // + ", next: " + nextTime);
368 // Step through process of initializing the sensor in a non-
369 // blocking manner. This sequence of events follows the process
370 // defined in the original adafruit source as closely as possible.
371 // XXX: It's likely some of these delays can be optimized out.
374 // Wait for the sensor to be present
375 if ((0xFF & read8(reg_t
.BNO055_CHIP_ID_ADDR
)) != BNO055_ID
) {
376 // Sensor not present, keep trying
377 sensorPresent
= false;
379 // Sensor present, go to next state
380 sensorPresent
= true;
382 nextTime
= Timer
.getFPGATimestamp() + 0.050;
386 if (currentTime
>= nextTime
) {
387 // Switch to config mode (just in case since this is the default)
388 setMode(opmode_t
.OPERATION_MODE_CONFIG
.getVal());
389 nextTime
= Timer
.getFPGATimestamp() + 0.050;
395 if (currentTime
>= nextTime
) {
396 write8(reg_t
.BNO055_SYS_TRIGGER_ADDR
, (byte) 0x20);
401 // Wait for the sensor to be present
402 if ((0xFF & read8(reg_t
.BNO055_CHIP_ID_ADDR
)) == BNO055_ID
) {
403 // Sensor present, go to next state
406 nextTime
= Timer
.getFPGATimestamp() + 0.050;
410 // Wait at least 50ms
411 if (currentTime
>= nextTime
) {
412 /* Set to normal power mode */
413 write8(reg_t
.BNO055_PWR_MODE_ADDR
,
414 (byte) powermode_t
.POWER_MODE_NORMAL
.getVal());
415 nextTime
= Timer
.getFPGATimestamp() + 0.050;
420 // Use external crystal - 32.768 kHz
421 if (currentTime
>= nextTime
) {
422 write8(reg_t
.BNO055_PAGE_ID_ADDR
, (byte) 0x00);
423 nextTime
= Timer
.getFPGATimestamp() + 0.050;
428 if (currentTime
>= nextTime
) {
429 write8(reg_t
.BNO055_SYS_TRIGGER_ADDR
, (byte) 0x80);
430 nextTime
= Timer
.getFPGATimestamp() + 0.500;
435 // Set operating mode to mode requested at instantiation
436 if (currentTime
>= nextTime
) {
437 setMode(requestedMode
);
438 nextTime
= Timer
.getFPGATimestamp() + 1.05;
443 if (currentTime
>= nextTime
) {
444 nextTime
= Timer
.getFPGATimestamp() + 1.05;
448 if (currentTime
>= nextTime
) {
450 zeroReferenceConst
= getRawHeading();
455 // Should never get here - Fail safe
459 // Sensor is initialized, periodically query position data
465 * Query the sensor for position data.
467 private void calculateVector() {
468 double[] pos
= new double[3];
469 short x
= 0, y
= 0, z
= 0;
470 double headingDiff
= 0.0;
472 // Read vector data (6 bytes)
473 readLen(requestedVectorType
.getVal(), positionVector
);
475 x
= (short) ((positionVector
[0] & 0xFF)
476 | ((positionVector
[1] << 8) & 0xFF00));
477 y
= (short) ((positionVector
[2] & 0xFF)
478 | ((positionVector
[3] << 8) & 0xFF00));
479 z
= (short) ((positionVector
[4] & 0xFF)
480 | ((positionVector
[5] << 8) & 0xFF00));
482 /* Convert the value to an appropriate range (section 3.6.4) */
483 /* and assign the value to the Vector type */
484 switch (requestedVectorType
) {
485 case VECTOR_MAGNETOMETER
:
491 case VECTOR_GYROSCOPE
:
493 pos
[0] = (x
) / 900.0;
494 pos
[1] = (y
) / 900.0;
495 pos
[2] = (z
) / 900.0;
498 /* 1 degree = 16 LSB */
503 case VECTOR_ACCELEROMETER
:
504 case VECTOR_LINEARACCEL
:
506 /* 1m/s^2 = 100 LSB */
507 pos
[0] = (x
) / 100.0;
508 pos
[1] = (y
) / 100.0;
509 pos
[2] = (z
) / 100.0;
514 headingDiff
= xyz
[0] - pos
[0];
515 if (Math
.abs(headingDiff
) >= 350) {
516 // We've traveled past the zero heading position
517 if (headingDiff
> 0) {
524 // Update position vectors
529 * Puts the chip in the specified operating mode
533 public void setMode(opmode_t mode
) {
534 setMode(mode
.getVal());
537 private void setMode(int mode
) {
539 write8(reg_t
.BNO055_OPR_MODE_ADDR
, (byte) _mode
);
543 * Gets the latest system status info
547 public SystemStatus
getSystemStatus() {
548 SystemStatus status
= new SystemStatus();
550 write8(reg_t
.BNO055_PAGE_ID_ADDR
, (byte) 0x00);
553 * System Status (see section 4.3.58)
554 * ---------------------------------
557 * 2 = Initializing Peripherals
558 * 3 = System Initalization
559 * 4 = Executing Self-Test
560 * 5 = Sensor fusion algorithm running
561 * 6 = System running without fusion algorithms
564 status
.system_status
= read8(reg_t
.BNO055_SYS_STAT_ADDR
);
567 * Self Test Results (see section )
568 * --------------------------------
569 * 1 = test passed, 0 = test failed
571 * Bit 0 = Accelerometer self test
572 * Bit 1 = Magnetometer self test
573 * Bit 2 = Gyroscope self test
574 * Bit 3 = MCU self test
579 status
.self_test_result
= read8(reg_t
.BNO055_SELFTEST_RESULT_ADDR
);
582 * System Error (see section 4.3.59)
583 * ---------------------------------
585 * 1 = Peripheral initialization error
586 * 2 = System initialization error
587 * 3 = Self test result failed
588 * 4 = Register map value out of range
589 * 5 = Register map address out of range
590 * 6 = Register map write error
591 * 7 = BNO low power mode not available for selected operation mode
592 * 8 = Accelerometer power mode not available
593 * 9 = Fusion algorithm configuration error
594 * A = Sensor configuration error
596 status
.system_error
= read8(reg_t
.BNO055_SYS_ERR_ADDR
);
601 * Gets the chip revision numbers
603 * @return the chips revision information
605 public RevInfo
getRevInfo() {
607 RevInfo info
= new RevInfo();
609 /* Check the accelerometer revision */
610 info
.accel_rev
= read8(reg_t
.BNO055_ACCEL_REV_ID_ADDR
);
612 /* Check the magnetometer revision */
613 info
.mag_rev
= read8(reg_t
.BNO055_MAG_REV_ID_ADDR
);
615 /* Check the gyroscope revision */
616 info
.gyro_rev
= read8(reg_t
.BNO055_GYRO_REV_ID_ADDR
);
618 /* Check the SW revision */
619 info
.bl_rev
= read8(reg_t
.BNO055_BL_REV_ID_ADDR
);
621 a
= read8(reg_t
.BNO055_SW_REV_ID_LSB_ADDR
);
622 b
= read8(reg_t
.BNO055_SW_REV_ID_MSB_ADDR
);
623 info
.sw_rev
= (short) ((b
<< 8) | a
);
629 * Diagnostic method to determine if communications with the sensor are
631 * Note this method returns true after first establishing communications
633 * Communications are not actively monitored once sensor initialization
636 * @return true if the sensor is found on the I2C bus
638 public boolean isSensorPresent() {
639 return sensorPresent
;
643 * After power is applied, the sensor needs to be configured for use.
644 * During this initialization period the sensor will not return position
645 * vector data. Once initialization is complete, data can be read,
646 * although the sensor may not have completed calibration.
649 * @return true when the sensor is initialized.
651 public boolean isInitialized() {
656 * Gets current calibration state.
658 * @return each value will be set to 0 if not calibrated, 3 if fully
661 public CalData
getCalibration() {
662 CalData data
= new CalData();
663 int rawCalData
= read8(reg_t
.BNO055_CALIB_STAT_ADDR
);
665 data
.sys
= (byte) ((rawCalData
>> 6) & 0x03);
666 data
.gyro
= (byte) ((rawCalData
>> 4) & 0x03);
667 data
.accel
= (byte) ((rawCalData
>> 2) & 0x03);
668 data
.mag
= (byte) (rawCalData
& 0x03);
674 * Returns true if all required sensors (accelerometer, magnetometer,
675 * gyroscope) have completed their respective calibration sequence.
676 * Only sensors required by the current operating mode are checked.
679 * @return true if calibration is complete for all sensors required for the
680 * mode the sensor is currently operating in.
682 public boolean isCalibrated() {
683 boolean retVal
= true;
686 boolean[][] sensorModeMap
= new boolean[][] {
687 // {accel, mag, gyro}
688 { false, false, false }, // OPERATION_MODE_CONFIG
689 { true, false, false }, // OPERATION_MODE_ACCONLY
690 { false, true, false }, // OPERATION_MODE_MAGONLY
691 { false, false, true }, // OPERATION_MODE_GYRONLY
692 { true, true, false }, // OPERATION_MODE_ACCMAG
693 { true, false, true }, // OPERATION_MODE_ACCGYRO
694 { false, true, true }, // OPERATION_MODE_MAGGYRO
695 { true, true, true }, // OPERATION_MODE_AMG
696 { true, false, true }, // OPERATION_MODE_IMUPLUS
697 { true, true, false }, // OPERATION_MODE_COMPASS
698 { true, true, false }, // OPERATION_MODE_M4G
699 { true, true, true }, // OPERATION_MODE_NDOF_FMC_OFF
700 { true, true, true } // OPERATION_MODE_NDOF
703 CalData data
= getCalibration();
705 if (sensorModeMap
[_mode
][0]) // Accelerometer used
706 retVal
= retVal
&& (data
.accel
>= 3);
707 if (sensorModeMap
[_mode
][1]) // Magnetometer used
708 retVal
= retVal
&& (data
.mag
>= 3);
709 if (sensorModeMap
[_mode
][2]) // Gyroscope used
710 retVal
= retVal
&& (data
.gyro
>= 3);
716 * Get the sensors internal temperature.
718 * @return temperature in degrees celsius.
720 public int getTemp() {
721 return (read8(reg_t
.BNO055_TEMP_ADDR
));
725 * Gets a vector representing the sensors position (heading, roll, pitch).
726 * heading: 0 to 360 degrees
727 * roll: -90 to +90 degrees
728 * pitch: -180 to +180 degrees
730 * For continuous rotation heading (doesn't roll over between 360/0) see
731 * the getHeading() method.
733 * Maximum data output rates for Fusion modes - See 3.6.3
735 * Operating Mode Data Output Rate
739 * NDOF_FMC_OFF 100 Hz
742 * @return a vector [heading, roll, pitch]
744 public double[] getVector() {
749 * Heading not relative to the starting angle of the robot.
753 public double getRawHeading() {
754 return xyz
[0] + turns
* 360;
758 * The heading of the sensor (x axis) in continuous format. Eg rotating the
759 * sensor clockwise two full rotations will return a value of 720 degrees.
760 * The getVector method will return heading in a constrained 0 - 360 deg
761 * format if required.
763 * @return heading in degrees
765 public double getHeading() {
766 double reading
= getRawHeading();
768 return reading
- zeroReferenceConst
;
772 * Writes an 8 bit value over I2C
775 * the register to write the data to
777 * a byte of data to write
778 * @return whatever I2CJNI.i2CWrite returns. It's not documented in the wpilib
781 private boolean write8(reg_t reg
, byte value
) {
782 boolean retVal
= false;
784 retVal
= imu
.write(reg
.getVal(), value
);
790 * Reads an 8 bit value over I2C
793 * the register to read from.
796 private byte read8(reg_t reg
) {
797 byte[] vals
= new byte[1];
804 * Reads the specified number of bytes over I2C
807 * the address to read from
809 * to store the read data into
810 * @return true on success
812 private boolean readLen(reg_t reg
, byte[] buffer
) {
813 return readLen(reg
.getVal(), buffer
);
817 * Reads the specified number of bytes over I2C
820 * the address to read from
822 * the size of the data to read
823 * @return true on success
825 private boolean readLen(int reg
, byte[] buffer
) {
826 boolean retVal
= true;
828 if (buffer
== null || buffer
.length
< 1) {
832 retVal
= !imu
.read(reg
, buffer
.length
, buffer
);
837 private class BNO055UpdateTask
extends TimerTask
{
840 private BNO055UpdateTask(BNO055 imu
) {
842 throw new NullPointerException("BNO055 pointer null");
848 * Called periodically in its own thread