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.
  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     setZeroReferenceConst();
 319   }
 320
 321   /***
 322    * Sets the starting heading to the zero reference angle of the readings
 323    * This method should be left private and only be ran once
 324    */
 325   private void setZeroReferenceConst() {
 326     zeroReferenceConst = getHeading();
 327   }
 328
 329   /**
 330    * Get an instance of the IMU object.
 331    *
 332    * @param mode
 333    *          the operating mode to run the sensor in.
 334    * @param port
 335    *          the physical port the sensor is plugged into on the roboRio
 336    * @param address
 337    *          the address the sensor is at (0x28 or 0x29)
 338    * @return the instantiated BNO055 object
 339    */
 340   public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType,
 341       I2C.Port port, byte address) {
 342     if (instance == null) {
 343       System.out.println("gyro called");
 344       instance = new BNO055(port, address);
 345     }
 346     requestedMode = mode;
 347     requestedVectorType = vectorType;
 348     return instance;
 349   }
 350
 351   /**
 352    * Get an instance of the IMU object plugged into the onboard I2C header.
 353    * Using the default address (0x28)
 354    *
 355    * @param mode
 356    *          the operating mode to run the sensor in.
 357    * @param vectorType
 358    *          the format the position vector data should be returned
 359    *          in (if you don't know use VECTOR_EULER).
 360    * @return the instantiated BNO055 object
 361    */
 362   public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType) {
 363     return getInstance(mode, vectorType, I2C.Port.kOnboard,
 364         BNO055_ADDRESS_A);
 365   }
 366
 367   /**
 368    * Called periodically. Communicates with the sensor, and checks its state.
 369    */
 370   private void update() {
 371     currentTime = Timer.getFPGATimestamp(); // seconds
 372     if (!initialized) {
 373       // System.out.println("State: " + state + ". curr: " + currentTime
 374       // + ", next: " + nextTime);
 375
 376       // Step through process of initializing the sensor in a non-
 377       // blocking manner. This sequence of events follows the process
 378       // defined in the original adafruit source as closely as possible.
 379       // XXX: It's likely some of these delays can be optimized out.
 380       System.out.println("State: " + state);
 381       switch (state) {
 382       case 0:
 383         // Wait for the sensor to be present
 384         if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) != BNO055_ID) {
 385           // Sensor not present, keep trying
 386           sensorPresent = false;
 387         } else {
 388           // Sensor present, go to next state
 389           sensorPresent = true;
 390           state++;
 391           nextTime = Timer.getFPGATimestamp() + 0.050;
 392         }
 393         break;
 394       case 1:
 395         if (currentTime >= nextTime) {
 396           // Switch to config mode (just in case since this is the default)
 397           setMode(opmode_t.OPERATION_MODE_CONFIG.getVal());
 398           nextTime = Timer.getFPGATimestamp() + 0.050;
 399           state++;
 400         }
 401         break;
 402       case 2:
 403         // Reset
 404         if (currentTime >= nextTime) {
 405           write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x20);
 406           state++;
 407         }
 408         break;
 409       case 3:
 410         // Wait for the sensor to be present
 411         if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) == BNO055_ID) {
 412           // Sensor present, go to next state
 413           state++;
 414           // Log current time
 415           nextTime = Timer.getFPGATimestamp() + 0.050;
 416         }
 417         break;
 418       case 4:
 419         // Wait at least 50ms
 420         if (currentTime >= nextTime) {
 421           /* Set to normal power mode */
 422           write8(reg_t.BNO055_PWR_MODE_ADDR,
 423               (byte) powermode_t.POWER_MODE_NORMAL.getVal());
 424           nextTime = Timer.getFPGATimestamp() + 0.050;
 425           state++;
 426         }
 427         break;
 428       case 5:
 429         // Use external crystal - 32.768 kHz
 430         if (currentTime >= nextTime) {
 431           write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
 432           nextTime = Timer.getFPGATimestamp() + 0.050;
 433           state++;
 434         }
 435         break;
 436       case 6:
 437         if (currentTime >= nextTime) {
 438           write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x80);
 439           nextTime = Timer.getFPGATimestamp() + 0.500;
 440           state++;
 441         }
 442         break;
 443       case 7:
 444         // Set operating mode to mode requested at instantiation
 445         if (currentTime >= nextTime) {
 446           setMode(requestedMode);
 447           nextTime = Timer.getFPGATimestamp() + 1.05;
 448           state++;
 449         }
 450         break;
 451       case 8:
 452         if (currentTime >= nextTime) {
 453           nextTime = Timer.getFPGATimestamp() + 1.05;
 454           state++;
 455         }
 456       case 9:
 457         initialized = true;
 458         break;
 459       default:
 460         // Should never get here - Fail safe
 461         initialized = false;
 462       }
 463     } else {
 464       // Sensor is initialized, periodically query position data
 465       calculateVector();
 466     }
 467   }
 468
 469   /**
 470    * Query the sensor for position data.
 471    */
 472   private void calculateVector() {
 473     double[] pos = new double[3];
 474     short x = 0, y = 0, z = 0;
 475     double headingDiff = 0.0;
 476
 477     // Read vector data (6 bytes)
 478     readLen(requestedVectorType.getVal(), positionVector);
 479
 480     x = (short) ((positionVector[0] & 0xFF)
 481         | ((positionVector[1] << 8) & 0xFF00));
 482     y = (short) ((positionVector[2] & 0xFF)
 483         | ((positionVector[3] << 8) & 0xFF00));
 484     z = (short) ((positionVector[4] & 0xFF)
 485         | ((positionVector[5] << 8) & 0xFF00));
 486
 487     /* Convert the value to an appropriate range (section 3.6.4) */
 488     /* and assign the value to the Vector type */
 489     switch (requestedVectorType) {
 490     case VECTOR_MAGNETOMETER:
 491       /* 1uT = 16 LSB */
 492       pos[0] = (x) / 16.0;
 493       pos[1] = (y) / 16.0;
 494       pos[2] = (z) / 16.0;
 495       break;
 496     case VECTOR_GYROSCOPE:
 497       /* 1rps = 900 LSB */
 498       pos[0] = (x) / 900.0;
 499       pos[1] = (y) / 900.0;
 500       pos[2] = (z) / 900.0;
 501       break;
 502     case VECTOR_EULER:
 503       /* 1 degree = 16 LSB */
 504       pos[0] = (x) / 16.0;
 505       pos[1] = (y) / 16.0;
 506       pos[2] = (z) / 16.0;
 507       break;
 508     case VECTOR_ACCELEROMETER:
 509     case VECTOR_LINEARACCEL:
 510     case VECTOR_GRAVITY:
 511       /* 1m/s^2 = 100 LSB */
 512       pos[0] = (x) / 100.0;
 513       pos[1] = (y) / 100.0;
 514       pos[2] = (z) / 100.0;
 515       break;
 516     }
 517
 518     // calculate turns
 519     headingDiff = xyz[0] - pos[0];
 520     if (Math.abs(headingDiff) >= 350) {
 521       // We've traveled past the zero heading position
 522       if (headingDiff > 0) {
 523         turns++;
 524       } else {
 525         turns--;
 526       }
 527     }
 528
 529     // Update position vectors
 530     xyz = pos;
 531   }
 532
 533   /**
 534    * Puts the chip in the specified operating mode
 535    *
 536    * @param mode
 537    */
 538   public void setMode(opmode_t mode) {
 539     setMode(mode.getVal());
 540   }
 541
 542   private void setMode(int mode) {
 543     _mode = mode;
 544     write8(reg_t.BNO055_OPR_MODE_ADDR, (byte) _mode);
 545   }
 546
 547   /**
 548    * Gets the latest system status info
 549    *
 550    * @return
 551    */
 552   public SystemStatus getSystemStatus() {
 553     SystemStatus status = new SystemStatus();
 554
 555     write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00);
 556
 557     /*
 558      * System Status (see section 4.3.58)
 559      * ---------------------------------
 560      * 0 = Idle
 561      * 1 = System Error
 562      * 2 = Initializing Peripherals
 563      * 3 = System Initalization
 564      * 4 = Executing Self-Test
 565      * 5 = Sensor fusion algorithm running
 566      * 6 = System running without fusion algorithms
 567      */
 568
 569     status.system_status = read8(reg_t.BNO055_SYS_STAT_ADDR);
 570
 571     /*
 572      * Self Test Results (see section )
 573      * --------------------------------
 574      * 1 = test passed, 0 = test failed
 575      *
 576      * Bit 0 = Accelerometer self test
 577      * Bit 1 = Magnetometer self test
 578      * Bit 2 = Gyroscope self test
 579      * Bit 3 = MCU self test
 580      *
 581      * 0x0F = all good!
 582      */
 583
 584     status.self_test_result = read8(reg_t.BNO055_SELFTEST_RESULT_ADDR);
 585
 586     /*
 587      * System Error (see section 4.3.59)
 588      * ---------------------------------
 589      * 0 = No error
 590      * 1 = Peripheral initialization error
 591      * 2 = System initialization error
 592      * 3 = Self test result failed
 593      * 4 = Register map value out of range
 594      * 5 = Register map address out of range
 595      * 6 = Register map write error
 596      * 7 = BNO low power mode not available for selected operation mode
 597      * 8 = Accelerometer power mode not available
 598      * 9 = Fusion algorithm configuration error
 599      * A = Sensor configuration error
 600      */
 601     status.system_error = read8(reg_t.BNO055_SYS_ERR_ADDR);
 602     return status;
 603   }
 604
 605   /**
 606    * Gets the chip revision numbers
 607    *
 608    * @return the chips revision information
 609    */
 610   public RevInfo getRevInfo() {
 611     int a = 0, b = 0;
 612     RevInfo info = new RevInfo();
 613
 614     /* Check the accelerometer revision */
 615     info.accel_rev = read8(reg_t.BNO055_ACCEL_REV_ID_ADDR);
 616
 617     /* Check the magnetometer revision */
 618     info.mag_rev = read8(reg_t.BNO055_MAG_REV_ID_ADDR);
 619
 620     /* Check the gyroscope revision */
 621     info.gyro_rev = read8(reg_t.BNO055_GYRO_REV_ID_ADDR);
 622
 623     /* Check the SW revision */
 624     info.bl_rev = read8(reg_t.BNO055_BL_REV_ID_ADDR);
 625
 626     a = read8(reg_t.BNO055_SW_REV_ID_LSB_ADDR);
 627     b = read8(reg_t.BNO055_SW_REV_ID_MSB_ADDR);
 628     info.sw_rev = (short) ((b << 8) | a);
 629
 630     return info;
 631   }
 632
 633   /**
 634    * Diagnostic method to determine if communications with the sensor are
 635    * active.
 636    * Note this method returns true after first establishing communications
 637    * with the sensor.
 638    * Communications are not actively monitored once sensor initialization
 639    * has started.
 640    *
 641    * @return true if the sensor is found on the I2C bus
 642    */
 643   public boolean isSensorPresent() {
 644     return sensorPresent;
 645   }
 646
 647   /**
 648    * After power is applied, the sensor needs to be configured for use.
 649    * During this initialization period the sensor will not return position
 650    * vector data. Once initialization is complete, data can be read,
 651    * although the sensor may not have completed calibration.
 652    * See isCalibrated.
 653    *
 654    * @return true when the sensor is initialized.
 655    */
 656   public boolean isInitialized() {
 657     return initialized;
 658   }
 659
 660   /**
 661    * Gets current calibration state.
 662    *
 663    * @return each value will be set to 0 if not calibrated, 3 if fully
 664    *         calibrated.
 665    */
 666   public CalData getCalibration() {
 667     CalData data = new CalData();
 668     int rawCalData = read8(reg_t.BNO055_CALIB_STAT_ADDR);
 669
 670     data.sys = (byte) ((rawCalData >> 6) & 0x03);
 671     data.gyro = (byte) ((rawCalData >> 4) & 0x03);
 672     data.accel = (byte) ((rawCalData >> 2) & 0x03);
 673     data.mag = (byte) (rawCalData & 0x03);
 674
 675     return data;
 676   }
 677
 678   /**
 679    * Returns true if all required sensors (accelerometer, magnetometer,
 680    * gyroscope) have completed their respective calibration sequence.
 681    * Only sensors required by the current operating mode are checked.
 682    * See Section 3.3.
 683    *
 684    * @return true if calibration is complete for all sensors required for the
 685    *         mode the sensor is currently operating in.
 686    */
 687   public boolean isCalibrated() {
 688     boolean retVal = true;
 689
 690     // Per Table 3-3
 691     boolean[][] sensorModeMap = new boolean[][] {
 692         // {accel, mag, gyro}
 693         { false, false, false }, // OPERATION_MODE_CONFIG
 694         { true, false, false }, // OPERATION_MODE_ACCONLY
 695         { false, true, false }, // OPERATION_MODE_MAGONLY
 696         { false, false, true }, // OPERATION_MODE_GYRONLY
 697         { true, true, false }, // OPERATION_MODE_ACCMAG
 698         { true, false, true }, // OPERATION_MODE_ACCGYRO
 699         { false, true, true }, // OPERATION_MODE_MAGGYRO
 700         { true, true, true }, // OPERATION_MODE_AMG
 701         { true, false, true }, // OPERATION_MODE_IMUPLUS
 702         { true, true, false }, // OPERATION_MODE_COMPASS
 703         { true, true, false }, // OPERATION_MODE_M4G
 704         { true, true, true }, // OPERATION_MODE_NDOF_FMC_OFF
 705         { true, true, true } // OPERATION_MODE_NDOF
 706     };
 707
 708     CalData data = getCalibration();
 709
 710     if (sensorModeMap[_mode][0]) // Accelerometer used
 711       retVal = retVal && (data.accel >= 3);
 712     if (sensorModeMap[_mode][1]) // Magnetometer used
 713       retVal = retVal && (data.mag >= 3);
 714     if (sensorModeMap[_mode][2]) // Gyroscope used
 715       retVal = retVal && (data.gyro >= 3);
 716
 717     return retVal;
 718   }
 719
 720   /**
 721    * Get the sensors internal temperature.
 722    *
 723    * @return temperature in degrees celsius.
 724    */
 725   public int getTemp() {
 726     return (read8(reg_t.BNO055_TEMP_ADDR));
 727   }
 728
 729   /**
 730    * Gets a vector representing the sensors position (heading, roll, pitch).
 731    * heading: 0 to 360 degrees
 732    * roll: -90 to +90 degrees
 733    * pitch: -180 to +180 degrees
 734    *
 735    * For continuous rotation heading (doesn't roll over between 360/0) see
 736    * the getHeading() method.
 737    *
 738    * Maximum data output rates for Fusion modes - See 3.6.3
 739    *
 740    * Operating Mode Data Output Rate
 741    * IMU 100 Hz
 742    * COMPASS 20 Hz
 743    * M4G 50 Hz
 744    * NDOF_FMC_OFF 100 Hz
 745    * NDOF 100 Hz
 746    *
 747    * @return a vector [heading, roll, pitch]
 748    */
 749   public double[] getVector() {
 750     return xyz;
 751   }
 752
 753   /**
 754    * The heading of the sensor (x axis) in continuous format. Eg rotating the
 755    * sensor clockwise two full rotations will return a value of 720 degrees.
 756    * The getVector method will return heading in a constrained 0 - 360 deg
 757    * format if required.
 758    *
 759    * @return heading in degrees
 760    */
 761   public double getHeading() {
 762     double reading = (xyz[0] + turns * 360);
 763     if (zeroReferenceConst == 0)
 764       return (xyz[0] + turns * 360);
 765
 766     return (xyz[0] + turns * 360) - zeroReferenceConst;
 767   }
 768
 769   /**
 770    * Writes an 8 bit value over I2C
 771    *
 772    * @param reg
 773    *          the register to write the data to
 774    * @param value
 775    *          a byte of data to write
 776    * @return whatever I2CJNI.i2CWrite returns. It's not documented in the wpilib
 777    *         javadocs!
 778    */
 779   private boolean write8(reg_t reg, byte value) {
 780     boolean retVal = false;
 781
 782     retVal = imu.write(reg.getVal(), value);
 783
 784     return retVal;
 785   }
 786
 787   /**
 788    * Reads an 8 bit value over I2C
 789    *
 790    * @param reg
 791    *          the register to read from.
 792    * @return
 793    */
 794   private byte read8(reg_t reg) {
 795     byte[] vals = new byte[1];
 796
 797     readLen(reg, vals);
 798     return vals[0];
 799   }
 800
 801   /**
 802    * Reads the specified number of bytes over I2C
 803    *
 804    * @param reg
 805    *          the address to read from
 806    * @param buffer
 807    *          to store the read data into
 808    * @return true on success
 809    */
 810   private boolean readLen(reg_t reg, byte[] buffer) {
 811     return readLen(reg.getVal(), buffer);
 812   }
 813
 814   /**
 815    * Reads the specified number of bytes over I2C
 816    *
 817    * @param reg
 818    *          the address to read from
 819    * @param buffer
 820    *          the size of the data to read
 821    * @return true on success
 822    */
 823   private boolean readLen(int reg, byte[] buffer) {
 824     boolean retVal = true;
 825
 826     if (buffer == null || buffer.length < 1) {
 827       return false;
 828     }
 829
 830     retVal = !imu.read(reg, buffer.length, buffer);
 831
 832     return retVal;
 833   }
 834
 835   private class BNO055UpdateTask extends TimerTask {
 836     private BNO055 imu;
 837
 838     private BNO055UpdateTask(BNO055 imu) {
 839       if (imu == null) {
 840         throw new NullPointerException("BNO055 pointer null");
 841       }
 842       this.imu = imu;
 843     }
 844
 845     /**
 846      * Called periodically in its own thread
 847      */
 848     @Override
 849     public void run() {
 850       imu.update();
 851     }
 852   }
 853 }