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