BNO055.cpp

00001 /*
00002  * mbed library program
00003  *  BNO055 Intelligent 9-axis absolute orientation sensor
00004  *  by Bosch Sensortec
00005  *
00006  * Copyright (c) 2015 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: April     16th, 2015
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
00013  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
00014  * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018 
00019 #include "mbed.h"
00020 #include "BNO055.h"
00021 
00022 BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode):
00023     _i2c(p_sda, p_scl), _res(p_reset)
00024 {
00025     chip_addr = addr;
00026     chip_mode = mode;
00027     initialize ();
00028 }
00029 
00030 BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset) :
00031     _i2c(p_sda, p_scl), _res(p_reset)
00032 {
00033     chip_addr = BNO055_G_CHIP_ADDR;
00034     chip_mode = MODE_NDOF;
00035     initialize ();
00036 }
00037 
00038 BNO055::BNO055 (I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode) :
00039     _i2c(p_i2c), _res(p_reset)
00040 {
00041     chip_addr = addr;
00042     chip_mode = mode;
00043     initialize ();
00044 }
00045 
00046 BNO055::BNO055 (I2C& p_i2c, PinName p_reset) :
00047     _i2c(p_i2c), _res(p_reset)
00048 {
00049     chip_addr = BNO055_G_CHIP_ADDR;
00050     chip_mode = MODE_NDOF;
00051     initialize ();
00052 }
00053 
00054 /////////////// Read data & normalize /////////////////////
00055 void BNO055::get_Euler_Angles(BNO055_EULER_TypeDef *el)
00056 {
00057     uint8_t deg_or_rad;
00058     int16_t h,p,r;
00059 
00060     select_page(0);
00061     dt[0] = BNO055_UNIT_SEL;
00062     _i2c.write(chip_addr, dt, 1, true);
00063     _i2c.read(chip_addr, dt, 1, false);
00064     if (dt[0] & 0x04) {
00065         deg_or_rad = 1; // Radian
00066     } else {
00067         deg_or_rad = 0; // Degree
00068     }
00069     dt[0] = BNO055_EULER_H_LSB;
00070     _i2c.write(chip_addr, dt, 1, true);
00071     _i2c.read(chip_addr, dt, 6, false);
00072     h = dt[1] << 8 | dt[0];
00073     p = dt[3] << 8 | dt[2];
00074     r = dt[5] << 8 | dt[4];
00075     if (deg_or_rad) {
00076         el->h = (double)h / 900;
00077         el->p = (double)p / 900;
00078         el->r = (double)r / 900;
00079     } else {
00080         el->h = (double)h / 16;
00081         el->p = (double)p / 16;
00082         el->r = (double)r / 16;
00083     }
00084 }
00085 
00086 void BNO055::get_quaternion(BNO055_QUATERNION_TypeDef *qua)
00087 {
00088     select_page(0);
00089     dt[0] = BNO055_QUATERNION_W_LSB;
00090     _i2c.write(chip_addr, dt, 1, true);
00091     _i2c.read(chip_addr, dt, 8, false);
00092     qua->w = dt[1] << 8 | dt[0];
00093     qua->x = dt[3] << 8 | dt[2];
00094     qua->y = dt[5] << 8 | dt[4];
00095     qua->z = dt[7] << 8 | dt[6];
00096 }
00097 
00098 void BNO055::get_linear_accel(BNO055_LIN_ACC_TypeDef *la)
00099 {
00100     uint8_t ms2_or_mg;
00101     int16_t x,y,z;
00102 
00103     select_page(0);
00104     dt[0] = BNO055_UNIT_SEL;
00105     _i2c.write(chip_addr, dt, 1, true);
00106     _i2c.read(chip_addr, dt, 1, false);
00107     if (dt[0] & 0x01) {
00108         ms2_or_mg = 1; // mg
00109     } else {
00110         ms2_or_mg = 0; // m/s*s
00111     }
00112     dt[0] = BNO055_LINEAR_ACC_X_LSB;
00113     _i2c.write(chip_addr, dt, 1, true);
00114     _i2c.read(chip_addr, dt, 6, false);
00115     x = dt[1] << 8 | dt[0];
00116     y = dt[3] << 8 | dt[2];
00117     z = dt[5] << 8 | dt[4];
00118     if (ms2_or_mg) {
00119         la->x = (double)x;
00120         la->y = (double)y;
00121         la->z = (double)z;
00122     } else {
00123         la->x = (double)x / 100.0;
00124         la->y = (double)y / 100.0;
00125         la->z = (double)z / 100.0;
00126     }
00127 }
00128 
00129 void BNO055::get_gravity(BNO055_GRAVITY_TypeDef *gr)
00130 {
00131     uint8_t ms2_or_mg;
00132     int16_t x,y,z;
00133 
00134     select_page(0);
00135     dt[0] = BNO055_UNIT_SEL;
00136     _i2c.write(chip_addr, dt, 1, true);
00137     _i2c.read(chip_addr, dt, 1, false);
00138     if (dt[0] & 0x01) {
00139         ms2_or_mg = 1; // mg
00140     } else {
00141         ms2_or_mg = 0; // m/s*s
00142     }
00143     dt[0] = BNO055_GRAVITY_X_LSB;
00144     _i2c.write(chip_addr, dt, 1, true);
00145     _i2c.read(chip_addr, dt, 6, false);
00146     x = dt[1] << 8 | dt[0];
00147     y = dt[3] << 8 | dt[2];
00148     z = dt[5] << 8 | dt[4];
00149     if (ms2_or_mg) {
00150         gr->x = (double)x;
00151         gr->y = (double)y;
00152         gr->z = (double)z;
00153     } else {
00154         gr->x = (double)x / 100;
00155         gr->y = (double)y / 100;
00156         gr->z = (double)z / 100;
00157     }
00158 }
00159 
00160 void BNO055::get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp)
00161 {
00162     uint8_t c_or_f;
00163 
00164     select_page(0);
00165     dt[0] = BNO055_UNIT_SEL;
00166     _i2c.write(chip_addr, dt, 1, true);
00167     _i2c.read(chip_addr, dt, 1, false);
00168     if (dt[0] & 0x10) {
00169         c_or_f = 1; // Fahrenheit
00170     } else {
00171         c_or_f = 0; // degrees Celsius
00172     }
00173     dt[0] = BNO055_TEMP_SOURCE;
00174     dt[1] = 0;
00175     _i2c.write(chip_addr, dt, 2, false);
00176     wait_ms(1); // Do I need to wait?
00177     dt[0] = BNO055_TEMP;
00178     _i2c.write(chip_addr, dt, 1, true);
00179     _i2c.read(chip_addr, dt, 1, false);
00180     if (c_or_f) {
00181         tmp->acc_chip = (int8_t)dt[0] * 2;
00182     } else {
00183         tmp->acc_chip = (int8_t)dt[0];
00184     }
00185     dt[0] = BNO055_TEMP_SOURCE;
00186     dt[1] = 1;
00187     _i2c.write(chip_addr, dt, 2, false);
00188     wait_ms(1); // Do I need to wait?
00189     dt[0] = BNO055_TEMP;
00190     _i2c.write(chip_addr, dt, 1, true);
00191     _i2c.read(chip_addr, dt, 1, false);
00192     if (c_or_f) {
00193         tmp->gyr_chip = (int8_t)dt[0] * 2;
00194     } else {
00195         tmp->gyr_chip = (int8_t)dt[0];
00196     }
00197 }
00198 
00199 /////////////// Initialize ////////////////////////////////
00200 void BNO055::initialize (void)
00201 {
00202 #if defined(TARGET_STM32L152RE)
00203     _i2c.frequency(100000);
00204 #else
00205     _i2c.frequency(400000);
00206 #endif
00207     page_flag = 0xff;
00208     select_page(0);
00209     // Check Acc & Mag & Gyro are available of not
00210     check_id();
00211     // Set initial data
00212     set_initial_dt_to_regs();
00213     // Unit selection
00214     unit_selection();
00215     // Set fusion mode
00216     change_fusion_mode(chip_mode);
00217 }
00218 
00219 void BNO055::unit_selection(void)
00220 {
00221     select_page(0);
00222     dt[0] = BNO055_UNIT_SEL;
00223     dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + 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      wait_ms(1);   // Reset 1mS
00249      _res = 1;
00250      wait(0.7);  // Need to wait at least 650mS
00251 #if defined(TARGET_STM32L152RE)
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             wait_ms(19);    // 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             if (current_mode != CONFIGMODE) {   // Can we change the mode directry?
00354                 dt[0] = BNO055_OPR_MODE;
00355                 dt[1] = CONFIGMODE;
00356                 _i2c.write(chip_addr, dt, 2, false);
00357                 wait_ms(19);    // wait 19mS
00358             }
00359             dt[0] = BNO055_OPR_MODE;
00360             dt[1] = mode;
00361             _i2c.write(chip_addr, dt, 2, false);
00362             wait_ms(7);    // wait 7mS
00363             break;
00364         default:
00365             break;
00366     }
00367 }
00368 
00369 uint8_t BNO055::check_operating_mode(void)
00370 {
00371     select_page(0);
00372     dt[0] = BNO055_OPR_MODE;
00373     _i2c.write(chip_addr, dt, 1, true);
00374     _i2c.read(chip_addr, dt, 1, false);
00375     return dt[0];
00376 }
00377 
00378 /////////////// Set Mouting position  /////////////////////
00379 void BNO055::set_mounting_position(uint8_t position)
00380 {
00381     uint8_t remap_config;
00382     uint8_t remap_sign;
00383     uint8_t current_mode;
00384 
00385     current_mode = check_operating_mode();
00386     change_fusion_mode(CONFIGMODE);
00387     switch (position) {
00388         case MT_P0:
00389             remap_config = 0x21;
00390             remap_sign = 0x04;
00391             break;
00392         case MT_P2:
00393             remap_config = 0x24;
00394             remap_sign = 0x06;
00395             break;
00396         case MT_P3:
00397             remap_config = 0x21;
00398             remap_sign = 0x02;
00399             break;
00400         case MT_P4:
00401             remap_config = 0x24;
00402             remap_sign = 0x03;
00403             break;
00404         case MT_P5:
00405             remap_config = 0x21;
00406             remap_sign = 0x01;
00407             break;
00408         case MT_P6:
00409             remap_config = 0x21;
00410             remap_sign = 0x07;
00411             break;
00412         case MT_P7:
00413             remap_config = 0x24;
00414             remap_sign = 0x05;
00415             break;
00416         case MT_P1:
00417         default:
00418             remap_config = 0x24;
00419             remap_sign = 0x00;
00420             break;
00421     }
00422     dt[0] = BNO055_AXIS_MAP_CONFIG;
00423     dt[1] = remap_config;
00424     dt[2] = remap_sign;
00425     _i2c.write(chip_addr, dt, 3, false);
00426     change_fusion_mode(current_mode);
00427 }
00428 
00429 /////////////// I2C Freq. /////////////////////////////////
00430 void BNO055::frequency(int hz)
00431 {
00432     _i2c.frequency(hz);
00433 }
00434 
00435 /////////////// Read/Write specific register //////////////
00436 uint8_t BNO055::read_reg0(uint8_t addr)
00437 {
00438     select_page(0);
00439     dt[0] = addr;
00440     _i2c.write(chip_addr, dt, 1, true);
00441     _i2c.read(chip_addr, dt, 1, false);
00442     return (uint8_t)dt[0];
00443 }
00444 
00445 uint8_t BNO055::write_reg0(uint8_t addr, uint8_t data)
00446 {
00447     uint8_t current_mode;
00448     uint8_t d;
00449 
00450     current_mode = check_operating_mode();
00451     change_fusion_mode(CONFIGMODE);
00452     dt[0] = addr;
00453     dt[1] = data;
00454     _i2c.write(chip_addr, dt, 2, false);
00455     d = dt[0];
00456     change_fusion_mode(current_mode);
00457     return d;
00458 }
00459 
00460 uint8_t BNO055::read_reg1(uint8_t addr)
00461 {
00462     select_page(1);
00463     dt[0] = addr;
00464     _i2c.write(chip_addr, dt, 1, true);
00465     _i2c.read(chip_addr, dt, 1, false);
00466     return (uint8_t)dt[0];
00467 }
00468 
00469 uint8_t BNO055::write_reg1(uint8_t addr, uint8_t data)
00470 {
00471     uint8_t current_mode;
00472     uint8_t d;
00473 
00474     current_mode = check_operating_mode();
00475     change_fusion_mode(CONFIGMODE);
00476     select_page(1);
00477     dt[0] = addr;
00478     dt[1] = data;
00479     _i2c.write(chip_addr, dt, 2, false);
00480     d = dt[0];
00481     change_fusion_mode(current_mode);
00482     return d;
00483 }