BNO055.h

00001 /*
00002  * mbed library program
00003  *  BNO055 Intelligent 9-axis absolute orientation sensor
00004  *  by Bosch Sensortec
00005  *
00006  * Copyright (c) 2015 Kenji Arai / JH1PJL
00007  *  http://www.page.sannet.ne.jp/kenjia/index.html
00008  *  http://mbed.org/users/kenjiArai/
00009  *      Created: March     30th, 2015
00010  *      Revised: April     16th, 2015
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
00013  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
00014  * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018 /*
00019  *---------------- REFERENCE ----------------------------------------------------------------------
00020  * Original Information
00021  *  https://www.bosch-sensortec.com/en/homepage/products_3/sensor_hubs/iot_solutions/bno055_1/bno055_4
00022  *  Intelligent 9-axis absolute orientation sensor / Data Sheet  BST_BNO055_DS000_12 Nov. 2014 rev.1.2
00023  *  Sample software   https://github.com/BoschSensortec/BNO055_driver
00024  * Sensor board
00025  *  https://www.rutronik24.com/product/bosch+se/bno055+shuttle+board+mems/6431291.html
00026  *  http://microcontrollershop.com/product_info.php?products_id=7140&osCsid=10645k86db2crld4tfi0vol5g5
00027  */
00028 
00029 #ifndef BNO055_H
00030 #define BNO055_H
00031 
00032 #include "mbed.h"
00033 
00034 //  BNO055
00035 //  7bit address = 0b010100x(0x28 or 0x29 depends on COM3)
00036 #define BNO055_G_CHIP_ADDR      (0x28 << 1) // COM3 = GND
00037 #define BNO055_V_CHIP_ADDR      (0x29 << 1) // COM3 = Vdd
00038 
00039 // Fusion mode
00040 #define CONFIGMODE              0x00
00041 #define MODE_IMU                0x08
00042 #define MODE_COMPASS            0x09
00043 #define MODE_M4G                0x0a
00044 #define MODE_NDOF_FMC_OFF       0x0b
00045 #define MODE_NDOF               0x0c
00046 
00047 //  UNIT
00048 #define UNIT_ACC_MSS            0x00    // acc m/s2
00049 #define UNIT_ACC_MG             0x01    // acc mg
00050 #define UNIT_GYR_DPS            0x00    // gyro Dps
00051 #define UNIT_GYR_RPS            0x02    // gyro Rps
00052 #define UNIT_EULER_DEG          0x00    // euler Degrees
00053 #define UNIT_EULER_RAD          0x04    // euler Radians
00054 #define UNIT_TEMP_C             0x00    // temperature degC
00055 #define UNIT_TEMP_F             0x10    // temperature degF
00056 #define UNIT_ORI_WIN            0x00    // Windows orientation
00057 #define UNIT_ORI_ANDROID        0x80    // Android orientation
00058 
00059 //  ID's
00060 #define I_AM_BNO055_CHIP        0xa0    // CHIP ID
00061 #define I_AM_BNO055_ACC         0xfb    // ACC ID
00062 #define I_AM_BNO055_MAG         0x32    // MAG ID
00063 #define I_AM_BNO055_GYR         0x0f    // GYR ID
00064 
00065 ////////////// DATA TYPE DEFINITION ///////////////////////
00066 typedef struct {
00067     uint8_t  chip_id;
00068     uint8_t  acc_id;
00069     uint8_t  mag_id;
00070     uint8_t  gyr_id;
00071     uint8_t  bootldr_rev_id;
00072     uint16_t sw_rev_id;
00073 } BNO055_ID_INF_TypeDef;
00074 
00075 typedef struct {
00076     double h;
00077     double r;
00078     double p;
00079 } BNO055_EULER_TypeDef;
00080 
00081 typedef struct {
00082     int16_t x;
00083     int16_t y;
00084     int16_t z;
00085     int16_t w;
00086 } BNO055_QUATERNION_TypeDef;
00087 
00088 typedef struct {
00089     double x;
00090     double y;
00091     double z;
00092 } BNO055_LIN_ACC_TypeDef;
00093 
00094 typedef struct {
00095     double x;
00096     double y;
00097     double z;
00098 } BNO055_GRAVITY_TypeDef;
00099 
00100 typedef struct {
00101     int8_t acc_chip;
00102     int8_t gyr_chip;
00103 } BNO055_TEMPERATURE_TypeDef;
00104 
00105 enum {MT_P0 = 0, MT_P1, MT_P2, MT_P3, MT_P4, MT_P5, MT_P6, MT_P7};
00106 
00107 /** Interface for Bosch Sensortec Intelligent 9-axis absolute orientation sensor
00108  *      Chip: BNO055
00109  *
00110  * @code
00111  * #include    "mbed.h"
00112  * #include    "BNO055.h"
00113  *
00114  * Serial pc(USBTX,USBRX);
00115  * I2C    i2c(PB_9, PB_8);         // SDA, SCL
00116  * BNO055 imu(i2c, PA_8);          // Reset
00117  *
00118  * BNO055_ID_INF_TypeDef bno055_id_inf;
00119  * BNO055_EULER_TypeDef  euler_angles;
00120  *
00121  * int main() {
00122  *     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
00123  *     if (imu.chip_ready() == 0){
00124  *         pc.printf("Bosch BNO055 is NOT avirable!!\r\n");
00125  *     }
00126  *     imu.read_id_inf(&bno055_id_inf);
00127  *     pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n",
00128  *                bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
00129  *                bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
00130  *     while(1) {
00131  *         imu.get_Euler_Angles(&euler_angles);
00132  *         pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg]\r\n",
00133  *                    euler_angles.h, euler_angles.r, euler_angles.p);
00134  *         wait(0.5);
00135  *     }
00136  * }
00137  * @endcode
00138  */
00139 
00140 class BNO055
00141 {
00142 public:
00143     /** Configure data pin
00144       * @param data SDA and SCL pins
00145       * @param device address
00146       */
00147     BNO055(PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode);
00148 
00149     /** Configure data pin
00150       * @param data SDA and SCL pins
00151       * @param Other parameters are set default data
00152       */
00153     BNO055(PinName p_sda, PinName p_scl, PinName p_reset);
00154 
00155     /** Configure data pin (with other devices on I2C line)
00156       * @param I2C previous definition
00157       * @param device address
00158       */
00159     BNO055(I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode);
00160 
00161     /** Configure data pin (with other devices on I2C line)
00162       * @param I2C previous definition
00163       * @param Other parameters are set default data
00164       */
00165     BNO055(I2C& p_i2c, PinName p_reset);
00166     
00167     /** Get Euler Angles
00168      * @param double type of 3D data address
00169      */
00170     void get_Euler_Angles(BNO055_EULER_TypeDef *el);
00171 
00172     /** Get Quaternion XYZ&W
00173      * @param int16_t type of 4D data address
00174      */
00175     void get_quaternion(BNO055_QUATERNION_TypeDef *qua);
00176 
00177     /** Get Linear accel data
00178      * @param double type of 3D data address
00179      */
00180     void get_linear_accel(BNO055_LIN_ACC_TypeDef *la);
00181 
00182     /** Get Gravity data
00183      * @param double type of 3D data address
00184      */
00185     void get_gravity(BNO055_GRAVITY_TypeDef *gr);
00186 
00187     /** Get Chip temperature data both Acc & Gyro
00188      * @param int8_t type of data address
00189      */
00190     void get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp);
00191 
00192     /** Change fusion mode
00193       * @param fusion mode
00194       * @return none
00195       */
00196     void change_fusion_mode(uint8_t mode);
00197 
00198     /** Set Mouting position
00199       *  Please make sure your mounting direction of BNO055 chip
00200       *  refrence: BNO055 data sheet BST-BNO055-DS000-12 3.4 Axis remap
00201       * @param Set P0 to P7 mounting position data
00202       * @return none
00203       */
00204     void set_mounting_position(uint8_t position);
00205 
00206     /** Read BNO055 ID information
00207       * @param ID information address
00208       * @return none
00209       */
00210     void read_id_inf(BNO055_ID_INF_TypeDef *id);
00211 
00212     /** Check chip is avairable or not
00213       * @param none
00214       * @return OK = 1, NG = 0;
00215       */
00216     uint8_t chip_ready(void);
00217 
00218     /** Read calibration status
00219       * @param none
00220       * @return SYS(7:6),GYR(5:4),ACC(3:2),MAG(1:0) 3 = Calibrated, 0= not yet
00221       */
00222     uint8_t read_calib_status(void);
00223 
00224     /** Reset
00225       * @param none
00226       * @return 0 = sucess, 1 = Not available chip
00227       */
00228     uint8_t reset(void);
00229 
00230     /** Set I2C clock frequency
00231       * @param freq.
00232       * @return none
00233       */
00234     void frequency(int hz);
00235 
00236     /** Read page 0 register
00237       * @param register's address
00238       * @return register data
00239       */
00240     uint8_t read_reg0(uint8_t addr);
00241 
00242     /** Write page 0 register
00243       * @param register's address
00244       * @param data
00245       * @return register data
00246       */
00247     uint8_t write_reg0(uint8_t addr, uint8_t data);
00248 
00249     /** Read page 1 register
00250       * @param register's address
00251       * @return register data
00252       */
00253     uint8_t read_reg1(uint8_t addr);
00254 
00255     /** Write page 1 register
00256       * @param register's address
00257       * @param data
00258       * @return register data
00259       */
00260     uint8_t write_reg1(uint8_t addr, uint8_t data);
00261 
00262 protected:
00263     void initialize(void);
00264     void check_id(void);
00265     void set_initial_dt_to_regs(void);
00266     void unit_selection(void);
00267     uint8_t check_operating_mode(void);
00268     uint8_t select_page(uint8_t page);
00269 
00270     I2C _i2c;
00271     DigitalOut _res;
00272 
00273 private:
00274     char     dt[10];      // working buffer
00275     uint8_t  chip_addr;
00276     uint8_t  chip_mode;
00277     uint8_t  ready_flag;
00278     uint8_t  page_flag;
00279 
00280     uint8_t  chip_id;
00281     uint8_t  acc_id;
00282     uint8_t  mag_id;
00283     uint8_t  gyr_id;
00284     uint8_t  bootldr_rev_id;
00285     uint16_t sw_rev_id;
00286 
00287 };
00288 
00289 //---------------------------------------------------------
00290 //----- Register's definition -----------------------------
00291 //---------------------------------------------------------
00292 // Page id register definition
00293 #define BNO055_PAGE_ID          0x07
00294 
00295 //----- page0 ---------------------------------------------
00296 #define BNO055_CHIP_ID          0x00
00297 #define BNO055_ACCEL_REV_ID     0x01
00298 #define BNO055_MAG_REV_ID       0x02
00299 #define BNO055_GYRO_REV_ID      0x03
00300 #define BNO055_SW_REV_ID_LSB    0x04
00301 #define BNO055_SW_REV_ID_MSB    0x05
00302 #define BNO055_BL_REV_ID        0x06
00303 
00304 // Accel data register*/
00305 #define BNO055_ACC_X_LSB        0x08
00306 #define BNO055_ACC_X_MSB        0x09
00307 #define BNO055_ACC_Y_LSB        0x0a
00308 #define BNO055_ACC_Y_MSB        0x0b
00309 #define BNO055_ACC_Z_LSB        0x0c
00310 #define BNO055_ACC_Z_MSB        0x0d
00311 
00312 // Mag data register
00313 #define BNO055_MAG_X_LSB        0x0e
00314 #define BNO055_MAG_X_MSB        0x0f
00315 #define BNO055_MAG_Y_LSB        0x10
00316 #define BNO055_MAG_Y_MSB        0x11
00317 #define BNO055_MAG_Z_LSB        0x12
00318 #define BNO055_MAG_Z_MSB        0x13
00319 
00320 // Gyro data registers
00321 #define BNO055_GYR_X_LSB        0x14
00322 #define BNO055_GYR_X_MSB        0x15
00323 #define BNO055_GYR_Y_LSB        0x16
00324 #define BNO055_GYR_Y_MSB        0x17
00325 #define BNO055_GYR_Z_LSB        0x18
00326 #define BNO055_GYR_Z_MSB        0x19
00327 
00328 // Euler data registers
00329 #define BNO055_EULER_H_LSB      0x1a
00330 #define BNO055_EULER_H_MSB      0x1b
00331 
00332 #define BNO055_EULER_R_LSB      0x1c
00333 #define BNO055_EULER_R_MSB      0x1d
00334 
00335 #define BNO055_EULER_P_LSB      0x1e
00336 #define BNO055_EULER_P_MSB      0x1f
00337 
00338 // Quaternion data registers
00339 #define BNO055_QUATERNION_W_LSB 0x20
00340 #define BNO055_QUATERNION_W_MSB 0x21
00341 #define BNO055_QUATERNION_X_LSB 0x22
00342 #define BNO055_QUATERNION_X_MSB 0x23
00343 #define BNO055_QUATERNION_Y_LSB 0x24
00344 #define BNO055_QUATERNION_Y_MSB 0x25
00345 #define BNO055_QUATERNION_Z_LSB 0x26
00346 #define BNO055_QUATERNION_Z_MSB 0x27
00347 
00348 // Linear acceleration data registers
00349 #define BNO055_LINEAR_ACC_X_LSB 0x28
00350 #define BNO055_LINEAR_ACC_X_MSB 0x29
00351 #define BNO055_LINEAR_ACC_Y_LSB 0x2a
00352 #define BNO055_LINEAR_ACC_Y_MSB 0x2b
00353 #define BNO055_LINEAR_ACC_Z_LSB 0x2c
00354 #define BNO055_LINEAR_ACC_Z_MSB 0x2d
00355 
00356 // Gravity data registers
00357 #define BNO055_GRAVITY_X_LSB    0x2e
00358 #define BNO055_GRAVITY_X_MSB    0x2f
00359 #define BNO055_GRAVITY_Y_LSB    0x30
00360 #define BNO055_GRAVITY_Y_MSB    0x31
00361 #define BNO055_GRAVITY_Z_LSB    0x32
00362 #define BNO055_GRAVITY_Z_MSB    0x33
00363 
00364 // Temperature data register
00365 #define BNO055_TEMP             0x34
00366 
00367 // Status registers
00368 #define BNO055_CALIB_STAT       0x35
00369 #define BNO055_SELFTEST_RESULT  0x36
00370 #define BNO055_INTR_STAT        0x37
00371 #define BNO055_SYS_CLK_STAT     0x38
00372 #define BNO055_SYS_STAT         0x39
00373 #define BNO055_SYS_ERR          0x3a
00374 
00375 // Unit selection register
00376 #define BNO055_UNIT_SEL         0x3b
00377 #define BNO055_DATA_SELECT      0x3c
00378 
00379 // Mode registers
00380 #define BNO055_OPR_MODE         0x3d
00381 #define BNO055_PWR_MODE         0x3e
00382 #define BNO055_SYS_TRIGGER      0x3f
00383 #define BNO055_TEMP_SOURCE      0x40
00384 
00385 // Axis remap registers
00386 #define BNO055_AXIS_MAP_CONFIG  0x41
00387 #define BNO055_AXIS_MAP_SIGN    0x42
00388 
00389 // SIC registers
00390 #define BNO055_SIC_MTRX_0_LSB   0x43
00391 #define BNO055_SIC_MTRX_0_MSB   0x44
00392 #define BNO055_SIC_MTRX_1_LSB   0x45
00393 #define BNO055_SIC_MTRX_1_MSB   0x46
00394 #define BNO055_SIC_MTRX_2_LSB   0x47
00395 #define BNO055_SIC_MTRX_2_MSB   0x48
00396 #define BNO055_SIC_MTRX_3_LSB   0x49
00397 #define BNO055_SIC_MTRX_3_MSB   0x4a
00398 #define BNO055_SIC_MTRX_4_LSB   0x4b
00399 #define BNO055_SIC_MTRX_4_MSB   0x4c
00400 #define BNO055_SIC_MTRX_5_LSB   0x4d
00401 #define BNO055_SIC_MTRX_5_MSB   0x4e
00402 #define BNO055_SIC_MTRX_6_LSB   0x4f
00403 #define BNO055_SIC_MTRX_6_MSB   0x50
00404 #define BNO055_SIC_MTRX_7_LSB   0x51
00405 #define BNO055_SIC_MTRX_7_MSB   0x52
00406 #define BNO055_SIC_MTRX_8_LSB   0x53
00407 #define BNO055_SIC_MTRX_8_MSB   0x54
00408 
00409 // Accelerometer Offset registers
00410 #define ACCEL_OFFSET_X_LSB      0x55
00411 #define ACCEL_OFFSET_X_MSB      0x56
00412 #define ACCEL_OFFSET_Y_LSB      0x57
00413 #define ACCEL_OFFSET_Y_MSB      0x58
00414 #define ACCEL_OFFSET_Z_LSB      0x59
00415 #define ACCEL_OFFSET_Z_MSB      0x5a
00416 
00417 // Magnetometer Offset registers
00418 #define MAG_OFFSET_X_LSB        0x5b
00419 #define MAG_OFFSET_X_MSB        0x5c
00420 #define MAG_OFFSET_Y_LSB        0x5d
00421 #define MAG_OFFSET_Y_MSB        0x5e
00422 #define MAG_OFFSET_Z_LSB        0x5f
00423 #define MAG_OFFSET_Z_MSB        0x60
00424 
00425 // Gyroscope Offset registers
00426 #define GYRO_OFFSET_X_LSB       0x61
00427 #define GYRO_OFFSET_X_MSB       0x62
00428 #define GYRO_OFFSET_Y_LSB       0x63
00429 #define GYRO_OFFSET_Y_MSB       0x64
00430 #define GYRO_OFFSET_Z_LSB       0x65
00431 #define GYRO_OFFSET_Z_MSB       0x66
00432 
00433 // Radius registers
00434 #define ACCEL_RADIUS_LSB        0x67
00435 #define ACCEL_RADIUS_MSB        0x68
00436 #define MAG_RADIUS_LSB          0x69
00437 #define MAG_RADIUS_MSB          0x6a
00438 
00439 //----- page1 ---------------------------------------------
00440 // Configuration registers
00441 #define ACCEL_CONFIG            0x08
00442 #define MAG_CONFIG              0x09
00443 #define GYRO_CONFIG             0x0a
00444 #define GYRO_MODE_CONFIG        0x0b
00445 #define ACCEL_SLEEP_CONFIG      0x0c
00446 #define GYRO_SLEEP_CONFIG       0x0d
00447 #define MAG_SLEEP_CONFIG        0x0e
00448 
00449 // Interrupt registers
00450 #define INT_MASK                0x0f
00451 #define INT                     0x10
00452 #define ACCEL_ANY_MOTION_THRES  0x11
00453 #define ACCEL_INTR_SETTINGS     0x12
00454 #define ACCEL_HIGH_G_DURN       0x13
00455 #define ACCEL_HIGH_G_THRES      0x14
00456 #define ACCEL_NO_MOTION_THRES   0x15
00457 #define ACCEL_NO_MOTION_SET     0x16
00458 #define GYRO_INTR_SETING        0x17
00459 #define GYRO_HIGHRATE_X_SET     0x18
00460 #define GYRO_DURN_X             0x19
00461 #define GYRO_HIGHRATE_Y_SET     0x1a
00462 #define GYRO_DURN_Y             0x1b
00463 #define GYRO_HIGHRATE_Z_SET     0x1c
00464 #define GYRO_DURN_Z             0x1d
00465 #define GYRO_ANY_MOTION_THRES   0x1e
00466 #define GYRO_ANY_MOTION_SET     0x1f
00467 
00468 //---------------------------------------------------------
00469 //----- Calibration example -------------------------------
00470 //---------------------------------------------------------
00471 #if 0
00472 // Calibration
00473 //  Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
00474 void bno055_calbration(void){
00475     uint8_t d;
00476 
00477     pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
00478     //---------- Gyroscope Caliblation ------------------------------------------------------------
00479     // (a) Place the device in a single stable position for a period of few seconds to allow the
00480     //     gyroscope to calibrate
00481     pc.printf("Step1) Please wait few seconds\r\n");
00482     t.start();
00483     while (t.read() < 10){
00484         d = imu.read_calib_status();
00485         pc.printf("Calb dat = 0x%x target  = 0x30(at least)\r\n", d);
00486         if ((d & 0x30) == 0x30){
00487             break;
00488         }
00489         wait(1.0);
00490     }
00491     pc.printf("-> Step1) is done\r\n\r\n");
00492     //---------- Magnetometer Caliblation ---------------------------------------------------------
00493     // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
00494     //     CALIB_STAT register indicates fully calibrated.
00495     // (b) It takes more calibration movements to get the magnetometer calibrated than in the
00496     //     NDOF mode.
00497     pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
00498     t.start();
00499     while (t.read() < 30){
00500         d = imu.read_calib_status();
00501         pc.printf("Calb dat = 0x%x target  = 0x33(at least)\r\n", d);
00502         if ((d & 0x03) == 0x03){
00503             break;
00504         }
00505         wait(1.0);   
00506     }
00507     pc.printf("-> Step2) is done\r\n\r\n");
00508     //---------- Magnetometer Caliblation ---------------------------------------------------------
00509     // a) Place the device in 6 different stable positions for a period of few seconds
00510     //    to allow the accelerometer to calibrate.
00511     // b) Make sure that there is slow movement between 2 stable positions
00512     //    The 6 stable positions could be in any direction, but make sure that the device is
00513     //    lying at least once perpendicular to the x, y and z axis.
00514     pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
00515     pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
00516     pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
00517     pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
00518     pc.printf(" If you will give up, hit any key.\r\n", d);
00519     t.stop();
00520     while (true){
00521         d = imu.read_calib_status();
00522         imu.get_gravity(&gravity);
00523         pc.printf("Calb dat = 0x%x target  = 0xff ACC:X %3.0f, Y %3.0f, Z %3.0f\r\n",
00524                    d, gravity.x, gravity.y, gravity.z);
00525         if (d == 0xff){     break;}
00526         if (pc.readable()){ break;}
00527         wait(1.0);
00528     }
00529     if (imu.read_calib_status() == 0xff){
00530         pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
00531     } else {
00532         pc.printf("-> Calibration steps are suspended!\r\n\r\n");
00533     }
00534     t.stop();
00535 }
00536 #endif
00537 
00538 #endif      // BNO055_H