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