BNO055.cpp

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 #include "mbed.h"
00014 #include "BNO055.h"
00015 
00016 BNO055::BNO055 (PinName p_sda,
00017                 PinName p_scl,
00018                 PinName p_reset,
00019                 uint8_t addr,
00020                 uint8_t mode
00021                ):
00022     _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
00023 {
00024     chip_addr = addr;
00025     chip_mode = mode;
00026     initialize ();
00027 }
00028 
00029 BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset) :
00030     _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
00031 {
00032     chip_addr = BNO055_G_CHIP_ADDR;
00033     chip_mode = MODE_NDOF;
00034     initialize ();
00035 }
00036 
00037 BNO055::BNO055 (I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode) :
00038     _i2c(p_i2c), _res(p_reset)
00039 {
00040     chip_addr = addr;
00041     chip_mode = mode;
00042     initialize ();
00043 }
00044 
00045 BNO055::BNO055 (I2C& p_i2c, PinName p_reset) :
00046     _i2c(p_i2c), _res(p_reset)
00047 {
00048     chip_addr = BNO055_G_CHIP_ADDR;
00049     chip_mode = MODE_NDOF;
00050     initialize ();
00051 }
00052 
00053 /////////////// Read data & normalize //////////////////////////////////////////
00054 void BNO055::get_Euler_Angles(BNO055_EULER_TypeDef *el)
00055 {
00056     uint8_t deg_or_rad;
00057     int16_t h,p,r;
00058 
00059     select_page(0);
00060     dt[0] = BNO055_UNIT_SEL;
00061     _i2c.write(chip_addr, dt, 1, true);
00062     _i2c.read(chip_addr, dt, 1, false);
00063     if (dt[0] & 0x04) {
00064         deg_or_rad = 1; // Radian
00065     } else {
00066         deg_or_rad = 0; // Degree
00067     }
00068     dt[0] = BNO055_EULER_H_LSB;
00069     _i2c.write(chip_addr, dt, 1, true);
00070     _i2c.read(chip_addr, dt, 6, false);
00071     h = dt[1] << 8 | dt[0];
00072     p = dt[3] << 8 | dt[2];
00073     r = dt[5] << 8 | dt[4];
00074     if (deg_or_rad) {
00075         el->h = (double)h / 900;
00076         el->p = (double)p / 900;
00077         el->r = (double)r / 900;
00078     } else {
00079         el->h = (double)h / 16;
00080         el->p = (double)p / 16;
00081         el->r = (double)r / 16;
00082     }
00083 }
00084 
00085 void BNO055::get_quaternion(BNO055_QUATERNION_TypeDef *qua)
00086 {
00087     select_page(0);
00088     dt[0] = BNO055_QUATERNION_W_LSB;
00089     _i2c.write(chip_addr, dt, 1, true);
00090     _i2c.read(chip_addr, dt, 8, false);
00091     qua->w = dt[1] << 8 | dt[0];
00092     qua->x = dt[3] << 8 | dt[2];
00093     qua->y = dt[5] << 8 | dt[4];
00094     qua->z = dt[7] << 8 | dt[6];
00095 }
00096 
00097 void BNO055::get_linear_accel(BNO055_LIN_ACC_TypeDef *la)
00098 {
00099     uint8_t ms2_or_mg;
00100     int16_t x,y,z;
00101 
00102     select_page(0);
00103     dt[0] = BNO055_UNIT_SEL;
00104     _i2c.write(chip_addr, dt, 1, true);
00105     _i2c.read(chip_addr, dt, 1, false);
00106     if (dt[0] & 0x01) {
00107         ms2_or_mg = 1; // mg
00108     } else {
00109         ms2_or_mg = 0; // m/s*s
00110     }
00111     dt[0] = BNO055_LINEAR_ACC_X_LSB;
00112     _i2c.write(chip_addr, dt, 1, true);
00113     _i2c.read(chip_addr, dt, 6, false);
00114     x = dt[1] << 8 | dt[0];
00115     y = dt[3] << 8 | dt[2];
00116     z = dt[5] << 8 | dt[4];
00117     if (ms2_or_mg) {
00118         la->x = (double)x;
00119         la->y = (double)y;
00120         la->z = (double)z;
00121     } else {
00122         la->x = (double)x / 100;
00123         la->y = (double)y / 100;
00124         la->z = (double)z / 100;
00125     }
00126 }
00127 
00128 void BNO055::get_gravity(BNO055_GRAVITY_TypeDef *gr)
00129 {
00130     uint8_t ms2_or_mg;
00131     int16_t x,y,z;
00132 
00133     select_page(0);
00134     dt[0] = BNO055_UNIT_SEL;
00135     _i2c.write(chip_addr, dt, 1, true);
00136     _i2c.read(chip_addr, dt, 1, false);
00137     if (dt[0] & 0x01) {
00138         ms2_or_mg = 1; // mg
00139     } else {
00140         ms2_or_mg = 0; // m/s*s
00141     }
00142     dt[0] = BNO055_GRAVITY_X_LSB;
00143     _i2c.write(chip_addr, dt, 1, true);
00144     _i2c.read(chip_addr, dt, 6, false);
00145     x = dt[1] << 8 | dt[0];
00146     y = dt[3] << 8 | dt[2];
00147     z = dt[5] << 8 | dt[4];
00148     if (ms2_or_mg) {
00149         gr->x = (double)x;
00150         gr->y = (double)y;
00151         gr->z = (double)z;
00152     } else {
00153         gr->x = (double)x / 100;
00154         gr->y = (double)y / 100;
00155         gr->z = (double)z / 100;
00156     }
00157 }
00158 
00159 void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp)
00160 {
00161     uint8_t c_or_f;
00162 
00163     select_page(0);
00164     dt[0] = BNO055_UNIT_SEL;
00165     _i2c.write(chip_addr, dt, 1, true);
00166     _i2c.read(chip_addr, dt, 1, false);
00167     if (dt[0] & 0x10) {
00168         c_or_f = 1; // Fahrenheit
00169     } else {
00170         c_or_f = 0; // degrees Celsius
00171     }
00172     dt[0] = BNO055_TEMP_SOURCE;
00173     dt[1] = 0;
00174     _i2c.write(chip_addr, dt, 2, false);
00175     ThisThread::sleep_for(1ms); // Do I need to wait?
00176     dt[0] = BNO055_TEMP;
00177     _i2c.write(chip_addr, dt, 1, true);
00178     _i2c.read(chip_addr, dt, 1, false);
00179     if (c_or_f) {
00180         tmp->acc_chip = (int8_t)dt[0] * 2;
00181     } else {
00182         tmp->acc_chip = (int8_t)dt[0];
00183     }
00184     dt[0] = BNO055_TEMP_SOURCE;
00185     dt[1] = 1;
00186     _i2c.write(chip_addr, dt, 2, false);
00187     ThisThread::sleep_for(1ms); // Do I need to wait?
00188     dt[0] = BNO055_TEMP;
00189     _i2c.write(chip_addr, dt, 1, true);
00190     _i2c.read(chip_addr, dt, 1, false);
00191     if (c_or_f) {
00192         tmp->gyr_chip = (int8_t)dt[0] * 2;
00193     } else {
00194         tmp->gyr_chip = (int8_t)dt[0];
00195     }
00196 }
00197 
00198 /////////////// Initialize /////////////////////////////////////////////////////
00199 void BNO055::initialize (void)
00200 {
00201 #if defined(TARGET_NUCLEO_L152RE)
00202     _i2c.frequency(100000);
00203 #else
00204     _i2c.frequency(400000);
00205 #endif
00206     page_flag = 0xff;
00207     select_page(0);
00208     // Check Acc & Mag & Gyro are available of not
00209     check_id();
00210     // Set initial data
00211     set_initial_dt_to_regs();
00212     // Unit selection
00213     unit_selection();
00214     // Set fusion mode
00215     change_fusion_mode(chip_mode);
00216 }
00217 
00218 void BNO055::unit_selection(void)
00219 {
00220     select_page(0);
00221     dt[0] = BNO055_UNIT_SEL;
00222     dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS +
00223             UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
00224     _i2c.write(chip_addr, dt, 2, false);
00225 }
00226 
00227 uint8_t BNO055::select_page(uint8_t page)
00228 {
00229     if (page != page_flag) {
00230         dt[0] = BNO055_PAGE_ID;
00231         if (page == 1) {
00232             dt[1] = 1;  // select page 1
00233         } else {
00234             dt[1] = 0;  // select page 0
00235         }
00236         _i2c.write(chip_addr, dt, 2, false);
00237         dt[0] = BNO055_PAGE_ID;
00238         _i2c.write(chip_addr, dt, 1, true);
00239         _i2c.read(chip_addr, dt, 1, false);
00240         page_flag = dt[0];
00241     }
00242     return page_flag;
00243 }
00244 
00245 uint8_t BNO055::reset(void)
00246 {
00247     _res = 0;
00248     ThisThread::sleep_for(1ms);   // Reset 1mS
00249     _res = 1;
00250     ThisThread::sleep_for(700ms); // Need to wait at least 650mS
00251 #if defined(TARGET_NUCLEO_L152RE)
00252     _i2c.frequency(400000);
00253 #else
00254     _i2c.frequency(400000);
00255 #endif
00256     _i2c.stop();
00257     page_flag = 0xff;
00258     select_page(0);
00259     check_id();
00260     if (chip_id != I_AM_BNO055_CHIP) {
00261         return 1;
00262     } else {
00263         initialize();
00264         return 0;
00265     }
00266 }
00267 
00268 ////// Set initialize data to related registers ////////////////////////////////
00269 void BNO055::set_initial_dt_to_regs(void)
00270 {
00271     // select_page(0);
00272     // current setting is only used default values
00273 }
00274 
00275 /////////////// Check Who am I? ////////////////////////////////////////////////
00276 void BNO055::check_id(void)
00277 {
00278     select_page(0);
00279     // ID
00280     dt[0] = BNO055_CHIP_ID;
00281     _i2c.write(chip_addr, dt, 1, true);
00282     _i2c.read(chip_addr, dt, 7, false);
00283     chip_id = dt[0];
00284     if (chip_id == I_AM_BNO055_CHIP) {
00285         ready_flag = 1;
00286     } else {
00287         ready_flag = 0;
00288     }
00289     acc_id = dt[1];
00290     if (acc_id == I_AM_BNO055_ACC) {
00291         ready_flag |= 2;
00292     }
00293     mag_id = dt[2];
00294     if (mag_id == I_AM_BNO055_MAG) {
00295         ready_flag |= 4;
00296     }
00297     gyr_id = dt[3];
00298     if (mag_id == I_AM_BNO055_MAG) {
00299         ready_flag |= 8;
00300     }
00301     bootldr_rev_id = dt[5]<< 8 | dt[4];
00302     sw_rev_id = dt[6];
00303 }
00304 
00305 void BNO055::read_id_inf(BNO055_ID_INF_TypeDef *id)
00306 {
00307     id->chip_id = chip_id;
00308     id->acc_id = acc_id;
00309     id->mag_id = mag_id;
00310     id->gyr_id = gyr_id;
00311     id->bootldr_rev_id = bootldr_rev_id;
00312     id->sw_rev_id = sw_rev_id;
00313 }
00314 
00315 /////////////// Check chip ready or not  ///////////////////////////////////////
00316 uint8_t BNO055::chip_ready(void)
00317 {
00318     if (ready_flag == 0x0f) {
00319         return 1;
00320     }
00321     return 0;
00322 }
00323 
00324 /////////////// Read Calibration status  ///////////////////////////////////////
00325 uint8_t BNO055::read_calib_status(void)
00326 {
00327     select_page(0);
00328     dt[0] = BNO055_CALIB_STAT;
00329     _i2c.write(chip_addr, dt, 1, true);
00330     _i2c.read(chip_addr, dt, 1, false);
00331     return dt[0];
00332 }
00333 
00334 /////////////// Change Fusion mode  ////////////////////////////////////////////
00335 void BNO055::change_fusion_mode(uint8_t mode)
00336 {
00337     uint8_t current_mode;
00338 
00339     select_page(0);
00340     current_mode = check_operating_mode();
00341     switch (mode) {
00342         case CONFIGMODE:
00343             dt[0] = BNO055_OPR_MODE;
00344             dt[1] = mode;
00345             _i2c.write(chip_addr, dt, 2, false);
00346             ThisThread::sleep_for(19ms);    // wait 19mS
00347             break;
00348         case MODE_IMU:
00349         case MODE_COMPASS:
00350         case MODE_M4G:
00351         case MODE_NDOF_FMC_OFF:
00352         case MODE_NDOF:
00353             // Can we change the mode directry?
00354             if (current_mode != CONFIGMODE) {
00355                 dt[0] = BNO055_OPR_MODE;
00356                 dt[1] = CONFIGMODE;
00357                 _i2c.write(chip_addr, dt, 2, false);
00358                 ThisThread::sleep_for(19ms);    // wait 19mS
00359             }
00360             dt[0] = BNO055_OPR_MODE;
00361             dt[1] = mode;
00362             _i2c.write(chip_addr, dt, 2, false);
00363             ThisThread::sleep_for(7ms);    // wait 7mS
00364             break;
00365         default:
00366             break;
00367     }
00368 }
00369 
00370 uint8_t BNO055::check_operating_mode(void)
00371 {
00372     select_page(0);
00373     dt[0] = BNO055_OPR_MODE;
00374     _i2c.write(chip_addr, dt, 1, true);
00375     _i2c.read(chip_addr, dt, 1, false);
00376     return dt[0];
00377 }
00378 
00379 /////////////// Set Mouting position  //////////////////////////////////////////
00380 void BNO055::set_mounting_position(uint8_t position)
00381 {
00382     uint8_t remap_config;
00383     uint8_t remap_sign;
00384     uint8_t current_mode;
00385 
00386     current_mode = check_operating_mode();
00387     change_fusion_mode(CONFIGMODE);
00388     switch (position) {
00389         case MT_P0:
00390             remap_config = 0x21;
00391             remap_sign = 0x04;
00392             break;
00393         case MT_P2:
00394             remap_config = 0x24;
00395             remap_sign = 0x06;
00396             break;
00397         case MT_P3:
00398             remap_config = 0x21;
00399             remap_sign = 0x02;
00400             break;
00401         case MT_P4:
00402             remap_config = 0x24;
00403             remap_sign = 0x03;
00404             break;
00405         case MT_P5:
00406             remap_config = 0x21;
00407             remap_sign = 0x01;
00408             break;
00409         case MT_P6:
00410             remap_config = 0x21;
00411             remap_sign = 0x07;
00412             break;
00413         case MT_P7:
00414             remap_config = 0x24;
00415             remap_sign = 0x05;
00416             break;
00417         case MT_P1:
00418         default:
00419             remap_config = 0x24;
00420             remap_sign = 0x00;
00421             break;
00422     }
00423     dt[0] = BNO055_AXIS_MAP_CONFIG;
00424     dt[1] = remap_config;
00425     dt[2] = remap_sign;
00426     _i2c.write(chip_addr, dt, 3, false);
00427     change_fusion_mode(current_mode);
00428 }
00429 
00430 /////////////// I2C Freq. //////////////////////////////////////////////////////
00431 void BNO055::frequency(int hz)
00432 {
00433     _i2c.frequency(hz);
00434 }
00435 
00436 /////////////// Read/Write specific register ///////////////////////////////////
00437 uint8_t BNO055::read_reg0(uint8_t addr)
00438 {
00439     select_page(0);
00440     dt[0] = addr;
00441     _i2c.write(chip_addr, dt, 1, true);
00442     _i2c.read(chip_addr, dt, 1, false);
00443     return (uint8_t)dt[0];
00444 }
00445 
00446 uint8_t BNO055::write_reg0(uint8_t addr, uint8_t data)
00447 {
00448     uint8_t current_mode;
00449     uint8_t d;
00450 
00451     current_mode = check_operating_mode();
00452     change_fusion_mode(CONFIGMODE);
00453     dt[0] = addr;
00454     dt[1] = data;
00455     _i2c.write(chip_addr, dt, 2, false);
00456     d = dt[0];
00457     change_fusion_mode(current_mode);
00458     return d;
00459 }
00460 
00461 uint8_t BNO055::read_reg1(uint8_t addr)
00462 {
00463     select_page(1);
00464     dt[0] = addr;
00465     _i2c.write(chip_addr, dt, 1, true);
00466     _i2c.read(chip_addr, dt, 1, false);
00467     return (uint8_t)dt[0];
00468 }
00469 
00470 uint8_t BNO055::write_reg1(uint8_t addr, uint8_t data)
00471 {
00472     uint8_t current_mode;
00473     uint8_t d;
00474 
00475     current_mode = check_operating_mode();
00476     change_fusion_mode(CONFIGMODE);
00477     select_page(1);
00478     dt[0] = addr;
00479     dt[1] = data;
00480     _i2c.write(chip_addr, dt, 2, false);
00481     d = dt[0];
00482     change_fusion_mode(current_mode);
00483     return d;
00484 }