22/02/16
Fork of BNO055_fusion by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 14 2022 02:23:01 by 1.7.2