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)       Thread::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_reset_pin();
00031     initialize ();
00032 }
00033 
00034 BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset) :
00035     _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
00036 {
00037     chip_addr = BNO055_G_CHIP_ADDR;
00038     chip_mode = MODE_NDOF;
00039     initialize_reset_pin();
00040     initialize ();
00041 }
00042 
00043 BNO055::BNO055 (I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode) :
00044     _i2c(p_i2c), _res(p_reset)
00045 {
00046     chip_addr = addr;
00047     chip_mode = mode;
00048     initialize_reset_pin();
00049     initialize ();
00050 }
00051 
00052 BNO055::BNO055 (I2C& p_i2c, PinName p_reset) :
00053     _i2c(p_i2c), _res(p_reset)
00054 {
00055     chip_addr = BNO055_G_CHIP_ADDR;
00056     chip_mode = MODE_NDOF;
00057     initialize_reset_pin();
00058     initialize ();
00059 }
00060 
00061 /////////////// Read data & normalize /////////////////////
00062 void BNO055::get_euler_angles(BNO055_EULER_TypeDef *result)
00063 {
00064     int16_t h,p,r;
00065 
00066     select_page(0);
00067     dt[0] = BNO055_EULER_H_LSB;
00068     _i2c.write(chip_addr, dt, 1, true);
00069     _i2c.read(chip_addr, dt, 6, false);
00070     h = dt[1] << 8 | dt[0];
00071     r = dt[3] << 8 | dt[2];
00072     p = dt[5] << 8 | dt[4];
00073 
00074     if (use_degrees()) {
00075         result->h = (double)h / 16;
00076         result->p = (double)p / 16;
00077         result->r = (double)r / 16;
00078     } else {
00079         result->h = (double)h / 900;
00080         result->p = (double)p / 900;
00081         result->r = (double)r / 900;
00082     }
00083 }
00084 
00085 void BNO055::get_quaternion(BNO055_QUATERNION_TypeDef *result)
00086 {
00087     int16_t w,x,y,z;
00088 
00089     select_page(0);
00090     dt[0] = BNO055_QUATERNION_W_LSB;
00091     _i2c.write(chip_addr, dt, 1, true);
00092     _i2c.read(chip_addr, dt, 8, false);
00093     w = (dt[1] << 8 | dt[0]);
00094     x = (dt[3] << 8 | dt[2]);
00095     y = (dt[5] << 8 | dt[4]);
00096     z = (dt[7] << 8 | dt[6]);
00097 
00098     result->w = double(w) / 16384.0f;
00099     result->x = double(x) / 16384.0f;
00100     result->y = double(y) / 16384.0f;
00101     result->z = double(z) / 16384.0f;
00102 }
00103 
00104 void BNO055::get_linear_accel(BNO055_VECTOR_TypeDef *result)
00105 {
00106     int16_t x,y,z;
00107 
00108     select_page(0);
00109     dt[0] = BNO055_LINEAR_ACC_X_LSB;
00110     _i2c.write(chip_addr, dt, 1, true);
00111     _i2c.read(chip_addr, dt, 6, false);
00112     x = dt[1] << 8 | dt[0];
00113     y = dt[3] << 8 | dt[2];
00114     z = dt[5] << 8 | dt[4];
00115 
00116     if (use_mss()) {
00117         result->x = (double)x / 100;
00118         result->y = (double)y / 100;
00119         result->z = (double)z / 100;
00120     } else {
00121         result->x = (double)x;
00122         result->y = (double)y;
00123         result->z = (double)z;
00124     }
00125 }
00126 
00127 void BNO055::get_gravity(BNO055_VECTOR_TypeDef *result)
00128 {
00129     int16_t x,y,z;
00130 
00131     select_page(0);
00132     dt[0] = BNO055_GRAVITY_X_LSB;
00133     _i2c.write(chip_addr, dt, 1, true);
00134     _i2c.read(chip_addr, dt, 6, false);
00135     x = dt[1] << 8 | dt[0];
00136     y = dt[3] << 8 | dt[2];
00137     z = dt[5] << 8 | dt[4];
00138 
00139     if (use_mss()) {
00140         result->x = (double)x / 100;
00141         result->y = (double)y / 100;
00142         result->z = (double)z / 100;
00143     } else {
00144         result->x = (double)x;
00145         result->y = (double)y;
00146         result->z = (double)z;
00147     }
00148 }
00149 
00150 void BNO055::get_mag(BNO055_VECTOR_TypeDef *result)
00151 {
00152     int16_t x,y,z;
00153 
00154     select_page(0);
00155     dt[0] = BNO055_MAG_X_LSB;
00156     _i2c.write(chip_addr, dt, 1, true);
00157     _i2c.read(chip_addr, dt, 6, false);
00158     x = dt[1] << 8 | dt[0];
00159     y = dt[3] << 8 | dt[2];
00160     z = dt[5] << 8 | dt[4];
00161 
00162     result->x = (double)x;
00163     result->y = (double)y;
00164     result->z = (double)z;
00165 }
00166 
00167 void BNO055::get_accel(BNO055_VECTOR_TypeDef *result)
00168 {
00169     int16_t x,y,z;
00170 
00171     select_page(0);
00172     dt[0] = BNO055_ACC_X_LSB;
00173     _i2c.write(chip_addr, dt, 1, true);
00174     _i2c.read(chip_addr, dt, 6, false);
00175     x = dt[1] << 8 | dt[0];
00176     y = dt[3] << 8 | dt[2];
00177     z = dt[5] << 8 | dt[4];
00178 
00179     if (use_mss()) {
00180         result->x = (double)x / 100;
00181         result->y = (double)y / 100;
00182         result->z = (double)z / 100;
00183     } else {
00184         result->x = (double)x;
00185         result->y = (double)y;
00186         result->z = (double)z;
00187     }
00188 }
00189 
00190 void BNO055::get_gyro(BNO055_VECTOR_TypeDef *result)
00191 {
00192     int16_t x,y,z;
00193 
00194     select_page(0);
00195     dt[0] = BNO055_GYR_X_LSB;
00196     _i2c.write(chip_addr, dt, 1, true);
00197     _i2c.read(chip_addr, dt, 6, false);
00198     x = dt[1] << 8 | dt[0];
00199     y = dt[3] << 8 | dt[2];
00200     z = dt[5] << 8 | dt[4];
00201     if (use_dps()) {
00202         result->x = (double)x / 16;
00203         result->y = (double)y / 16;
00204         result->z = (double)z / 16;
00205     } else {
00206         result->x = (double)x / 900;
00207         result->y = (double)y / 900;
00208         result->z = (double)z / 900;
00209     }
00210 }
00211 
00212 void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *result)
00213 {
00214     select_page(0);
00215 
00216     uint8_t use_celsius_result = use_celsius();
00217 
00218     dt[0] = BNO055_TEMP_SOURCE;
00219     dt[1] = 0;
00220     _i2c.write(chip_addr, dt, 2, false);
00221     WAIT_MS(1); // Do I need to wait?
00222     dt[0] = BNO055_TEMP;
00223     _i2c.write(chip_addr, dt, 1, true);
00224     _i2c.read(chip_addr, dt, 1, false);
00225 
00226     if (use_celsius_result) {
00227         result->acc_chip = (int8_t)dt[0];
00228     } else {
00229         result->acc_chip = (int8_t)dt[0] * 2;
00230     }
00231 
00232     dt[0] = BNO055_TEMP_SOURCE;
00233     dt[1] = 1;
00234     _i2c.write(chip_addr, dt, 2, false);
00235     WAIT_MS(1); // Do I need to wait?
00236     dt[0] = BNO055_TEMP;
00237     _i2c.write(chip_addr, dt, 1, true);
00238     _i2c.read(chip_addr, dt, 1, false);
00239 
00240     if (use_celsius_result) {
00241         result->gyr_chip = (int8_t)dt[0];
00242     } else {
00243         result->gyr_chip = (int8_t)dt[0] * 2;
00244     }
00245 }
00246 
00247 /////////////// Initialize ////////////////////////////////
00248 void BNO055::initialize (void)
00249 {
00250 #if defined(TARGET_STM32L152RE)
00251     _i2c.frequency(100000);
00252 #else
00253     _i2c.frequency(400000);
00254 #endif
00255     page_flag = 0xff;
00256     select_page(0);
00257     // Check Acc & Mag & Gyro are available of not
00258     get_id();
00259     // Set initial data
00260     set_initial_dt_to_regs();
00261     // Unit selection
00262     unit_selection();
00263     // Set fusion mode
00264     change_fusion_mode(chip_mode);
00265 }
00266 
00267 void BNO055::initialize_reset_pin(void)
00268 {
00269     _res = 1;
00270     WAIT_MS(700); // Need to wait at least 650mS
00271 }
00272 
00273 void BNO055::unit_selection(void)
00274 {
00275     select_page(0);
00276     dt[0] = BNO055_UNIT_SEL;
00277     dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
00278     _i2c.write(chip_addr, dt, 2, false);
00279 }
00280 
00281 bool BNO055::use_degrees()
00282 {
00283     if (unit_flag_is_set(UNIT_EULER_RAD)) {
00284         return false;
00285     }
00286     return true;
00287 }
00288 
00289 bool BNO055::use_mss()
00290 {
00291     if (unit_flag_is_set(UNIT_ACC_MG)) {
00292         return false;
00293     }
00294     return true;
00295 }
00296 
00297 bool BNO055::use_dps()
00298 {
00299     if (unit_flag_is_set(UNIT_GYR_RPS)) {
00300         return false;
00301     }
00302     return true;
00303 }
00304 
00305 bool BNO055::use_celsius()
00306 {
00307     if (unit_flag_is_set(UNIT_TEMP_F)) {
00308         return false;
00309     }
00310     return true;
00311 }
00312 
00313 bool BNO055::unit_flag_is_set(uint8_t flag)
00314 {
00315     select_page(0);
00316     dt[0] = BNO055_UNIT_SEL;
00317     _i2c.write(chip_addr, dt, 1, true);
00318     _i2c.read(chip_addr, dt, 1, false);
00319     if (dt[0] & flag) {
00320         return true;
00321     }
00322     return false;
00323 }
00324 
00325 uint8_t BNO055::select_page(uint8_t page)
00326 {
00327     if (page != page_flag){
00328         dt[0] = BNO055_PAGE_ID;
00329         if (page == 1) {
00330             dt[1] = 1;  // select page 1
00331         } else {
00332             dt[1] = 0;  // select page 0
00333         }
00334         _i2c.write(chip_addr, dt, 2, false);
00335         dt[0] = BNO055_PAGE_ID;
00336         _i2c.write(chip_addr, dt, 1, true);
00337         _i2c.read(chip_addr, dt, 1, false);
00338         page_flag = dt[0];
00339     }
00340     return page_flag;
00341 }
00342 
00343 uint8_t BNO055::reset(void)
00344 {
00345      _res = 0;
00346      WAIT_MS(1);   // Reset 1mS
00347      _res = 1;
00348      WAIT_MS(700); // Need to wait at least 650mS
00349 #if defined(TARGET_STM32L152RE)
00350     _i2c.frequency(400000);
00351 #else
00352     _i2c.frequency(400000);
00353 #endif
00354     _i2c.stop();
00355     page_flag = 0xff;
00356     select_page(0);
00357     get_id();
00358     if (chip_id != I_AM_BNO055_CHIP){
00359         return 1;
00360     } else {
00361         initialize();
00362         return 0;
00363     }
00364 }
00365 
00366 ////// Set initialize data to related registers ///////////
00367 void BNO055::set_initial_dt_to_regs(void)
00368 {
00369     // select_page(0);
00370     // current setting is only used default values
00371 }
00372 
00373 /////////////// Check Who am I? ///////////////////////////
00374 void BNO055::get_id(void)
00375 {
00376     select_page(0);
00377     // ID
00378     dt[0] = BNO055_CHIP_ID;
00379     _i2c.write(chip_addr, dt, 1, true);
00380     _i2c.read(chip_addr, dt, 7, false);
00381     chip_id = dt[0];
00382     if (chip_id == I_AM_BNO055_CHIP) {
00383         ready_flag = 1;
00384     } else {
00385         ready_flag = 0;
00386     }
00387     acc_id = dt[1];
00388     if (acc_id == I_AM_BNO055_ACC) {
00389         ready_flag |= 2;
00390     }
00391     mag_id = dt[2];
00392     if (mag_id == I_AM_BNO055_MAG) {
00393         ready_flag |= 4;
00394     }
00395     gyr_id = dt[3];
00396     if (mag_id == I_AM_BNO055_MAG) {
00397         ready_flag |= 8;
00398     }
00399     bootldr_rev_id = dt[5]<< 8 | dt[4];
00400     sw_rev_id = dt[6];
00401 }
00402 
00403 void BNO055::read_id_inf(BNO055_ID_INF_TypeDef *id)
00404 {
00405     id->chip_id = chip_id;
00406     id->acc_id = acc_id;
00407     id->mag_id = mag_id;
00408     id->gyr_id = gyr_id;
00409     id->bootldr_rev_id = bootldr_rev_id;
00410     id->sw_rev_id = sw_rev_id;
00411 }
00412 
00413 /////////////// Check chip ready or not  //////////////////
00414 uint8_t BNO055::chip_ready(void)
00415 {
00416     if (ready_flag == 0x0f) {
00417         return 1;
00418     }
00419     return 0;
00420 }
00421 
00422 /////////////// Read Calibration status  //////////////////
00423 uint8_t BNO055::read_calib_status(void)
00424 {
00425     select_page(0);
00426     dt[0] = BNO055_CALIB_STAT;
00427     _i2c.write(chip_addr, dt, 1, true);
00428     _i2c.read(chip_addr, dt, 1, false);
00429     return dt[0];
00430 }
00431 
00432 /////////////// Change Fusion mode  ///////////////////////
00433 void BNO055::change_fusion_mode(uint8_t mode)
00434 {
00435     uint8_t current_mode;
00436 
00437     select_page(0);
00438     current_mode = get_operating_mode();
00439     switch (mode) {
00440         case CONFIGMODE:
00441             dt[0] = BNO055_OPR_MODE;
00442             dt[1] = mode;
00443             _i2c.write(chip_addr, dt, 2, false);
00444             WAIT_MS(19);    // wait 19mS
00445             break;
00446         case MODE_IMU:
00447         case MODE_COMPASS:
00448         case MODE_M4G:
00449         case MODE_NDOF_FMC_OFF:
00450         case MODE_NDOF:
00451             if (current_mode != CONFIGMODE) {   // Can we change the mode directry?
00452                 dt[0] = BNO055_OPR_MODE;
00453                 dt[1] = CONFIGMODE;
00454                 _i2c.write(chip_addr, dt, 2, false);
00455                 WAIT_MS(19);    // wait 19mS
00456             }
00457             dt[0] = BNO055_OPR_MODE;
00458             dt[1] = mode;
00459             _i2c.write(chip_addr, dt, 2, false);
00460             WAIT_MS(7);    // wait 7mS
00461             break;
00462         default:
00463             break;
00464     }
00465 }
00466 
00467 uint8_t BNO055::get_operating_mode(void)
00468 {
00469     select_page(0);
00470     dt[0] = BNO055_OPR_MODE;
00471     _i2c.write(chip_addr, dt, 1, true);
00472     _i2c.read(chip_addr, dt, 1, false);
00473     return dt[0];
00474 }
00475 
00476 /////////////// Set Mouting position  /////////////////////
00477 void BNO055::set_mounting_position(uint8_t position)
00478 {
00479     uint8_t remap_config;
00480     uint8_t remap_sign;
00481     uint8_t current_mode;
00482 
00483     current_mode = get_operating_mode();
00484     change_fusion_mode(CONFIGMODE);
00485     switch (position) {
00486         case MT_P0:
00487             remap_config = 0x21;
00488             remap_sign = 0x04;
00489             break;
00490         case MT_P2:
00491             remap_config = 0x24;
00492             remap_sign = 0x06;
00493             break;
00494         case MT_P3:
00495             remap_config = 0x21;
00496             remap_sign = 0x02;
00497             break;
00498         case MT_P4:
00499             remap_config = 0x24;
00500             remap_sign = 0x03;
00501             break;
00502         case MT_P5:
00503             remap_config = 0x21;
00504             remap_sign = 0x01;
00505             break;
00506         case MT_P6:
00507             remap_config = 0x21;
00508             remap_sign = 0x07;
00509             break;
00510         case MT_P7:
00511             remap_config = 0x24;
00512             remap_sign = 0x05;
00513             break;
00514         case MT_P1:
00515         default:
00516             remap_config = 0x24;
00517             remap_sign = 0x00;
00518             break;
00519     }
00520     dt[0] = BNO055_AXIS_MAP_CONFIG;
00521     dt[1] = remap_config;
00522     dt[2] = remap_sign;
00523     _i2c.write(chip_addr, dt, 3, false);
00524     change_fusion_mode(current_mode);
00525 }
00526 
00527 /////////////// I2C Freq. /////////////////////////////////
00528 void BNO055::frequency(int hz)
00529 {
00530     _i2c.frequency(hz);
00531 }
00532 
00533 /////////////// Read/Write specific register //////////////
00534 uint8_t BNO055::read_reg0(uint8_t addr)
00535 {
00536     select_page(0);
00537     dt[0] = addr;
00538     _i2c.write(chip_addr, dt, 1, true);
00539     _i2c.read(chip_addr, dt, 1, false);
00540     return (uint8_t)dt[0];
00541 }
00542 
00543 uint8_t BNO055::write_reg0(uint8_t addr, uint8_t data)
00544 {
00545     uint8_t current_mode;
00546     uint8_t d;
00547 
00548     current_mode = get_operating_mode();
00549     change_fusion_mode(CONFIGMODE);
00550     dt[0] = addr;
00551     dt[1] = data;
00552     _i2c.write(chip_addr, dt, 2, false);
00553     d = dt[0];
00554     change_fusion_mode(current_mode);
00555     return d;
00556 }
00557 
00558 uint8_t BNO055::read_reg1(uint8_t addr)
00559 {
00560     select_page(1);
00561     dt[0] = addr;
00562     _i2c.write(chip_addr, dt, 1, true);
00563     _i2c.read(chip_addr, dt, 1, false);
00564     return (uint8_t)dt[0];
00565 }
00566 
00567 uint8_t BNO055::write_reg1(uint8_t addr, uint8_t data)
00568 {
00569     uint8_t current_mode;
00570     uint8_t d;
00571 
00572     current_mode = get_operating_mode();
00573     change_fusion_mode(CONFIGMODE);
00574     select_page(1);
00575     dt[0] = addr;
00576     dt[1] = data;
00577     _i2c.write(chip_addr, dt, 2, false);
00578     d = dt[0];
00579     change_fusion_mode(current_mode);
00580     return d;
00581 }