move setting of zeroReferenceConst into the initialization switch statement
[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / utils / BNO055.java
1 package org.usfirst.frc.team3501.robot.utils;
2
3 import java.util.TimerTask;
4
5 import edu.wpi.first.wpilibj.I2C;
6 import edu.wpi.first.wpilibj.Timer;
7
8 /**
9 * BNO055 IMU for the FIRST Robotics Competition.
10 * References throughout the code are to the following sensor documentation:
11 * http://git.io/vuOl1
12 *
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.
17 *
18 * Example:
19 * private static BNO055 imu;
20 *
21 * public Robot() {
22 * imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
23 * BNO055.vector_type_t.VECTOR_EULER);
24 * }
25 *
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.
37 *
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.
40 *
41 * This code was originally ported from arduino source developed by Adafruit.
42 * See the original comment header below.
43 *
44 * @author james@team2168.org
45 *
46 *
47 * ORIGINAL ADAFRUIT HEADER -
48 * https://github.com/adafruit/Adafruit_BNO055/
49 * =======================================================================
50 * This is a library for the BNO055 orientation sensor
51 *
52 * Designed specifically to work with the Adafruit BNO055 Breakout.
53 *
54 * Pick one up today in the adafruit shop!
55 * ------> http://www.adafruit.com/products
56 *
57 * These sensors use I2C to communicate, 2 pins are required to
58 * interface.
59 *
60 * Adafruit invests time and resources providing this open source code,
61 * please support Adafruit and open-source hardware by purchasing
62 * products
63 * from Adafruit!
64 *
65 * Written by KTOWN for Adafruit Industries.
66 *
67 * MIT license, all text above must be included in any redistribution
68 *
69 */
70 public class BNO055 {
71 // Tread variables
72 private java.util.Timer executor;
73 private static final long THREAD_PERIOD = 20; // ms - max poll rate on sensor.
74
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;
78
79 private static BNO055 instance;
80
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;
85
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];
95
96 private double zeroReferenceConst = 0;
97
98 public class SystemStatus {
99 public int system_status;
100 public int self_test_result;
101 public int system_error;
102 }
103
104 public enum reg_t {
105 /* Page id register definition */
106 BNO055_PAGE_ID_ADDR(0X07),
107
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),
113
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),
119
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),
125
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),
131
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),
136
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),
145
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),
153
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),
160
161 /* Temperature data register */
162 BNO055_TEMP_ADDR(0X34),
163
164 /* Status registers */
165 BNO055_CALIB_STAT_ADDR(0X35), BNO055_SELFTEST_RESULT_ADDR(
166 0X36), BNO055_INTR_STAT_ADDR(0X37),
167
168 BNO055_SYS_CLK_STAT_ADDR(0X38), BNO055_SYS_STAT_ADDR(
169 0X39), BNO055_SYS_ERR_ADDR(0X3A),
170
171 /* Unit selection register */
172 BNO055_UNIT_SEL_ADDR(0X3B), BNO055_DATA_SELECT_ADDR(0X3C),
173
174 /* Mode registers */
175 BNO055_OPR_MODE_ADDR(0X3D), BNO055_PWR_MODE_ADDR(0X3E),
176
177 BNO055_SYS_TRIGGER_ADDR(0X3F), BNO055_TEMP_SOURCE_ADDR(0X40),
178
179 /* Axis remap registers */
180 BNO055_AXIS_MAP_CONFIG_ADDR(0X41), BNO055_AXIS_MAP_SIGN_ADDR(0X42),
181
182 /* SIC registers */
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(
199 0X54),
200
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),
205
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),
210
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),
215
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);
219
220 private final int val;
221
222 reg_t(int val) {
223 this.val = val;
224 }
225
226 public int getVal() {
227 return val;
228 }
229 };
230
231 public enum powermode_t {
232 POWER_MODE_NORMAL(0X00), POWER_MODE_LOWPOWER(0X01), POWER_MODE_SUSPEND(
233 0X02);
234
235 private final int val;
236
237 powermode_t(int val) {
238 this.val = val;
239 }
240
241 public int getVal() {
242 return val;
243 }
244 };
245
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);
256
257 private final int val;
258
259 opmode_t(int val) {
260 this.val = val;
261 }
262
263 public int getVal() {
264 return val;
265 }
266 }
267
268 public class RevInfo {
269 public byte accel_rev;
270 public byte mag_rev;
271 public byte gyro_rev;
272 public short sw_rev;
273 public byte bl_rev;
274 }
275
276 public class CalData {
277 public byte sys;
278 public byte gyro;
279 public byte accel;
280 public byte mag;
281 }
282
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());
292
293 private final int val;
294
295 vector_type_t(int val) {
296 this.val = val;
297 }
298
299 public int getVal() {
300 return val;
301 }
302 };
303
304 /**
305 * Instantiates a new BNO055 class.
306 *
307 * @param port
308 * the physical port the sensor is plugged into on the roboRio
309 * @param address
310 * the address the sensor is at (0x28 or 0x29)
311 */
312 private BNO055(I2C.Port port, byte address) {
313 imu = new I2C(port, address);
314
315 executor = new java.util.Timer();
316 executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD);
317 }
318
319 /**
320 * Get an instance of the IMU object.
321 *
322 * @param mode
323 * the operating mode to run the sensor in.
324 * @param port
325 * the physical port the sensor is plugged into on the roboRio
326 * @param address
327 * the address the sensor is at (0x28 or 0x29)
328 * @return the instantiated BNO055 object
329 */
330 public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType,
331 I2C.Port port, byte address) {
332
333 if (instance == null) {
334
335 instance = new BNO055(port, address);
336 }
337
338 requestedMode = mode;
339 requestedVectorType = vectorType;
340 return instance;
341 }
342
343 /**
344 * Get an instance of the IMU object plugged into the onboard I2C header.
345 * Using the default address (0x28)
346 *
347 * @param mode
348 * the operating mode to run the sensor in.
349 * @param vectorType
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
353 */
354 public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType) {
355 return getInstance(mode, vectorType, I2C.Port.kOnboard,
356 BNO055_ADDRESS_A);
357 }
358
359 /**
360 * Called periodically. Communicates with the sensor, and checks its state.
361 */
362 private void update() {
363 currentTime = Timer.getFPGATimestamp(); // seconds
364 if (!initialized) {
365 // System.out.println("State: " + state + ". curr: " + currentTime
366 // + ", next: " + nextTime);
367
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.
372 switch (state) {
373 case 0:
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;
378 } else {
379 // Sensor present, go to next state
380 sensorPresent = true;
381 state++;
382 nextTime = Timer.getFPGATimestamp() + 0.050;
383 }
384 break;
385 case 1:
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;
390 state++;
391 }
392 break;
393 case 2:
394 // Reset
395 if (currentTime >= nextTime) {
396 write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x20);
397 state++;
398 }
399 break;
400 case 3:
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
404 state++;
405 // Log current time
406 nextTime = Timer.getFPGATimestamp() + 0.050;
407 }
408 break;
409 case 4:
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;
416 state++;
417 }
418 break;
419 case 5:
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;
424 state++;
425 }
426 break;
427 case 6:
428 if (currentTime >= nextTime) {
429 write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x80);
430 nextTime = Timer.getFPGATimestamp() + 0.500;
431 state++;
432 }
433 break;
434 case 7:
435 // Set operating mode to mode requested at instantiation
436 if (currentTime >= nextTime) {
437 setMode(requestedMode);
438 nextTime = Timer.getFPGATimestamp() + 1.05;
439 state++;
440 }
441 break;
442 case 8:
443 if (currentTime >= nextTime) {
444 nextTime = Timer.getFPGATimestamp() + 1.05;
445 state++;
446 }
447 case 9:
448 if (currentTime >= nextTime) {
449 calculateVector();
450 zeroReferenceConst = getRawHeading();
451 initialized = true;
452 }
453 break;
454 default:
455 // Should never get here - Fail safe
456 initialized = false;
457 }
458 } else {
459 // Sensor is initialized, periodically query position data
460 calculateVector();
461 }
462 }
463
464 /**
465 * Query the sensor for position data.
466 */
467 private void calculateVector() {
468 double[] pos = new double[3];
469 short x = 0, y = 0, z = 0;
470 double headingDiff = 0.0;
471
472 // Read vector data (6 bytes)
473 readLen(requestedVectorType.getVal(), positionVector);
474
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));
481
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:
486 /* 1uT = 16 LSB */
487 pos[0] = (x) / 16.0;
488 pos[1] = (y) / 16.0;
489 pos[2] = (z) / 16.0;
490 break;
491 case VECTOR_GYROSCOPE:
492 /* 1rps = 900 LSB */
493 pos[0] = (x) / 900.0;
494 pos[1] = (y) / 900.0;
495 pos[2] = (z) / 900.0;
496 break;
497 case VECTOR_EULER:
498 /* 1 degree = 16 LSB */
499 pos[0] = (x) / 16.0;
500 pos[1] = (y) / 16.0;
501 pos[2] = (z) / 16.0;
502 break;
503 case VECTOR_ACCELEROMETER:
504 case VECTOR_LINEARACCEL:
505 case VECTOR_GRAVITY:
506 /* 1m/s^2 = 100 LSB */
507 pos[0] = (x) / 100.0;
508 pos[1] = (y) / 100.0;
509 pos[2] = (z) / 100.0;
510 break;
511 }
512
513 // calculate turns
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) {
518 turns++;
519 } else {
520 turns--;
521 }
522 }
523
524 // Update position vectors
525 xyz = pos;
526 }
527
528 /**
529 * Puts the chip in the specified operating mode
530 *
531 * @param mode
532 */
533 public void setMode(opmode_t mode) {
534 setMode(mode.getVal());
535 }
536
537 private void setMode(int mode) {
538 _mode = mode;
539 write8(reg_t.BNO055_OPR_MODE_ADDR, (byte) _mode);
540 }
541
542 /**
543 * Gets the latest system status info
544 *
545 * @return
546 */
547 public SystemStatus getSystemStatus() {
548 SystemStatus status = new SystemStatus();
549
550 write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
551
552 /*
553 * System Status (see section 4.3.58)
554 * ---------------------------------
555 * 0 = Idle
556 * 1 = System Error
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
562 */
563
564 status.system_status = read8(reg_t.BNO055_SYS_STAT_ADDR);
565
566 /*
567 * Self Test Results (see section )
568 * --------------------------------
569 * 1 = test passed, 0 = test failed
570 *
571 * Bit 0 = Accelerometer self test
572 * Bit 1 = Magnetometer self test
573 * Bit 2 = Gyroscope self test
574 * Bit 3 = MCU self test
575 *
576 * 0x0F = all good!
577 */
578
579 status.self_test_result = read8(reg_t.BNO055_SELFTEST_RESULT_ADDR);
580
581 /*
582 * System Error (see section 4.3.59)
583 * ---------------------------------
584 * 0 = No error
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
595 */
596 status.system_error = read8(reg_t.BNO055_SYS_ERR_ADDR);
597 return status;
598 }
599
600 /**
601 * Gets the chip revision numbers
602 *
603 * @return the chips revision information
604 */
605 public RevInfo getRevInfo() {
606 int a = 0, b = 0;
607 RevInfo info = new RevInfo();
608
609 /* Check the accelerometer revision */
610 info.accel_rev = read8(reg_t.BNO055_ACCEL_REV_ID_ADDR);
611
612 /* Check the magnetometer revision */
613 info.mag_rev = read8(reg_t.BNO055_MAG_REV_ID_ADDR);
614
615 /* Check the gyroscope revision */
616 info.gyro_rev = read8(reg_t.BNO055_GYRO_REV_ID_ADDR);
617
618 /* Check the SW revision */
619 info.bl_rev = read8(reg_t.BNO055_BL_REV_ID_ADDR);
620
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);
624
625 return info;
626 }
627
628 /**
629 * Diagnostic method to determine if communications with the sensor are
630 * active.
631 * Note this method returns true after first establishing communications
632 * with the sensor.
633 * Communications are not actively monitored once sensor initialization
634 * has started.
635 *
636 * @return true if the sensor is found on the I2C bus
637 */
638 public boolean isSensorPresent() {
639 return sensorPresent;
640 }
641
642 /**
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.
647 * See isCalibrated.
648 *
649 * @return true when the sensor is initialized.
650 */
651 public boolean isInitialized() {
652 return initialized;
653 }
654
655 /**
656 * Gets current calibration state.
657 *
658 * @return each value will be set to 0 if not calibrated, 3 if fully
659 * calibrated.
660 */
661 public CalData getCalibration() {
662 CalData data = new CalData();
663 int rawCalData = read8(reg_t.BNO055_CALIB_STAT_ADDR);
664
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);
669
670 return data;
671 }
672
673 /**
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.
677 * See Section 3.3.
678 *
679 * @return true if calibration is complete for all sensors required for the
680 * mode the sensor is currently operating in.
681 */
682 public boolean isCalibrated() {
683 boolean retVal = true;
684
685 // Per Table 3-3
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
701 };
702
703 CalData data = getCalibration();
704
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);
711
712 return retVal;
713 }
714
715 /**
716 * Get the sensors internal temperature.
717 *
718 * @return temperature in degrees celsius.
719 */
720 public int getTemp() {
721 return (read8(reg_t.BNO055_TEMP_ADDR));
722 }
723
724 /**
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
729 *
730 * For continuous rotation heading (doesn't roll over between 360/0) see
731 * the getHeading() method.
732 *
733 * Maximum data output rates for Fusion modes - See 3.6.3
734 *
735 * Operating Mode Data Output Rate
736 * IMU 100 Hz
737 * COMPASS 20 Hz
738 * M4G 50 Hz
739 * NDOF_FMC_OFF 100 Hz
740 * NDOF 100 Hz
741 *
742 * @return a vector [heading, roll, pitch]
743 */
744 public double[] getVector() {
745 return xyz;
746 }
747
748 /**
749 * Heading not relative to the starting angle of the robot.
750 *
751 * @return
752 */
753 public double getRawHeading() {
754 return xyz[0] + turns * 360;
755 }
756
757 /**
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.
762 *
763 * @return heading in degrees
764 */
765 public double getHeading() {
766 double reading = getRawHeading();
767
768 return reading - zeroReferenceConst;
769 }
770
771 /**
772 * Writes an 8 bit value over I2C
773 *
774 * @param reg
775 * the register to write the data to
776 * @param value
777 * a byte of data to write
778 * @return whatever I2CJNI.i2CWrite returns. It's not documented in the wpilib
779 * javadocs!
780 */
781 private boolean write8(reg_t reg, byte value) {
782 boolean retVal = false;
783
784 retVal = imu.write(reg.getVal(), value);
785
786 return retVal;
787 }
788
789 /**
790 * Reads an 8 bit value over I2C
791 *
792 * @param reg
793 * the register to read from.
794 * @return
795 */
796 private byte read8(reg_t reg) {
797 byte[] vals = new byte[1];
798
799 readLen(reg, vals);
800 return vals[0];
801 }
802
803 /**
804 * Reads the specified number of bytes over I2C
805 *
806 * @param reg
807 * the address to read from
808 * @param buffer
809 * to store the read data into
810 * @return true on success
811 */
812 private boolean readLen(reg_t reg, byte[] buffer) {
813 return readLen(reg.getVal(), buffer);
814 }
815
816 /**
817 * Reads the specified number of bytes over I2C
818 *
819 * @param reg
820 * the address to read from
821 * @param buffer
822 * the size of the data to read
823 * @return true on success
824 */
825 private boolean readLen(int reg, byte[] buffer) {
826 boolean retVal = true;
827
828 if (buffer == null || buffer.length < 1) {
829 return false;
830 }
831
832 retVal = !imu.read(reg, buffer.length, buffer);
833
834 return retVal;
835 }
836
837 private class BNO055UpdateTask extends TimerTask {
838 private BNO055 imu;
839
840 private BNO055UpdateTask(BNO055 imu) {
841 if (imu == null) {
842 throw new NullPointerException("BNO055 pointer null");
843 }
844 this.imu = imu;
845 }
846
847 /**
848 * Called periodically in its own thread
849 */
850 @Override
851 public void run() {
852 imu.update();
853 }
854 }
855 }