BNO055.h

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