Joel von Rotz
/
BNO055
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
bno055.cpp
Go to the documentation of this file.
00001 /** 00002 * @brief Implementation of BNO055 Class. 00003 * 00004 * @file bno055.cpp 00005 * @author Joel von Rotz 00006 * @date 18.07.2018 00007 */ 00008 #include "mbed.h" 00009 #include "bno055.h" 00010 00011 /* TODO ---------------------------------- 00012 * [+] comment blocks for all function 00013 * [+] error handler 00014 * [+] finish up documentation 00015 * [~] rename variables to more understandable names 00016 * [~] replace magic numbers with constants 00017 * TODONE -------------------------------- 00018 * 00019 * --------------------------------------- 00020 */ 00021 00022 #ifdef DEBUGGING_ENABLED 00023 /** 00024 * @brief Construct a new BNO055::BNO055 object 00025 * 00026 * This is the Debugging-version of the BNO055 class. The constructor requires an additional Serial-Object. 00027 * 00028 * @param slave_address The slave-address (determined by the Address-Pin on the chip) 00029 * @param i2c_master I2C-Master object 00030 * @param ResetPin Reference of the Output-Pin, which is connected to the Reset-Pin of the Sensor 00031 * @param external_clk Boolean to determine if the Sensor uses an external 32kHz oscillator 00032 * @param DEBUG_SERIAL Reference of the Serial-object used to debug the different functions. 00033 */ 00034 BNO055::BNO055(uint8_t slave_address, I2C_Master& i2c_master, DigitalOut& ResetPin, bool external_clk, Serial& DEBUG_SERIAL) : 00035 m_i2c_master(i2c_master), 00036 m_ResetPin(ResetPin), 00037 m_DEBUG_SERIAL(DEBUG_SERIAL), 00038 m_bno055_address(slave_address << 1) 00039 { 00040 ResetPin = 1; 00041 wait_ms(500); 00042 00043 #ifdef HARDWARE_RESET 00044 resetHW(); 00045 #else 00046 setOperationMode(OPERATION_MODE_CONFIGMODE); 00047 resetSW(); 00048 #endif 00049 00050 setPage(PAGE_0); 00051 getIDs(); 00052 setOperationMode(OPERATION_MODE_CONFIGMODE); 00053 setPowerMode(POWER_NORMAL); 00054 setUnitFormat(WINDOWS, CELSIUS, DEGREES, DEGREE_PER_SEC, ACCELERATION); 00055 00056 useExternalOscillator(external_clk); 00057 00058 setOrientation(REMAP_OPTION_P1); 00059 } 00060 #else 00061 /** 00062 * @brief Construct a new BNO055::BNO055 object 00063 * 00064 * @param slave_address The slave-address (determined by the Address-Pin on the chip) 00065 * @param i2c_master I2C-Master object 00066 * @param ResetPin Reference of the Output-Pin, which is connected to the Reset-Pin of the Sensor 00067 * @param external_clk Boolean to determine if the Sensor uses an external 32kHz oscillator 00068 */ 00069 BNO055::BNO055(uint8_t slave_address, I2C_Master& i2c_master, DigitalOut& ResetPin, bool external_clk) : 00070 m_i2c_master(i2c_master), 00071 m_ResetPin(ResetPin), 00072 m_bno055_address(slave_address << 1) 00073 { 00074 ResetPin = 1; 00075 wait_ms(500); 00076 00077 #ifdef HARDWARE_RESET 00078 resetHW(); 00079 #else 00080 setOperationMode(OPERATION_MODE_CONFIGMODE); 00081 resetSW(); 00082 #endif 00083 00084 setPage(); 00085 getIDs(); 00086 setOperationMode(OPERATION_MODE_CONFIGMODE); 00087 setPowerMode(POWER_NORMAL); 00088 setUnitFormat(WINDOWS, CELSIUS, DEGREES, DEGREE_PER_SEC, ACCELERATION); 00089 00090 useExternalOscillator(external_clk); 00091 00092 setOrientation(REMAP_OPTION_P1); 00093 } 00094 #endif 00095 00096 /** 00097 * @brief Defines the format of the measurment units 00098 * 00099 * Following is a table, which displays all the possible formats for each measurement units. The column <em>Code</em> 00100 * contain the values, which are used in the programm. 00101 * 00102 * <table> 00103 * <caption>Measurement Unit Formats</caption> 00104 * <tr><th>Type <th>Formats <th>Code 00105 * <tr><td>Orientation <td>Windows, Android - this changes the ranges of the axis <td>WINDOWS, ANDROID 00106 * <tr><td>Temperature <td>Celsius, Fahrenheit <td>CELSIUS, FAHRENHEIT 00107 * <tr><td>Euler <td>Degree, Radians <td>DEGREE, RADIANS 00108 * <tr><td>Gyroscope <td>degree per seconds, radian per seconds <td>DEGREE_PER_SEC, RADIAN_PER_SEC 00109 * <tr><td>Acceleration <td>acceleration (m/s2), milli g-force <td>ACCELERATION, MILLI_G_FORCE 00110 * </table> 00111 * 00112 * @param new_orientation_format 00113 * @param new_temperature_format 00114 * @param new_euler_format 00115 * @param new_gyroscope_format 00116 * @param new_acceleration_format 00117 */ 00118 void BNO055::setUnitFormat(bno055_orientation_t new_orientation_format, 00119 bno055_temperature_t new_temperature_format, 00120 bno055_euler_t new_euler_format, 00121 bno055_gyro_t new_gyroscope_format, 00122 bno055_acceleration_t new_acceleration_format) 00123 { 00124 uint8_t new_unit_data = new_orientation_format + new_temperature_format + new_euler_format + new_gyroscope_format + new_acceleration_format; 00125 00126 format.units = new_unit_data; 00127 format.orientation = new_orientation_format; 00128 format.temperature = new_temperature_format; 00129 format.euler = new_euler_format; 00130 format.gyroscope = new_gyroscope_format; 00131 format.acceleration = new_acceleration_format; 00132 00133 00134 #ifdef DEBUGGING_ENABLED 00135 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tUnits Set\t 0x%02X",new_unit_data); 00136 #endif 00137 m_i2c_master.i2c_writeBits(m_bno055_address, UNIT_SEL, new_unit_data, MASK_UNIT_SEL); 00138 } 00139 00140 /** 00141 * @brief Not Finished 00142 */ 00143 void BNO055::getUnitFormat(void) 00144 { 00145 00146 } 00147 00148 /** 00149 * @brief Get IDs 00150 * 00151 * Read the datasheet, what exactly these IDs are. The data is accessible through the <em>id</em>-struct (object.id.xxx). 00152 */ 00153 void BNO055::getIDs(void) 00154 { 00155 uint8_t dataArray[7]; 00156 m_i2c_master.i2c_readSeries(m_bno055_address, CHIP_ID, dataArray, 7); 00157 id.chip = dataArray[0]; 00158 id.accel = dataArray[1]; 00159 id.magneto = dataArray[2]; 00160 id.gyro = dataArray[3]; 00161 id.sw_rev = dataArray[4] + (static_cast<uint16_t>(dataArray[5]) << 8); 00162 id.bl_rev = dataArray[6]; 00163 00164 #ifdef DEBUGGING_ENABLED 00165 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tIDs---------------------"); 00166 m_DEBUG_SERIAL.printf("\r\n\tChip\t\t 0x%02X",id.chip); 00167 m_DEBUG_SERIAL.printf("\r\n\tAcceleraiton\t 0x%02X",id.accel); 00168 m_DEBUG_SERIAL.printf("\r\n\tMagnetometer\t 0x%02X",id.magneto); 00169 m_DEBUG_SERIAL.printf("\r\n\tGyroscope\t 0x%02X",id.gyro); 00170 m_DEBUG_SERIAL.printf("\r\n\tSofware Rev.\t 0x%04X",id.sw_rev); 00171 m_DEBUG_SERIAL.printf("\r\n\tBootloader\t 0x%02X",id.bl_rev); 00172 #endif 00173 } 00174 00175 /** 00176 * @brief Sets the Power Mode 00177 * 00178 * All of the information aren't definitive or could be straight up wrong. It is therefore highly recommended to read the datasheet of the BNO055. 00179 * 00180 * <table> 00181 * <caption>Power Modes</caption> 00182 * <tr><th>Type <th>Description 00183 * <tr><td>POWER_NORMAL <td>All sensors are activated 00184 * <tr><td>POWER_LOW_POWER_MODE <td>If no action is detected for a configurable duration (default 5 seconds), the system is put into low power mode. 00185 * <tr><td>POWER_SUSPEND_MODE <td>System is paused and sensors and mcu is put into sleep mode. 00186 * </table> 00187 * 00188 * @param new_power_mode New Power Mode 00189 */ 00190 void BNO055::setPowerMode(bno055_powermode_t new_power_mode) 00191 { 00192 if(new_power_mode != mode.power) 00193 { 00194 do 00195 { 00196 m_i2c_master.i2c_writeBits(m_bno055_address, PWR_MODE, new_power_mode, MASK_POWER_MODE); 00197 wait_ms(50.0); 00198 } 00199 while((m_i2c_master.i2c_read(m_bno055_address, PWR_MODE) & MASK_POWER_MODE) != new_power_mode); 00200 //and the old operation mode should only be updated, when there's a new mode. 00201 mode.power = new_power_mode; 00202 00203 #ifdef DEBUGGING_ENABLED 00204 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode new\t %3i", new_power_mode); 00205 #endif 00206 } 00207 else 00208 { 00209 #ifdef DEBUGGING_ENABLED 00210 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode\t no change"); 00211 #endif 00212 } 00213 } 00214 00215 /** 00216 * @brief Returns the currently used Power Mode 00217 * 00218 * The data is also accessible through the <em>mode</em>-struct (object.mode.power). 00219 * 00220 * @return uint8_t Currently used Power Mode 00221 */ 00222 uint8_t BNO055::getPowerMode(void) 00223 { 00224 mode.power = m_i2c_master.i2c_read(m_bno055_address, PWR_MODE); 00225 #ifdef DEBUGGING_ENABLED 00226 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpwrMode current\t %3i",mode.power); 00227 #endif 00228 return mode.power; 00229 } 00230 00231 /** 00232 * @brief Sets the desired Operation Mode 00233 * 00234 * All of the information aren't definitive or could be straight up wrong. It is therefore highly recommended to read the datasheet of the BNO055. 00235 * 00236 * <table> 00237 * <caption>Operation Modes</caption> 00238 * <tr><th>Type <th>Description 00239 * <tr><td>OPERATION_MODE_CONFIGMODE <td>Used when configuring the system 00240 * <tr><td>OPERATION_MODE_ACCONLY <td>Enables Accelerometer only 00241 * <tr><td>OPERATION_MODE_MAGONLY <td>Enables Magnetometer only 00242 * <tr><td>OPERATION_MODE_GYROONLY <td>Enables Gyroscope only 00243 * <tr><td>OPERATION_MODE_ACCMAG <td>Enables Accelerometer and Magnetometer only 00244 * <tr><td>OPERATION_MODE_ACCGYRO <td>Enables Accelerometer and Gyroscope only 00245 * <tr><td>OPERATION_MODE_MAGGYRO <td>Enables Magnetometer and Gyroscope only 00246 * <tr><td>OPERATION_MODE_AMG <td>Enables all three sensors 00247 * <tr><td>OPERATION_MODE_IMU <td>Enables Accelerometer & Gyroscope and relative position 00248 * <tr><td>OPERATION_MODE_COMPASS <td>Enables Accelerometer & Magnetometer and absolute position 00249 * <tr><td>OPERATION_MODE_M4G <td>"Magnet for Gyroscope" is similar to <em>IMU</em>, but instead using the gyroscope, it uses the magnetometer 00250 * <tr><td>OPERATION_MODE_NDOF_FMC_OFF <td>"9 degrees of freedom" but without fast magnetometer calibration. 00251 * <tr><td>OPERATION_MODE_NDOF <td>"9 degrees of freedom" - Magnetometer is calibrated very fast and increased data output 00252 * </table> 00253 * 00254 * @param new_opr_mode 00255 */ 00256 void BNO055::setOperationMode(bno055_opr_mode_t new_opr_mode) 00257 { 00258 //trying to write a mode, that is already being used, doesn't make any sense and can be ignored. 00259 if(new_opr_mode != mode.operation) 00260 { 00261 do 00262 { 00263 m_i2c_master.i2c_writeBits(m_bno055_address, OPR_MODE, new_opr_mode, MASK_OPERAITON_MODE); 00264 wait_ms(50.0); 00265 } 00266 while((m_i2c_master.i2c_read(m_bno055_address, OPR_MODE) & MASK_OPERAITON_MODE) != new_opr_mode); 00267 #ifdef DEBUGGING_ENABLED 00268 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode new\t %3i", new_opr_mode); 00269 #endif 00270 00271 //and the old operation mode should only be updated, when there's a new mode. 00272 mode.operation = new_opr_mode; 00273 } 00274 else 00275 { 00276 #ifdef DEBUGGING_ENABLED 00277 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode\t\t no change"); 00278 #endif 00279 } 00280 } 00281 00282 /** 00283 * @brief Returns the current Operation Mode 00284 * 00285 * The data is also accessible through the <em>mode</em>-struct (object.mode.operation). 00286 * 00287 * @return uint8_t The current Operation Mode 00288 */ 00289 uint8_t BNO055::getOperationMode(void) 00290 { 00291 mode.operation = m_i2c_master.i2c_read(m_bno055_address, OPR_MODE); 00292 #ifdef DEBUGGING_ENABLED 00293 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode current\t %3i", mode.operation); 00294 #endif 00295 return mode.operation; 00296 } 00297 00298 /** 00299 * @brief Set I2C-Registermap Page 00300 * 00301 * The BNO055 contains two Register Map pages: Page 0 and Page 1. This function is used to switch between those two. 00302 * 00303 * Possible Options are: PAGE_0 & PAGE_1 00304 * 00305 * @param new_page Desired new page number 00306 */ 00307 void BNO055::setPage(bno055_page_t new_page) 00308 { 00309 page = new_page; 00310 00311 #ifdef DEBUGGING_ENABLED 00312 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage set\t %3i", page); 00313 #endif 00314 00315 m_i2c_master.i2c_write(m_bno055_address, PAGE_ID, page); 00316 } 00317 00318 /** 00319 * @brief Returns the current Registermap-Page 00320 * 00321 * The data is also accessible through the <em>page</em>-struct (object.page). 00322 * 00323 * @return uint8_t The Current Page 00324 */ 00325 uint8_t BNO055::getPage(void) 00326 { 00327 page = m_i2c_master.i2c_read(m_bno055_address, PAGE_ID); 00328 00329 #ifdef DEBUGGING_ENABLED 00330 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage current\t %3i",page); 00331 #endif 00332 00333 return page; 00334 } 00335 00336 /** 00337 * @brief Returns the System Status 00338 * 00339 * The data is also accessible through the <em>system</em>-struct (object.system.status). 00340 * 00341 * @return uint8_t System Status 00342 */ 00343 uint8_t BNO055::getSystemStatus(void) 00344 { 00345 system.status = m_i2c_master.i2c_read(m_bno055_address, SYS_STATUS); 00346 #ifdef DEBUGGING_ENABLED 00347 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysStatus\t %3i",system.status); 00348 #endif 00349 return system.status; 00350 } 00351 00352 /** 00353 * @brief Returns the System Error 00354 * 00355 * The data is also accessible through the <em>system</em>-struct (object.system.error). 00356 * 00357 * @return uint8_t System Error 00358 */ 00359 uint8_t BNO055::getSystemError(void) 00360 { 00361 system.error = m_i2c_master.i2c_read(m_bno055_address, SYS_ERR); 00362 #ifdef DEBUGGING_ENABLED 00363 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysError\t %3i",system.error); 00364 #endif 00365 return system.error; 00366 } 00367 00368 /** 00369 * @brief Configure External Oscillator Input 00370 * 00371 * The external oscillator increases data accuracy. 00372 * 00373 * @param enabled <strong>TRUE</strong> enables external oscillator input, <strong>FALSE</strong> disabled input 00374 */ 00375 void BNO055::useExternalOscillator(bool enabled) 00376 { 00377 char data = static_cast<char>(enabled) << 7; 00378 #ifdef DEBUGGING_ENABLED 00379 if(enabled) 00380 { 00381 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source\t active"); 00382 } 00383 else 00384 { 00385 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source \tinactive"); 00386 } 00387 #endif 00388 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, data, MASK_EXT_CLOCK); 00389 } 00390 00391 /** 00392 * @brief Resets the System via Software 00393 */ 00394 void BNO055::resetSW(void) 00395 { 00396 #ifdef DEBUGGING_ENABLED 00397 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSoftware Reset"); 00398 #endif 00399 00400 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x20, MASK_SYSTEM_RESET); 00401 00402 //datasheet (p.13 of revision 14) indicate a Power-On-Reset-time of 650ms 00403 //from Reset to Config Mode. By constantly reading the chip id, it can be 00404 //determined, when the chip is fully functional again. 00405 while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID) 00406 { 00407 wait_ms(10.0); 00408 } 00409 00410 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x00, MASK_SYSTEM_RESET); 00411 } 00412 00413 /** 00414 * @brief Resets the System via Reset-Pin 00415 * 00416 * Resets the System with the referenced Reset-Pin during object-construction 00417 */ 00418 void BNO055::resetHW(void) 00419 { 00420 #ifdef DEBUGGING_ENABLED 00421 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tHardware Reset"); 00422 #endif 00423 m_ResetPin = 0; 00424 wait_ms(10.0); 00425 m_ResetPin = 1; 00426 while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID) 00427 { 00428 wait_ms(10.0); 00429 } 00430 00431 } 00432 00433 /** 00434 * @brief Remaps the three Axes to respect the new Orientation 00435 * 00436 * use <code>REMAP_OPTION_PX</code> with X defining one of the 8 orientation options (see datasheet) 00437 * 00438 * @param orientation_placement Orientation of the IC 00439 */ 00440 void BNO055::setOrientation(bno055_remap_options_t orientation_placement) 00441 { 00442 axis.map = (orientation_placement >> SHIFT_1BYTE) & 0xFF; 00443 axis.sign = (orientation_placement & 0xFF); 00444 00445 #ifdef DEBUGGING_ENABLED 00446 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap New\t 0x%02X", axis.map); 00447 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign New\t 0x%02X", axis.sign); 00448 #endif 00449 00450 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS); 00451 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS); 00452 } 00453 00454 /** 00455 * @brief Returns the current Orientation 00456 * 00457 * The data is accessible through the <em>axis</em>-struct (object.axis.map & object.axis.sign). 00458 */ 00459 void BNO055::getOrientation(void) 00460 { 00461 m_i2c_master.i2c_readSeries(m_bno055_address, AXIS_MAP_CONFIG,register_data,LENGTH_2_BYTES); 00462 00463 #ifdef DEBUGGING_ENABLED 00464 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap Current\t 0x%02X", axis.map); 00465 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign Current\t 0x%02X", axis.sign); 00466 #endif 00467 00468 axis.map = register_data[0]; 00469 axis.sign = register_data[1]; 00470 } 00471 00472 /** 00473 * @brief Assign Axis to different Axes 00474 * 00475 * But it's not possible to map the two axes on the same axis. It will not return anything, even if it's correct. 00476 * 00477 * Possible values are: <code>X_AXIS, Y_AXIS, Z_AXIS</code> 00478 * 00479 * @param x_axis New Axis which will be applied to the X-Axis 00480 * @param y_axis New Axis which will be applied to the Y-Axis 00481 * @param z_axis New Axis which will be applied to the Z-Axis 00482 */ 00483 void BNO055::assignAxis(bno055_axis_t x_axis, bno055_axis_t y_axis, bno055_axis_t z_axis) 00484 { 00485 //check if multiple axis have the same remap-axis. If true, then this part can be skipped, as 00486 //it's useless (the chip reverts to the previous state) by the datasheet and doing it now saves some time 00487 if((x_axis == y_axis) || (x_axis == z_axis) || (z_axis == y_axis)) 00488 { 00489 #ifdef DEBUGGING_ENABLED 00490 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis \tmultiple Axis"); 00491 #endif 00492 } 00493 else 00494 { 00495 00496 axis.map = x_axis + (y_axis << 2) + (z_axis << 4); 00497 #ifdef DEBUGGING_ENABLED 00498 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis new \t%3i",axis.map); 00499 #endif 00500 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS); 00501 } 00502 } 00503 00504 /** 00505 * @brief Inverts the Axis 00506 * 00507 * Possible values are: <code>POSITIVE & NEGATIVE</code> 00508 * 00509 * @param x_sign New Sign for the X-Axis 00510 * @param y_sign New Sign for the Y-Axis 00511 * @param z_sign New Sign for the Z-Axis 00512 */ 00513 void BNO055::setAxisSign(bno055_axis_sign_t x_sign, bno055_axis_sign_t y_sign, bno055_axis_sign_t z_sign) 00514 { 00515 axis.sign = x_sign + (y_sign << 1) + (z_sign << 2); 00516 #ifdef DEBUGGING_ENABLED 00517 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tAxis Sign new \t%3i",axis.sign); 00518 #endif 00519 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS); 00520 } 00521 00522 /** 00523 * @brief Gets the System's Calibration Status 00524 * 00525 * The data is accessible through the <em>calibration</em>-struct (object.calibration.status, object.calibration.system, object.calibration.gyro, object.calibration.acceleration, object.calibration.magneto) 00526 * 00527 */ 00528 void BNO055::getCalibrationStatus(void) 00529 { 00530 calibration.status = m_i2c_master.i2c_read(m_bno055_address, CALIB_STAT); 00531 calibration.system = (calibration.status >> SHIFT_CALIB_SYSTEM) & MASK_CALIBRATION_BITS; 00532 calibration.gyro = (calibration.status >> SHIFT_CALIB_GYRO) & MASK_CALIBRATION_BITS; 00533 calibration.acceleration = (calibration.status >> SHIFT_CALIB_ACCEL) & MASK_CALIBRATION_BITS; 00534 calibration.magneto = calibration.status & MASK_CALIBRATION_BITS; 00535 00536 #ifdef DEBUGGING_ENABLED 00537 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[S] %1i [G] %1i [A] %1i [M] %1i\t[REG]%3X",calibration.system 00538 ,calibration.gyro 00539 ,calibration.acceleration 00540 ,calibration.magneto 00541 ,calibration.status 00542 ); 00543 #endif 00544 } 00545 00546 /** 00547 * @brief Gets the Interrupt-flags 00548 * 00549 * The data is accessible through the <em>interrupt</em>-struct (object.interrupt.xxx) 00550 */ 00551 void BNO055::getInterruptFlag(void) 00552 { 00553 setPage(PAGE_1); 00554 interrupt.status = m_i2c_master.i2c_read(m_bno055_address, INT_STATUS); 00555 00556 interrupt.gyroscope.any_motion = ((interrupt.status & MASK_GYRO_ANY_MOTION ) == GYRO_ANY_MOTION ); 00557 interrupt.gyroscope.high_rate = ((interrupt.status & MASK_GYRO_HIGH_RATE ) == GYRO_HIGH_RATE ); 00558 interrupt.acceleration.high_g = ((interrupt.status & MASK_ACCEL_HIGH_G ) == ACCEL_HIGH_G ); 00559 interrupt.acceleration.any_motion = ((interrupt.status & MASK_ACCEL_ANY_MOTION) == ACCEL_ANY_MOTION); 00560 interrupt.acceleration.no_motion = ((interrupt.status & MASK_ACCEL_NO_MOTION ) == ACCEL_NO_MOTION ); 00561 00562 #ifdef DEBUGGING_ENABLED 00563 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Interrupt Flags\n\r\tGyro Any Motion\t\t%1u\n\r\tGyro High Rate\t\t%1u\n\r\tAccel High G-Force\t%1u\n\r\tAccel Any Motion\t%1u\n\r\tAccel No Motion\t\t%1u\n\r\tRegister Value\t\t0x%02X", 00564 interrupt.gyroscope.any_motion, 00565 interrupt.gyroscope.high_rate, 00566 interrupt.acceleration.high_g, 00567 interrupt.acceleration.any_motion, 00568 interrupt.acceleration.no_motion, 00569 interrupt.status 00570 ); 00571 #endif 00572 00573 setPage(PAGE_0); 00574 } 00575 00576 /** 00577 * @brief Returns the enabled Interrupt 00578 * 00579 * @return uint8_t The value containing the enabled interrupts (see datasheet) 00580 */ 00581 uint8_t BNO055::getEnabledInterrupts(void) 00582 { 00583 setPage(PAGE_1); 00584 uint8_t data_interrupt_enable = m_i2c_master.i2c_read(m_bno055_address, INT_EN); 00585 setPage(PAGE_0); 00586 00587 #ifdef DEBUGGING_ENABLED 00588 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Enabled Interrupt\t0x%02X",data_interrupt_enable); 00589 #endif 00590 00591 return data_interrupt_enable; 00592 } 00593 00594 /** 00595 * @brief Configure the Interrupts 00596 * 00597 * @param accel_no_motion <code>ENABLE</code> to enable interrupt, <code>DISABLE</code> to disable interrupt 00598 * @param accel_any_motion <code>ENABLE</code> to enable interrupt, <code>DISABLE</code> to disable interrupt 00599 * @param accel_high_g <code>ENABLE</code> to enable interrupt, <code>DISABLE</code> to disable interrupt 00600 * @param gyro_high_rate <code>ENABLE</code> to enable interrupt, <code>DISABLE</code> to disable interrupt 00601 * @param gyro_any_motion <code>ENABLE</code> to enable interrupt, <code>DISABLE</code> to disable interrupt 00602 */ 00603 void BNO055::setEnableInterrupts(bno055_enable_t accel_no_motion, 00604 bno055_enable_t accel_any_motion, 00605 bno055_enable_t accel_high_g, 00606 bno055_enable_t gyro_high_rate, 00607 bno055_enable_t gyro_any_motion 00608 ) 00609 { 00610 setPage(PAGE_1); 00611 uint8_t data_interrupt_enable = (gyro_any_motion << SHIFT_INT_GYRO_AM) + (gyro_high_rate << SHIFT_INT_GYRO_HR) + (accel_high_g << SHIFT_INT_ACCEL_HG) + (accel_any_motion << SHIFT_INT_ACCEL_AM) + (accel_no_motion << SHIFT_INT_ACCEL_NM); 00612 m_i2c_master.i2c_write(m_bno055_address, INT_EN, data_interrupt_enable); 00613 00614 #ifdef DEBUGGING_ENABLED 00615 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Enabled Interrupt\t0x%02X",data_interrupt_enable); 00616 #endif 00617 00618 setPage(PAGE_0); 00619 } 00620 00621 /** 00622 * @brief Sets the Interrupt Mask 00623 * 00624 * This function enables or disabled the changing of the Interrupt-pin. 00625 * 00626 * @param accel_no_motion <code>ENABLE</code> to enable interrupt-calling, <code>DISABLE</code> to disable it 00627 * @param accel_any_motion <code>ENABLE</code> to enable interrupt-calling, <code>DISABLE</code> to disable it 00628 * @param accel_high_g <code>ENABLE</code> to enable interrupt-calling, <code>DISABLE</code> to disable it 00629 * @param gyro_high_rate <code>ENABLE</code> to enable interrupt-calling, <code>DISABLE</code> to disable it 00630 * @param gyro_any_motion <code>ENABLE</code> to enable interrupt-calling, <code>DISABLE</code> to disable it 00631 */ 00632 void BNO055::setInterruptMask(bno055_enable_t accel_no_motion, 00633 bno055_enable_t accel_any_motion, 00634 bno055_enable_t accel_high_g, 00635 bno055_enable_t gyro_high_rate, 00636 bno055_enable_t gyro_any_motion 00637 ) 00638 { 00639 setPage(PAGE_1); 00640 uint8_t data_interrupt_mask = (gyro_any_motion << SHIFT_INT_GYRO_AM) + (gyro_high_rate << SHIFT_INT_GYRO_HR) + (accel_high_g << SHIFT_INT_ACCEL_HG) + (accel_any_motion << SHIFT_INT_ACCEL_AM) + (accel_no_motion << SHIFT_INT_ACCEL_NM); 00641 m_i2c_master.i2c_write(m_bno055_address, INT_MSK, data_interrupt_mask); 00642 00643 #ifdef DEBUGGING_ENABLED 00644 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Interrupt Mask\t0x%02X",data_interrupt_mask); 00645 #endif 00646 00647 setPage(PAGE_0); 00648 } 00649 00650 /** 00651 * @brief Returns current Interrupt Mask 00652 * 00653 * @return uint8_t Current Interrupt Mask (see datasheet) 00654 */ 00655 uint8_t BNO055::getInterruptMask(void) 00656 { 00657 setPage(PAGE_1); 00658 uint8_t data_interrupt_mask = m_i2c_master.i2c_read(m_bno055_address, INT_MSK); 00659 00660 #ifdef DEBUGGING_ENABLED 00661 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Interrupt Mask\t0x%02X",data_interrupt_mask); 00662 #endif 00663 00664 setPage(PAGE_0); 00665 00666 return data_interrupt_mask; 00667 } 00668 00669 00670 void BNO055::configAccelerationInterrupt(bno055_config_int_axis_t high_axis, 00671 bno055_config_int_axis_t am_nm_axis, 00672 bno055_any_motion_sample_t am_dur 00673 ) 00674 { 00675 setPage(PAGE_1); 00676 uint8_t data = am_dur + (am_nm_axis << SHIFT_AM_NM_AXIS) + (high_axis << SHIFT_HIGH_AXIS); 00677 m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data); 00678 00679 #ifdef DEBUGGING_ENABLED 00680 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tAcceleration Interrupt Configured\n\r\tHigh G Axis\t\t0x%02X\n\r\tAny & No Motion axis\t0x%02X\n\r\tAny Motion Samples\t0x%02X\n\r\tRegister Value\t\t0x%02X", high_axis, am_nm_axis, am_dur,data); 00681 #endif 00682 00683 setPage(PAGE_0); 00684 } 00685 00686 void BNO055::configGyroscopeInterrupt(bno055_config_int_axis_t hr_axis, 00687 bno055_config_int_axis_t am_axis, 00688 bool high_rate_unfilt, 00689 bool any_motion_unfilt 00690 ) 00691 { 00692 setPage(PAGE_1); 00693 uint8_t data = am_axis + (hr_axis << SHIFT_HIGH_RATE_AXIS) + (static_cast<uint8_t>(high_rate_unfilt) << SHIFT_HIGH_RATE_FILT) + (static_cast<uint8_t>(any_motion_unfilt) << SHIFT_AM_FILT); 00694 m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data); 00695 00696 #ifdef DEBUGGING_ENABLED 00697 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tGyroscope Interrupt Configured\n\r\tHigh Rate Axis\t\t0x%02X\n\r\tAny Motion axis\t\t0x%02X\n\r\tHigh Rate Unfilterd\t0x%02X\n\r\tAny Motion Unfiltered\t0x%02X\n\r\tRegister Value\t\t0x%02X", hr_axis, am_axis, high_rate_unfilt, any_motion_unfilt, data); 00698 #endif 00699 00700 setPage(PAGE_0); 00701 } 00702 00703 //ACCELERATION 00704 /** 00705 * @brief Returns Acceleration Data 00706 * 00707 * Data is accessible through the <em>accel</em>-struct (object.accel.x, object.accel.y, object.accel.z), after calling this function 00708 */ 00709 void BNO055::getAcceleration(void) 00710 { 00711 if(format.acceleration == ACCELERATION) 00712 { 00713 get(ACC_DATA_VECTOR, accel, ACCELERATION_FORMAT, false); 00714 } 00715 else if(format.acceleration == MILLI_G_FORCE) 00716 { 00717 get(ACC_DATA_VECTOR, accel, MILLI_G_FORCE_FORMAT, false); 00718 } 00719 00720 #ifdef DEBUGGING_ENABLED 00721 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[ACCELERAION]\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", accel.x, accel.y, accel.z); 00722 #endif 00723 } 00724 00725 //MAGNETOMETER 00726 /** 00727 * @brief Returns Magnetometer Data 00728 * 00729 * Data is accessible through the <em>magneto</em>-struct (object.magneto.x, object.magneto.y, object.magneto.z), after calling this function 00730 */ 00731 void BNO055::getMagnetometer(void) 00732 { 00733 get(MAG_DATA_VECTOR, magneto, MICRO_TESLA_FORMAT, false); 00734 00735 #ifdef DEBUGGING_ENABLED 00736 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[MAG]\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", magneto.x, magneto.y, magneto.z); 00737 #endif 00738 } 00739 00740 //GYROSCOPE 00741 /** 00742 * @brief Returns Gyroscope Data 00743 * 00744 * Data is accessible through the <em>gyro</em>-struct (object.gyro.x, object.gyro.y, object.gyro.z), after calling this function 00745 */ 00746 void BNO055::getGyroscope(void) 00747 { 00748 if(format.gyroscope == DEGREE_PER_SEC) 00749 { 00750 get(GYR_DATA_VECTOR, gyro, GYRO_DEGREE_PER_SEC_FORMAT, false); 00751 } 00752 else if(format.gyroscope == RADIAN_PER_SEC) 00753 { 00754 get(GYR_DATA_VECTOR, gyro, GYRO_RADIAN_PER_SEC_FORMAT, false); 00755 } 00756 #ifdef DEBUGGING_ENABLED 00757 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[GYRO]\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", gyro.x, gyro.y, gyro.z); 00758 #endif 00759 } 00760 00761 //EULER-DEGREES 00762 /** 00763 * @brief Returns Euler Degree Data 00764 * 00765 * Data is accessible through the <em>euler</em>-struct (object.euler.x, object.euler.y, object.euler.z), after calling this function 00766 */ 00767 void BNO055::getEulerDegrees(void) 00768 { 00769 if(format.euler == DEGREES) 00770 { 00771 get(EUL_DATA_VECTOR, euler, EULER_DEGREES_FORMAT, true); 00772 } 00773 else if(format.euler == RADIANS) 00774 { 00775 get(EUL_DATA_VECTOR, euler, EULER_RADIANS_FORMAT, true); 00776 } 00777 #ifdef DEBUGGING_ENABLED 00778 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[EULER]\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", euler.x, euler.y, euler.z); 00779 #endif 00780 } 00781 00782 //QUATERNION 00783 /** 00784 * @brief Returns Quaternion Data 00785 * 00786 * Data is accessible through the <em>quaternion</em>-struct (object.quaternion.x, object.quaternion.y, object.quaternion.z), after calling this function 00787 */ 00788 void BNO055::getQuaternion(void) 00789 { 00790 get(QUA_DATA_VECTOR, quaternion, QUATERNION_FORMAT); 00791 00792 #ifdef DEBUGGING_ENABLED 00793 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[QUAT]\t[W] %4.3f\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", quaternion.w, quaternion.x, quaternion.y, quaternion.z); 00794 #endif 00795 } 00796 00797 //LINEAR ACCELERATION 00798 /** 00799 * @brief Returns Linear Acceleration Data 00800 * 00801 * Data is accessible through the <em>linear_accel</em>-struct (object.linear_accel.x, object.linear_accel.y, object.linear_accel.z), after calling this function 00802 */ 00803 void BNO055::getLinearAcceleration(void) 00804 { 00805 if(format.acceleration == ACCELERATION) 00806 { 00807 get(LIA_DATA_VECTOR, linear_accel, ACCELERATION_FORMAT, false); 00808 } 00809 else if(format.acceleration == MILLI_G_FORCE) 00810 { 00811 get(LIA_DATA_VECTOR, linear_accel, MILLI_G_FORCE_FORMAT, false); 00812 } 00813 #ifdef DEBUGGING_ENABLED 00814 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[LINEAR ACCELERATION]\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", linear_accel.x, linear_accel.y, linear_accel.z); 00815 #endif 00816 } 00817 00818 //GRAVITY VECTOR 00819 /** 00820 * @brief Returns Gravity Vector Data 00821 * 00822 * Data is accessible through the <em>gravity_vector</em>-struct (object.gravity_vector.x, object.gravity_vector.y, object.gravity_vector.z), after calling this function 00823 */ 00824 void BNO055::getGravityVector(void) 00825 { 00826 if(format.acceleration == ACCELERATION) 00827 { 00828 get(ACC_DATA_VECTOR, gravity_vector, ACCELERATION_FORMAT, false); 00829 } 00830 else if(format.acceleration == MILLI_G_FORCE) 00831 { 00832 get(ACC_DATA_VECTOR, gravity_vector, MILLI_G_FORCE_FORMAT, false); 00833 } 00834 #ifdef DEBUGGING_ENABLED 00835 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[LINEAR ACCELERATION]\t[X] %4.3f\t[Y] %4.3f\t[Z] %4.3f", gravity_vector.x, gravity_vector.y, gravity_vector.z); 00836 #endif 00837 } 00838 00839 //TEMPERATURE 00840 /** 00841 * @brief Returns Temperature Data 00842 * 00843 * Data is accessible through the <em>temperature</em>-struct (object.temperature), after calling this function 00844 */ 00845 void BNO055::getTemperature(void) 00846 { 00847 m_i2c_master.i2c_readSeries(m_bno055_address, TEMP, register_data, LENGTH_1_BYTES); 00848 if(format.temperature == CELSIUS) 00849 { 00850 temperature = static_cast<float>(register_data[0]) * TEMPERATURE_CELSIUS_FORMAT; 00851 } 00852 if(format.temperature == FAHRENHEIT) 00853 { 00854 temperature = static_cast<float>(register_data[0]) * TEMPERATURE_FAHRENHEIT_FORMAT; 00855 } 00856 } 00857 00858 void BNO055::get(bno055_reg_t address, bno055_data_s& data, const float format, bool is_euler) 00859 { 00860 m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_6_BYTES); 00861 00862 for(arrayIndex = 0 ; arrayIndex < (LENGTH_6_BYTES/2) ; arrayIndex++) 00863 { 00864 sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE); 00865 } 00866 00867 if(is_euler) 00868 { 00869 data.z = format * static_cast<float>(sensor_data[0]); 00870 data.y = format * static_cast<float>(sensor_data[1]); 00871 data.x = format * static_cast<float>(sensor_data[2]); 00872 } 00873 else 00874 { 00875 data.x = format * static_cast<float>(sensor_data[0]); 00876 data.y = format * static_cast<float>(sensor_data[1]); 00877 data.z = format * static_cast<float>(sensor_data[2]); 00878 } 00879 } 00880 00881 void BNO055::get(bno055_reg_t address, bno055_quaternion_s& data, const float format) 00882 { 00883 m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_8_BYTES); 00884 00885 for(arrayIndex = 0 ; arrayIndex < (LENGTH_8_BYTES/2) ; arrayIndex++) 00886 { 00887 sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE); 00888 } 00889 00890 data.w = format * static_cast<float>(sensor_data[0]); 00891 data.x = format * static_cast<float>(sensor_data[1]); 00892 data.y = format * static_cast<float>(sensor_data[2]); 00893 data.z = format * static_cast<float>(sensor_data[3]); 00894 } 00895
Generated on Tue Jul 19 2022 06:30:21 by
1.7.2