challenge-bot.com Git - 3501/2017steamworks/blob - 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. References throughout the code
  10  * are to the following sensor documentation: http://git.io/vuOl1
  11  *
  12  * To use the sensor, wire up to it over I2C on the roboRIO. Creating an
  13  * instance of this class will cause communications with the sensor to being.All
  14  * communications with the sensor occur in a separate thread from your robot
  15  * code to avoid blocking the main robot program execution.
  16  *
  17  * Example: private static BNO055 imu;
  18  *
  19  * public Robot() { imu =
  20  * BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
  21  * BNO055.vector_type_t.VECTOR_EULER); }
  22  *
  23  * You can check the status of the sensor by using the following methods:
  24  * isSensorPresent(); //Checks if the code can talk to the sensor over I2C // If
  25  * this returns false, check your wiring. isInitialized(); //Checks if the
  26  * sensor initialization has completed. // Initialization takes about 3 seconds.
  27  * You won't get // position data back from the sensor until its init'd.
  28  * isCalibrated(); //The BNO055 will return position data after its init'd, //
  29  * but the position data may be inaccurate until all // required sensors report
  30  * they are calibrated. Some // Calibration sequences require you to move the
  31  * BNO055 // around. See the method comments for more info.
  32  *
  33  * Once the sensor calibration is complete , you can get position data by by
  34  * using the getVector() method. See this method definiton for usage info.
  35  *
  36  * This code was originally ported from arduino source developed by Adafruit.
  37  * See the original comment header below.
  38  *
  39  * @author james@team2168.org
  40  *
  41  *
  42  *         ORIGINAL ADAFRUIT HEADER -
  43  *         https://github.com/adafruit/Adafruit_BNO055/
  44  *         ====================================================================
  45  *         ===
  46  *         This is a library for the BNO055 orientation sensor
  47  *
  48  *         Designed specifically to work with the Adafruit BNO055 Breakout.
  49  *
  50  *         Pick one up today in the adafruit shop! ------>
  51  *         http://www.adafruit.com/products
  52  *
  53  *         These sensors use I2C to communicate, 2 pins are required to
  54  *         interface.
  55  *
  56  *         Adafruit invests time and resources providing this open source code,
  57  *         please support Adafruit and open-source hardware by purchasing
  58  *         products from Adafruit!
  59  *
  60  *         Written by KTOWN for Adafruit Industries.
  61  *
  62  *         MIT license, all text above must be included in any redistribution
  63  *
  64  */
  65 public class BNO055 {
  66   // Tread variables
  67   private java.util.Timer executor;
  68   private static final long THREAD_PERIOD = 20; // ms - max poll rate on sensor.
  69
  70   public static final byte BNO055_ADDRESS_A = 0x28;
  71   public static final byte BNO055_ADDRESS_B = 0x29;
  72   public static final int BNO055_ID = 0xA0;
  73
  74   private static BNO055 instance;
  75
  76   private static I2C imu;
  77   private static int _mode;
  78   private static opmode_t requestedMode; // user requested mode of operation.
  79   private static vector_type_t requestedVectorType;
  80
  81   // State machine variables
  82   private volatile int state = 0;
  83   private volatile boolean sensorPresent = false;
  84   private volatile boolean initialized = false;
  85   private volatile double currentTime; // seconds
  86   private volatile double nextTime; // seconds
  87   private volatile byte[] positionVector = new byte[6];
  88   private volatile long turns = 0;
  89   private volatile double[] xyz = new double[3];
  90
  91   private double zeroReferenceConst = 0;
  92
  93   public class SystemStatus {
  94     public int system_status;
  95     public int self_test_result;
  96     public int system_error;
  97   }
  98
  99   public enum reg_t {
 100     /* Page id register definition */
 101     BNO055_PAGE_ID_ADDR(0X07),
 102
 103     /* PAGE0 REGISTER DEFINITION START */
 104     BNO055_CHIP_ID_ADDR(0x00), BNO055_ACCEL_REV_ID_ADDR(
 105         0x01), BNO055_MAG_REV_ID_ADDR(0x02), BNO055_GYRO_REV_ID_ADDR(
 106         0x03), BNO055_SW_REV_ID_LSB_ADDR(0x04), BNO055_SW_REV_ID_MSB_ADDR(
 107         0x05), BNO055_BL_REV_ID_ADDR(0X06),
 108
 109     /* Accel data register */
 110     BNO055_ACCEL_DATA_X_LSB_ADDR(0X08), BNO055_ACCEL_DATA_X_MSB_ADDR(
 111         0X09), BNO055_ACCEL_DATA_Y_LSB_ADDR(0X0A), BNO055_ACCEL_DATA_Y_MSB_ADDR(
 112         0X0B), BNO055_ACCEL_DATA_Z_LSB_ADDR(
 113         0X0C), BNO055_ACCEL_DATA_Z_MSB_ADDR(0X0D),
 114
 115     /* Mag data register */
 116     BNO055_MAG_DATA_X_LSB_ADDR(0X0E), BNO055_MAG_DATA_X_MSB_ADDR(
 117         0X0F), BNO055_MAG_DATA_Y_LSB_ADDR(0X10), BNO055_MAG_DATA_Y_MSB_ADDR(
 118         0X11), BNO055_MAG_DATA_Z_LSB_ADDR(
 119         0X12), BNO055_MAG_DATA_Z_MSB_ADDR(0X13),
 120
 121     /* Gyro data registers */
 122     BNO055_GYRO_DATA_X_LSB_ADDR(0X14), BNO055_GYRO_DATA_X_MSB_ADDR(
 123         0X15), BNO055_GYRO_DATA_Y_LSB_ADDR(0X16), BNO055_GYRO_DATA_Y_MSB_ADDR(
 124         0X17), BNO055_GYRO_DATA_Z_LSB_ADDR(
 125         0X18), BNO055_GYRO_DATA_Z_MSB_ADDR(0X19),
 126
 127     /* Euler data registers */
 128     BNO055_EULER_H_LSB_ADDR(0X1A), BNO055_EULER_H_MSB_ADDR(
 129         0X1B), BNO055_EULER_R_LSB_ADDR(0X1C), BNO055_EULER_R_MSB_ADDR(
 130         0X1D), BNO055_EULER_P_LSB_ADDR(0X1E), BNO055_EULER_P_MSB_ADDR(0X1F),
 131
 132     /* Quaternion data registers */
 133     BNO055_QUATERNION_DATA_W_LSB_ADDR(0X20), BNO055_QUATERNION_DATA_W_MSB_ADDR(
 134         0X21), BNO055_QUATERNION_DATA_X_LSB_ADDR(
 135         0X22), BNO055_QUATERNION_DATA_X_MSB_ADDR(
 136         0X23), BNO055_QUATERNION_DATA_Y_LSB_ADDR(
 137         0X24), BNO055_QUATERNION_DATA_Y_MSB_ADDR(
 138         0X25), BNO055_QUATERNION_DATA_Z_LSB_ADDR(
 139         0X26), BNO055_QUATERNION_DATA_Z_MSB_ADDR(0X27),
 140
 141     /* Linear acceleration data registers */
 142     BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR(
 143         0X28), BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR(
 144         0X29), BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR(
 145         0X2A), BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR(
 146         0X2B), BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR(
 147         0X2C), BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR(0X2D),
 148
 149     /* Gravity data registers */
 150     BNO055_GRAVITY_DATA_X_LSB_ADDR(0X2E), BNO055_GRAVITY_DATA_X_MSB_ADDR(
 151         0X2F), BNO055_GRAVITY_DATA_Y_LSB_ADDR(
 152         0X30), BNO055_GRAVITY_DATA_Y_MSB_ADDR(
 153         0X31), BNO055_GRAVITY_DATA_Z_LSB_ADDR(
 154         0X32), BNO055_GRAVITY_DATA_Z_MSB_ADDR(0X33),
 155
 156     /* Temperature data register */
 157     BNO055_TEMP_ADDR(0X34),
 158
 159     /* Status registers */
 160     BNO055_CALIB_STAT_ADDR(0X35), BNO055_SELFTEST_RESULT_ADDR(
 161         0X36), BNO055_INTR_STAT_ADDR(0X37),
 162
 163     BNO055_SYS_CLK_STAT_ADDR(0X38), BNO055_SYS_STAT_ADDR(
 164         0X39), BNO055_SYS_ERR_ADDR(0X3A),
 165
 166     /* Unit selection register */
 167     BNO055_UNIT_SEL_ADDR(0X3B), BNO055_DATA_SELECT_ADDR(0X3C),
 168
 169     /* Mode registers */
 170     BNO055_OPR_MODE_ADDR(0X3D), BNO055_PWR_MODE_ADDR(0X3E),
 171
 172     BNO055_SYS_TRIGGER_ADDR(0X3F), BNO055_TEMP_SOURCE_ADDR(0X40),
 173
 174     /* Axis remap registers */
 175     BNO055_AXIS_MAP_CONFIG_ADDR(0X41), BNO055_AXIS_MAP_SIGN_ADDR(0X42),
 176
 177     /* SIC registers */
 178     BNO055_SIC_MATRIX_0_LSB_ADDR(0X43), BNO055_SIC_MATRIX_0_MSB_ADDR(
 179         0X44), BNO055_SIC_MATRIX_1_LSB_ADDR(0X45), BNO055_SIC_MATRIX_1_MSB_ADDR(
 180         0X46), BNO055_SIC_MATRIX_2_LSB_ADDR(
 181         0X47), BNO055_SIC_MATRIX_2_MSB_ADDR(
 182         0X48), BNO055_SIC_MATRIX_3_LSB_ADDR(
 183         0X49), BNO055_SIC_MATRIX_3_MSB_ADDR(
 184         0X4A), BNO055_SIC_MATRIX_4_LSB_ADDR(
 185         0X4B), BNO055_SIC_MATRIX_4_MSB_ADDR(
 186         0X4C), BNO055_SIC_MATRIX_5_LSB_ADDR(
 187         0X4D), BNO055_SIC_MATRIX_5_MSB_ADDR(
 188         0X4E), BNO055_SIC_MATRIX_6_LSB_ADDR(
 189         0X4F), BNO055_SIC_MATRIX_6_MSB_ADDR(
 190         0X50), BNO055_SIC_MATRIX_7_LSB_ADDR(
 191         0X51), BNO055_SIC_MATRIX_7_MSB_ADDR(
 192         0X52), BNO055_SIC_MATRIX_8_LSB_ADDR(
 193         0X53), BNO055_SIC_MATRIX_8_MSB_ADDR(
 194         0X54),
 195
 196     /* Accelerometer Offset registers */
 197     ACCEL_OFFSET_X_LSB_ADDR(0X55), ACCEL_OFFSET_X_MSB_ADDR(
 198         0X56), ACCEL_OFFSET_Y_LSB_ADDR(0X57), ACCEL_OFFSET_Y_MSB_ADDR(
 199         0X58), ACCEL_OFFSET_Z_LSB_ADDR(0X59), ACCEL_OFFSET_Z_MSB_ADDR(0X5A),
 200
 201     /* Magnetometer Offset registers */
 202     MAG_OFFSET_X_LSB_ADDR(0X5B), MAG_OFFSET_X_MSB_ADDR(
 203         0X5C), MAG_OFFSET_Y_LSB_ADDR(0X5D), MAG_OFFSET_Y_MSB_ADDR(
 204         0X5E), MAG_OFFSET_Z_LSB_ADDR(0X5F), MAG_OFFSET_Z_MSB_ADDR(0X60),
 205
 206     /* Gyroscope Offset register s */
 207     GYRO_OFFSET_X_LSB_ADDR(0X61), GYRO_OFFSET_X_MSB_ADDR(
 208         0X62), GYRO_OFFSET_Y_LSB_ADDR(0X63), GYRO_OFFSET_Y_MSB_ADDR(
 209         0X64), GYRO_OFFSET_Z_LSB_ADDR(0X65), GYRO_OFFSET_Z_MSB_ADDR(0X66),
 210
 211     /* Radius registers */
 212     ACCEL_RADIUS_LSB_ADDR(0X67), ACCEL_RADIUS_MSB_ADDR(
 213         0X68), MAG_RADIUS_LSB_ADDR(0X69), MAG_RADIUS_MSB_ADDR(0X6A);
 214
 215     private final int val;
 216
 217     reg_t(int val) {
 218       this.val = val;
 219     }
 220
 221     public int getVal() {
 222       return val;
 223     }
 224   };
 225
 226   public enum powermode_t {
 227     POWER_MODE_NORMAL(0X00), POWER_MODE_LOWPOWER(0X01), POWER_MODE_SUSPEND(
 228         0X02);
 229
 230     private final int val;
 231
 232     powermode_t(int val) {
 233       this.val = val;
 234     }
 235
 236     public int getVal() {
 237       return val;
 238     }
 239   };
 240
 241   public enum opmode_t {
 242     /* Operation mode settings */
 243     OPERATION_MODE_CONFIG(0X00), OPERATION_MODE_ACCONLY(
 244         0X01), OPERATION_MODE_MAGONLY(0X02), OPERATION_MODE_GYRONLY(
 245         0X03), OPERATION_MODE_ACCMAG(0X04), OPERATION_MODE_ACCGYRO(
 246         0X05), OPERATION_MODE_MAGGYRO(
 247         0X06), OPERATION_MODE_AMG(0X07), OPERATION_MODE_IMUPLUS(
 248         0X08), OPERATION_MODE_COMPASS(0X09), OPERATION_MODE_M4G(
 249         0X0A), OPERATION_MODE_NDOF_FMC_OFF(
 250         0X0B), OPERATION_MODE_NDOF(0X0C);
 251
 252     private final int val;
 253
 254     opmode_t(int val) {
 255       this.val = val;
 256     }
 257
 258     public int getVal() {
 259       return val;
 260     }
 261   }
 262
 263   public class RevInfo {
 264     public byte accel_rev;
 265     public byte mag_rev;
 266     public byte gyro_rev;
 267     public short sw_rev;
 268     public byte bl_rev;
 269   }
 270
 271   public class CalData {
 272     public byte sys;
 273     public byte gyro;
 274     public byte accel;
 275     public byte mag;
 276   }
 277
 278   public enum vector_type_t {
 279     VECTOR_ACCELEROMETER(
 280         reg_t.BNO055_ACCEL_DATA_X_LSB_ADDR.getVal()), VECTOR_MAGNETOMETER(
 281         reg_t.BNO055_MAG_DATA_X_LSB_ADDR.getVal()), VECTOR_GYROSCOPE(
 282         reg_t.BNO055_GYRO_DATA_X_LSB_ADDR.getVal()), VECTOR_EULER(
 283         reg_t.BNO055_EULER_H_LSB_ADDR.getVal()), VECTOR_LINEARACCEL(
 284         reg_t.BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR
 285             .getVal()), VECTOR_GRAVITY(
 286         reg_t.BNO055_GRAVITY_DATA_X_LSB_ADDR.getVal());
 287
 288     private final int val;
 289
 290     vector_type_t(int val) {
 291       this.val = val;
 292     }
 293
 294     public int getVal() {
 295       return val;
 296     }
 297   };
 298
 299   /**
 300    * Instantiates a new BNO055 class.
 301    *
 302    * @param port
 303    *          the physical port the sensor is plugged into on the roboRio
 304    * @param address
 305    *          the address the sensor is at (0x28 or 0x29)
 306    */
 307   private BNO055(I2C.Port port, byte address) {
 308     imu = new I2C(port, address);
 309
 310     executor = new java.util.Timer();
 311     executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD);
 312   }
 313
 314   /**
 315    * Get an instance of the IMU object.
 316    *
 317    * @param mode
 318    *          the operating mode to run the sensor in.
 319    * @param port
 320    *          the physical port the sensor is plugged into on the roboRio
 321    * @param address
 322    *          the address the sensor is at (0x28 or 0x29)
 323    * @return the instantiated BNO055 object
 324    */
 325   public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType,
 326       I2C.Port port, byte address) {
 327
 328     if (instance == null) {
 329
 330       instance = new BNO055(port, address);
 331     }
 332
 333     requestedMode = mode;
 334     requestedVectorType = vectorType;
 335     return instance;
 336   }
 337
 338   /**
 339    * Get an instance of the IMU object plugged into the onboard I2C header.
 340    * Using the default address (0x28)
 341    *
 342    * @param mode
 343    *          the operating mode to run the sensor in.
 344    * @param vectorType
 345    *          the format the position vector data should be returned in (if you
 346    *          don't know use VECTOR_EULER).
 347    * @return the instantiated BNO055 object
 348    */
 349   public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType) {
 350     return getInstance(mode, vectorType, I2C.Port.kOnboard, BNO055_ADDRESS_A);
 351   }
 352
 353   /**
 354    * Called periodically. Communicates with the sensor, and checks its state.
 355    */
 356   private void update() {
 357     currentTime = Timer.getFPGATimestamp(); // seconds
 358     if (!initialized) {
 359       // System.out.println("State: " + state + ". curr: " + currentTime
 360       // + ", next: " + nextTime);
 361
 362       // Step through process of initializing the sensor in a non-
 363       // blocking manner. This sequence of events follows the process
 364       // defined in the original adafruit source as closely as possible.
 365       // XXX: It's likely some of these delays can be optimized out.
 366       switch (state) {
 367       case 0:
 368         // Wait for the sensor to be present
 369         if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) != BNO055_ID) {
 370           // Sensor not present, keep trying
 371           sensorPresent = false;
 372         } else {
 373           // Sensor present, go to next state
 374           sensorPresent = true;
 375           state++;
 376           nextTime = Timer.getFPGATimestamp() + 0.050;
 377         }
 378         break;
 379       case 1:
 380         if (currentTime >= nextTime) {
 381           // Switch to config mode (just in case since this is the default)
 382           setMode(opmode_t.OPERATION_MODE_CONFIG.getVal());
 383           nextTime = Timer.getFPGATimestamp() + 0.050;
 384           state++;
 385         }
 386         break;
 387       case 2:
 388         // Reset
 389         if (currentTime >= nextTime) {
 390           write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x20);
 391           state++;
 392         }
 393         break;
 394       case 3:
 395         // Wait for the sensor to be present
 396         if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) == BNO055_ID) {
 397           // Sensor present, go to next state
 398           state++;
 399           // Log current time
 400           nextTime = Timer.getFPGATimestamp() + 0.050;
 401         }
 402         break;
 403       case 4:
 404         // Wait at least 50ms
 405         if (currentTime >= nextTime) {
 406           /* Set to normal power mode */
 407           write8(reg_t.BNO055_PWR_MODE_ADDR,
 408               (byte) powermode_t.POWER_MODE_NORMAL.getVal());
 409           nextTime = Timer.getFPGATimestamp() + 0.050;
 410           state++;
 411         }
 412         break;
 413       case 5:
 414         // Use external crystal - 32.768 kHz
 415         if (currentTime >= nextTime) {
 416           write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
 417           nextTime = Timer.getFPGATimestamp() + 0.050;
 418           state++;
 419         }
 420         break;
 421       case 6:
 422         if (currentTime >= nextTime) {
 423           write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x80);
 424           nextTime = Timer.getFPGATimestamp() + 0.500;
 425           state++;
 426         }
 427         break;
 428       case 7:
 429         // Set operating mode to mode requested at instantiation
 430         if (currentTime >= nextTime) {
 431           setMode(requestedMode);
 432           nextTime = Timer.getFPGATimestamp() + 1.05;
 433           state++;
 434         }
 435         break;
 436       case 8:
 437         if (currentTime >= nextTime) {
 438           nextTime = Timer.getFPGATimestamp() + 1.05;
 439           state++;
 440         }
 441       case 9:
 442         if (currentTime >= nextTime) {
 443           calculateVector();
 444           zeroReferenceConst = getDefaultHeading();
 445           initialized = true;
 446         }
 447         break;
 448       default:
 449         // Should never get here - Fail safe
 450         initialized = false;
 451       }
 452     } else {
 453       // Sensor is initialized, periodically query position data
 454       calculateVector();
 455     }
 456   }
 457
 458   /**
 459    * Query the sensor for position data.
 460    */
 461   private void calculateVector() {
 462     double[] pos = new double[3];
 463     short x = 0, y = 0, z = 0;
 464     double headingDiff = 0.0;
 465
 466     // Read vector data (6 bytes)
 467     readLen(requestedVectorType.getVal(), positionVector);
 468
 469     x = (short) ((positionVector[0] & 0xFF)
 470         | ((positionVector[1] << 8) & 0xFF00));
 471     y = (short) ((positionVector[2] & 0xFF)
 472         | ((positionVector[3] << 8) & 0xFF00));
 473     z = (short) ((positionVector[4] & 0xFF)
 474         | ((positionVector[5] << 8) & 0xFF00));
 475
 476     /* Convert the value to an appropriate range (section 3.6.4) */
 477     /* and assign the value to the Vector type */
 478     switch (requestedVectorType) {
 479     case VECTOR_MAGNETOMETER:
 480       /* 1uT = 16 LSB */
 481       pos[0] = (x) / 16.0;
 482       pos[1] = (y) / 16.0;
 483       pos[2] = (z) / 16.0;
 484       break;
 485     case VECTOR_GYROSCOPE:
 486       /* 1rps = 900 LSB */
 487       pos[0] = (x) / 900.0;
 488       pos[1] = (y) / 900.0;
 489       pos[2] = (z) / 900.0;
 490       break;
 491     case VECTOR_EULER:
 492       /* 1 degree = 16 LSB */
 493       pos[0] = (x) / 16.0;
 494       pos[1] = (y) / 16.0;
 495       pos[2] = (z) / 16.0;
 496       break;
 497     case VECTOR_ACCELEROMETER:
 498     case VECTOR_LINEARACCEL:
 499     case VECTOR_GRAVITY:
 500       /* 1m/s^2 = 100 LSB */
 501       pos[0] = (x) / 100.0;
 502       pos[1] = (y) / 100.0;
 503       pos[2] = (z) / 100.0;
 504       break;
 505     }
 506
 507     // calculate turns
 508     headingDiff = xyz[0] - pos[0];
 509     if (Math.abs(headingDiff) >= 350) {
 510       // We've traveled past the zero heading position
 511       if (headingDiff > 0) {
 512         turns++;
 513       } else {
 514         turns--;
 515       }
 516     }
 517
 518     // Update position vectors
 519     xyz = pos;
 520   }
 521
 522   /**
 523    * Puts the chip in the specified operating mode
 524    *
 525    * @param mode
 526    */
 527   public void setMode(opmode_t mode) {
 528     setMode(mode.getVal());
 529   }
 530
 531   private void setMode(int mode) {
 532     _mode = mode;
 533     write8(reg_t.BNO055_OPR_MODE_ADDR, (byte) _mode);
 534   }
 535
 536   /**
 537    * Gets the latest system status info
 538    *
 539    * @return
 540    */
 541   public SystemStatus getSystemStatus() {
 542     SystemStatus status = new SystemStatus();
 543
 544     write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
 545
 546     /*
 547      * System Status (see section 4.3.58) --------------------------------- 0 =
 548      * Idle 1 = System Error 2 = Initializing Peripherals 3 = System
 549      * Initalization 4 = Executing Self-Test 5 = Sensor fusion algorithm running
 550      * 6 = System running without fusion algorithms
 551      */
 552
 553     status.system_status = read8(reg_t.BNO055_SYS_STAT_ADDR);
 554
 555     /*
 556      * Self Test Results (see section ) -------------------------------- 1 =
 557      * test passed, 0 = test failed
 558      *
 559      * Bit 0 = Accelerometer self test Bit 1 = Magnetometer self test Bit 2 =
 560      * Gyroscope self test Bit 3 = MCU self test
 561      *
 562      * 0x0F = all good!
 563      */
 564
 565     status.self_test_result = read8(reg_t.BNO055_SELFTEST_RESULT_ADDR);
 566
 567     /*
 568      * System Error (see section 4.3.59) --------------------------------- 0 =
 569      * No error 1 = Peripheral initialization error 2 = System initialization
 570      * error 3 = Self test result failed 4 = Register map value out of range 5 =
 571      * Register map address out of range 6 = Register map write error 7 = BNO
 572      * low power mode not available for selected operation mode 8 =
 573      * Accelerometer power mode not available 9 = Fusion algorithm configuration
 574      * error A = Sensor configuration error
 575      */
 576     status.system_error = read8(reg_t.BNO055_SYS_ERR_ADDR);
 577     return status;
 578   }
 579
 580   /**
 581    * Gets the chip revision numbers
 582    *
 583    * @return the chips revision information
 584    */
 585   public RevInfo getRevInfo() {
 586     int a = 0, b = 0;
 587     RevInfo info = new RevInfo();
 588
 589     /* Check the accelerometer revision */
 590     info.accel_rev = read8(reg_t.BNO055_ACCEL_REV_ID_ADDR);
 591
 592     /* Check the magnetometer revision */
 593     info.mag_rev = read8(reg_t.BNO055_MAG_REV_ID_ADDR);
 594
 595     /* Check the gyroscope revision */
 596     info.gyro_rev = read8(reg_t.BNO055_GYRO_REV_ID_ADDR);
 597
 598     /* Check the SW revision */
 599     info.bl_rev = read8(reg_t.BNO055_BL_REV_ID_ADDR);
 600
 601     a = read8(reg_t.BNO055_SW_REV_ID_LSB_ADDR);
 602     b = read8(reg_t.BNO055_SW_REV_ID_MSB_ADDR);
 603     info.sw_rev = (short) ((b << 8) | a);
 604
 605     return info;
 606   }
 607
 608   /**
 609    * Diagnostic method to determine if communications with the sensor are
 610    * active. Note this method returns true after first establishing
 611    * communications with the sensor. Communications are not actively monitored
 612    * once sensor initialization has started.
 613    *
 614    * @return true if the sensor is found on the I2C bus
 615    */
 616   public boolean isSensorPresent() {
 617     return sensorPresent;
 618   }
 619
 620   /**
 621    * After power is applied, the sensor needs to be configured for use. During
 622    * this initialization period the sensor will not return position vector data.
 623    * Once initialization is complete, data can be read, although the sensor may
 624    * not have completed calibration. See isCalibrated.
 625    *
 626    * @return true when the sensor is initialized.
 627    */
 628   public boolean isInitialized() {
 629     return initialized;
 630   }
 631
 632   /**
 633    * Gets current calibration state.
 634    *
 635    * @return each value will be set to 0 if not calibrated, 3 if fully
 636    *         calibrated.
 637    */
 638   public CalData getCalibration() {
 639     CalData data = new CalData();
 640     int rawCalData = read8(reg_t.BNO055_CALIB_STAT_ADDR);
 641
 642     data.sys = (byte) ((rawCalData >> 6) & 0x03);
 643     data.gyro = (byte) ((rawCalData >> 4) & 0x03);
 644     data.accel = (byte) ((rawCalData >> 2) & 0x03);
 645     data.mag = (byte) (rawCalData & 0x03);
 646
 647     return data;
 648   }
 649
 650   /**
 651    * Returns true if all required sensors (accelerometer, magnetometer,
 652    * gyroscope) have completed their respective calibration sequence. Only
 653    * sensors required by the current operating mode are checked. See Section
 654    * 3.3.
 655    *
 656    * @return true if calibration is complete for all sensors required for the
 657    *         mode the sensor is currently operating in.
 658    */
 659   public boolean isCalibrated() {
 660     boolean retVal = true;
 661
 662     // Per Table 3-3
 663     boolean[][] sensorModeMap = new boolean[][] {
 664         // {accel, mag, gyro}
 665         { false, false, false }, // OPERATION_MODE_CONFIG
 666         { true, false, false }, // OPERATION_MODE_ACCONLY
 667         { false, true, false }, // OPERATION_MODE_MAGONLY
 668         { false, false, true }, // OPERATION_MODE_GYRONLY
 669         { true, true, false }, // OPERATION_MODE_ACCMAG
 670         { true, false, true }, // OPERATION_MODE_ACCGYRO
 671         { false, true, true }, // OPERATION_MODE_MAGGYRO
 672         { true, true, true }, // OPERATION_MODE_AMG
 673         { true, false, true }, // OPERATION_MODE_IMUPLUS
 674         { true, true, false }, // OPERATION_MODE_COMPASS
 675         { true, true, false }, // OPERATION_MODE_M4G
 676         { true, true, true }, // OPERATION_MODE_NDOF_FMC_OFF
 677         { true, true, true } // OPERATION_MODE_NDOF
 678     };
 679
 680     CalData data = getCalibration();
 681
 682     if (sensorModeMap[_mode][0]) // Accelerometer used
 683       retVal = retVal && (data.accel >= 3);
 684     if (sensorModeMap[_mode][1]) // Magnetometer used
 685       retVal = retVal && (data.mag >= 3);
 686     if (sensorModeMap[_mode][2]) // Gyroscope used
 687       retVal = retVal && (data.gyro >= 3);
 688
 689     return retVal;
 690   }
 691
 692   /**
 693    * Get the sensors internal temperature.
 694    *
 695    * @return temperature in degrees celsius.
 696    */
 697   public int getTemp() {
 698     return (read8(reg_t.BNO055_TEMP_ADDR));
 699   }
 700
 701   /**
 702    * Gets a vector representing the sensors position (heading, roll, pitch).
 703    * heading: 0 to 360 degrees roll: -90 to +90 degrees pitch: -180 to +180
 704    * degrees
 705    *
 706    * For continuous rotation heading (doesn't roll over between 360/0) see the
 707    * getHeading() method.
 708    *
 709    * Maximum data output rates for Fusion modes - See 3.6.3
 710    *
 711    * Operating Mode Data Output Rate IMU 100 Hz COMPASS 20 Hz M4G 50 Hz
 712    * NDOF_FMC_OFF 100 Hz NDOF 100 Hz
 713    *
 714    * @return a vector [heading, roll, pitch]
 715    */
 716   public double[] getVector() {
 717     return xyz;
 718   }
 719
 720   /**
 721    * The default sensor heading not relative to the starting angle of the robot.
 722    *
 723    * @return
 724    */
 725   public double getDefaultHeading() {
 726     return xyz[0] + turns * 360;
 727   }
 728
 729   /**
 730    * The heading of the sensor (x axis) in continuous format relative to the
 731    * initial heading of the robot. Eg rotating the sensor clockwise two full
 732    * rotations will return a value of 720 degrees. The getVector method will
 733    * return heading in a constrained 0 - 360 deg format if required.
 734    *
 735    * @return heading in degrees
 736    */
 737   public double getHeading() {
 738     double reading = getDefaultHeading();
 739
 740     return reading - zeroReferenceConst;
 741   }
 742
 743   /**
 744    * Writes an 8 bit value over I2C
 745    *
 746    * @param reg
 747    *          the register to write the data to
 748    * @param value
 749    *          a byte of data to write
 750    * @return whatever I2CJNI.i2CWrite returns. It's not documented in the wpilib
 751    *         javadocs!
 752    */
 753   private boolean write8(reg_t reg, byte value) {
 754     boolean retVal = false;
 755
 756     retVal = imu.write(reg.getVal(), value);
 757
 758     return retVal;
 759   }
 760
 761   /**
 762    * Reads an 8 bit value over I2C
 763    *
 764    * @param reg
 765    *          the register to read from.
 766    * @return
 767    */
 768   private byte read8(reg_t reg) {
 769     byte[] vals = new byte[1];
 770
 771     readLen(reg, vals);
 772     return vals[0];
 773   }
 774
 775   /**
 776    * Reads the specified number of bytes over I2C
 777    *
 778    * @param reg
 779    *          the address to read from
 780    * @param buffer
 781    *          to store the read data into
 782    * @return true on success
 783    */
 784   private boolean readLen(reg_t reg, byte[] buffer) {
 785     return readLen(reg.getVal(), buffer);
 786   }
 787
 788   /**
 789    * Reads the specified number of bytes over I2C
 790    *
 791    * @param reg
 792    *          the address to read from
 793    * @param buffer
 794    *          the size of the data to read
 795    * @return true on success
 796    */
 797   private boolean readLen(int reg, byte[] buffer) {
 798     boolean retVal = true;
 799
 800     if (buffer == null || buffer.length < 1) {
 801       return false;
 802     }
 803
 804     retVal = !imu.read(reg, buffer.length, buffer);
 805
 806     return retVal;
 807   }
 808
 809   private class BNO055UpdateTask extends TimerTask {
 810     private BNO055 imu;
 811
 812     private BNO055UpdateTask(BNO055 imu) {
 813       if (imu == null) {
 814         throw new NullPointerException("BNO055 pointer null");
 815       }
 816       this.imu = imu;
 817     }
 818
 819     /**
 820      * Called periodically in its own thread
 821      */
 822     @Override
 823     public void run() {
 824       imu.update();
 825     }
 826   }
 827 }