Joel von Rotz / BNO055

A library of the 9-Axis Sensor BNO055 from Bosch Sensortec.

Requires the I2C-Master library - Link

bno055.cpp@6:3dcf44b8289a, 2019-07-11 (annotated)

Committer:
joelvonrotz
Date:
Thu Jul 11 11:31:55 2019 +0000
Revision:
6:3dcf44b8289a
Parent:
3:ffc741483987
Child:
7:ccb2dcdfac08
asd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joelvonrotz 0:a2a71c38065e 1 /**
joelvonrotz 0:a2a71c38065e 2 * @brief Implementation of BNO055 Class.
joelvonrotz 0:a2a71c38065e 3 *
joelvonrotz 0:a2a71c38065e 4 * @file bno055.cpp
joelvonrotz 0:a2a71c38065e 5 * @author Joel von Rotz
joelvonrotz 0:a2a71c38065e 6 * @date 18.07.2018
joelvonrotz 0:a2a71c38065e 7 */
joelvonrotz 0:a2a71c38065e 8 #include "mbed.h"
joelvonrotz 0:a2a71c38065e 9 #include "bno055.h"
joelvonrotz 0:a2a71c38065e 10
joelvonrotz 0:a2a71c38065e 11 /* TODO ----------------------------------
joelvonrotz 0:a2a71c38065e 12 * [+] comment blocks for all function
joelvonrotz 0:a2a71c38065e 13 * [+] error handler
joelvonrotz 0:a2a71c38065e 14 * [+] finish up documentation
joelvonrotz 0:a2a71c38065e 15 * [~] rename variables to more understandable names
joelvonrotz 0:a2a71c38065e 16 * [~] replace magic numbers with constants
joelvonrotz 0:a2a71c38065e 17 * TODONE --------------------------------
joelvonrotz 0:a2a71c38065e 18 *
joelvonrotz 0:a2a71c38065e 19 * ---------------------------------------
joelvonrotz 0:a2a71c38065e 20 */
joelvonrotz 0:a2a71c38065e 21
joelvonrotz 0:a2a71c38065e 22 #ifdef DEBUGGING_ENABLED
joelvonrotz 1:6c64ebf1f2b5 23 /**
joelvonrotz 1:6c64ebf1f2b5 24 * @brief Construct a new BNO055::BNO055 object
joelvonrotz 1:6c64ebf1f2b5 25 *
joelvonrotz 1:6c64ebf1f2b5 26 * This is the Debugging-version of the BNO055 class. The constructor requires an additional Serial-Object.
joelvonrotz 1:6c64ebf1f2b5 27 *
joelvonrotz 1:6c64ebf1f2b5 28 * @param slave_address The slave-address (determined by the Address-Pin on the chip)
joelvonrotz 1:6c64ebf1f2b5 29 * @param i2c_master I2C-Master object
joelvonrotz 1:6c64ebf1f2b5 30 * @param ResetPin Reference of the Output-Pin, which is connected to the Reset-Pin of the Sensor
joelvonrotz 1:6c64ebf1f2b5 31 * @param external_clk Boolean to determine if the Sensor uses an external 32kHz oscillator
joelvonrotz 1:6c64ebf1f2b5 32 * @param DEBUG_SERIAL Reference of the Serial-object used to debug the different functions.
joelvonrotz 1:6c64ebf1f2b5 33 */
joelvonrotz 1:6c64ebf1f2b5 34 BNO055::BNO055(uint8_t slave_address, I2C_Master& i2c_master, DigitalOut& ResetPin, bool external_clk, Serial& DEBUG_SERIAL) :
joelvonrotz 1:6c64ebf1f2b5 35 m_i2c_master(i2c_master),
joelvonrotz 0:a2a71c38065e 36 m_ResetPin(ResetPin),
joelvonrotz 0:a2a71c38065e 37 m_DEBUG_SERIAL(DEBUG_SERIAL),
joelvonrotz 0:a2a71c38065e 38 m_bno055_address(slave_address << 1)
joelvonrotz 0:a2a71c38065e 39 {
joelvonrotz 0:a2a71c38065e 40 ResetPin = 1;
joelvonrotz 0:a2a71c38065e 41 wait_ms(500);
joelvonrotz 0:a2a71c38065e 42
joelvonrotz 0:a2a71c38065e 43 #ifdef HARDWARE_RESET
joelvonrotz 0:a2a71c38065e 44 resetHW();
joelvonrotz 0:a2a71c38065e 45 #else
joelvonrotz 0:a2a71c38065e 46 setOperationMode(OPERATION_MODE_CONFIGMODE);
joelvonrotz 0:a2a71c38065e 47 resetSW();
joelvonrotz 0:a2a71c38065e 48 #endif
joelvonrotz 0:a2a71c38065e 49
joelvonrotz 0:a2a71c38065e 50 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 51 getIDs();
joelvonrotz 0:a2a71c38065e 52 setOperationMode(OPERATION_MODE_CONFIGMODE);
joelvonrotz 0:a2a71c38065e 53 setPowerMode(POWER_NORMAL);
joelvonrotz 0:a2a71c38065e 54 setUnitFormat(WINDOWS, CELSIUS, DEGREES, DEGREE_PER_SEC, ACCELERATION);
joelvonrotz 0:a2a71c38065e 55
joelvonrotz 0:a2a71c38065e 56 useExternalOscillator(external_clk);
joelvonrotz 0:a2a71c38065e 57
joelvonrotz 0:a2a71c38065e 58 setOrientation(REMAP_OPTION_P1);
joelvonrotz 0:a2a71c38065e 59 }
joelvonrotz 0:a2a71c38065e 60 #else
joelvonrotz 1:6c64ebf1f2b5 61 /**
joelvonrotz 1:6c64ebf1f2b5 62 * @brief Construct a new BNO055::BNO055 object
joelvonrotz 1:6c64ebf1f2b5 63 *
joelvonrotz 1:6c64ebf1f2b5 64 * @param slave_address The slave-address (determined by the Address-Pin on the chip)
joelvonrotz 1:6c64ebf1f2b5 65 * @param i2c_master I2C-Master object
joelvonrotz 1:6c64ebf1f2b5 66 * @param ResetPin Reference of the Output-Pin, which is connected to the Reset-Pin of the Sensor
joelvonrotz 1:6c64ebf1f2b5 67 * @param external_clk Boolean to determine if the Sensor uses an external 32kHz oscillator
joelvonrotz 1:6c64ebf1f2b5 68 */
joelvonrotz 0:a2a71c38065e 69 BNO055::BNO055(uint8_t slave_address, I2C_Master& i2c_master, DigitalOut& ResetPin, bool external_clk) :
joelvonrotz 0:a2a71c38065e 70 m_i2c_master(i2c_master),
joelvonrotz 0:a2a71c38065e 71 m_ResetPin(ResetPin),
joelvonrotz 0:a2a71c38065e 72 m_bno055_address(slave_address << 1)
joelvonrotz 0:a2a71c38065e 73 {
joelvonrotz 0:a2a71c38065e 74 ResetPin = 1;
joelvonrotz 0:a2a71c38065e 75 wait_ms(500);
joelvonrotz 0:a2a71c38065e 76
joelvonrotz 0:a2a71c38065e 77 #ifdef HARDWARE_RESET
joelvonrotz 0:a2a71c38065e 78 resetHW();
joelvonrotz 0:a2a71c38065e 79 #else
joelvonrotz 0:a2a71c38065e 80 setOperationMode(OPERATION_MODE_CONFIGMODE);
joelvonrotz 0:a2a71c38065e 81 resetSW();
joelvonrotz 0:a2a71c38065e 82 #endif
joelvonrotz 0:a2a71c38065e 83
joelvonrotz 0:a2a71c38065e 84 setPage();
joelvonrotz 0:a2a71c38065e 85 getIDs();
joelvonrotz 0:a2a71c38065e 86 setOperationMode(OPERATION_MODE_CONFIGMODE);
joelvonrotz 0:a2a71c38065e 87 setPowerMode(POWER_NORMAL);
joelvonrotz 0:a2a71c38065e 88 setUnitFormat(WINDOWS, CELSIUS, DEGREES, DEGREE_PER_SEC, ACCELERATION);
joelvonrotz 0:a2a71c38065e 89
joelvonrotz 0:a2a71c38065e 90 useExternalOscillator(external_clk);
joelvonrotz 0:a2a71c38065e 91
joelvonrotz 0:a2a71c38065e 92 setOrientation(REMAP_OPTION_P1);
joelvonrotz 0:a2a71c38065e 93 }
joelvonrotz 0:a2a71c38065e 94 #endif
joelvonrotz 0:a2a71c38065e 95
joelvonrotz 1:6c64ebf1f2b5 96 /**
joelvonrotz 1:6c64ebf1f2b5 97 * @brief Defines the format of the measurment units
joelvonrotz 1:6c64ebf1f2b5 98 *
joelvonrotz 3:ffc741483987 99 * Following is a table, which displays all the possible formats for each measurement units. The column <em>Code</em>
joelvonrotz 1:6c64ebf1f2b5 100 * contain the values, which are used in the programm.
joelvonrotz 1:6c64ebf1f2b5 101 *
joelvonrotz 3:ffc741483987 102 * <table>
joelvonrotz 6:3dcf44b8289a 103 * <caption>Measurement Unit Formats</caption>
joelvonrotz 3:ffc741483987 104 * <tr><th>Type <th>Formats <th>Code
joelvonrotz 6:3dcf44b8289a 105 * <tr><td>Orientation <td>Windows, Android - this changes the ranges of the axis <td>WINDOWS, ANDROID
joelvonrotz 3:ffc741483987 106 * <tr><td>Temperature <td>Celsius, Fahrenheit <td>CELSIUS, FAHRENHEIT
joelvonrotz 3:ffc741483987 107 * <tr><td>Euler <td>Degree, Radians <td>DEGREE, RADIANS
joelvonrotz 3:ffc741483987 108 * <tr><td>Gyroscope <td>degree per seconds, radian per seconds <td>DEGREE_PER_SEC, RADIAN_PER_SEC
joelvonrotz 3:ffc741483987 109 * <tr><td>Acceleration <td>acceleration (m/s2), milli g-force <td>ACCELERATION, MILLI_G_FORCE
joelvonrotz 3:ffc741483987 110 * </table>
joelvonrotz 1:6c64ebf1f2b5 111 *
joelvonrotz 1:6c64ebf1f2b5 112 * @param new_orientation_format
joelvonrotz 1:6c64ebf1f2b5 113 * @param new_temperature_format
joelvonrotz 1:6c64ebf1f2b5 114 * @param new_euler_format
joelvonrotz 1:6c64ebf1f2b5 115 * @param new_gyroscope_format
joelvonrotz 1:6c64ebf1f2b5 116 * @param new_acceleration_format
joelvonrotz 1:6c64ebf1f2b5 117 */
joelvonrotz 0:a2a71c38065e 118 void BNO055::setUnitFormat(bno055_orientation_t new_orientation_format,
joelvonrotz 0:a2a71c38065e 119 bno055_temperature_t new_temperature_format,
joelvonrotz 0:a2a71c38065e 120 bno055_euler_t new_euler_format,
joelvonrotz 0:a2a71c38065e 121 bno055_gyro_t new_gyroscope_format,
joelvonrotz 0:a2a71c38065e 122 bno055_acceleration_t new_acceleration_format)
joelvonrotz 0:a2a71c38065e 123 {
joelvonrotz 0:a2a71c38065e 124 uint8_t new_unit_data = new_orientation_format + new_temperature_format + new_euler_format + new_gyroscope_format + new_acceleration_format;
joelvonrotz 0:a2a71c38065e 125
joelvonrotz 0:a2a71c38065e 126 format.units = new_unit_data;
joelvonrotz 0:a2a71c38065e 127 format.orientation = new_orientation_format;
joelvonrotz 0:a2a71c38065e 128 format.temperature = new_temperature_format;
joelvonrotz 0:a2a71c38065e 129 format.euler = new_euler_format;
joelvonrotz 0:a2a71c38065e 130 format.gyroscope = new_gyroscope_format;
joelvonrotz 0:a2a71c38065e 131 format.acceleration = new_acceleration_format;
joelvonrotz 0:a2a71c38065e 132
joelvonrotz 0:a2a71c38065e 133
joelvonrotz 0:a2a71c38065e 134 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 135 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tUnits Set\t 0x%02X",new_unit_data);
joelvonrotz 0:a2a71c38065e 136 #endif
joelvonrotz 0:a2a71c38065e 137 m_i2c_master.i2c_writeBits(m_bno055_address, UNIT_SEL, new_unit_data, MASK_UNIT_SEL);
joelvonrotz 0:a2a71c38065e 138 }
joelvonrotz 0:a2a71c38065e 139
joelvonrotz 6:3dcf44b8289a 140 /**
joelvonrotz 6:3dcf44b8289a 141 * @brief Not Finished
joelvonrotz 6:3dcf44b8289a 142 */
joelvonrotz 0:a2a71c38065e 143 void BNO055::getUnitFormat(void)
joelvonrotz 0:a2a71c38065e 144 {
joelvonrotz 0:a2a71c38065e 145
joelvonrotz 0:a2a71c38065e 146 }
joelvonrotz 0:a2a71c38065e 147
joelvonrotz 6:3dcf44b8289a 148 /**
joelvonrotz 6:3dcf44b8289a 149 * @brief Get IDs
joelvonrotz 6:3dcf44b8289a 150 *
joelvonrotz 6:3dcf44b8289a 151 * Read the datasheet, what exactly these IDs are. The data is accessible through the <em>id</em>-struct (object.id.xxx).
joelvonrotz 6:3dcf44b8289a 152 */
joelvonrotz 0:a2a71c38065e 153 void BNO055::getIDs(void)
joelvonrotz 0:a2a71c38065e 154 {
joelvonrotz 0:a2a71c38065e 155 uint8_t dataArray[7];
joelvonrotz 0:a2a71c38065e 156 m_i2c_master.i2c_readSeries(m_bno055_address, CHIP_ID, dataArray, 7);
joelvonrotz 0:a2a71c38065e 157 id.chip = dataArray[0];
joelvonrotz 0:a2a71c38065e 158 id.accel = dataArray[1];
joelvonrotz 0:a2a71c38065e 159 id.magneto = dataArray[2];
joelvonrotz 0:a2a71c38065e 160 id.gyro = dataArray[3];
joelvonrotz 0:a2a71c38065e 161 id.sw_rev = dataArray[4] + (static_cast<uint16_t>(dataArray[5]) << 8);
joelvonrotz 0:a2a71c38065e 162 id.bl_rev = dataArray[6];
joelvonrotz 0:a2a71c38065e 163
joelvonrotz 0:a2a71c38065e 164 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 165 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tIDs---------------------");
joelvonrotz 0:a2a71c38065e 166 m_DEBUG_SERIAL.printf("\r\n\tChip\t\t 0x%02X",id.chip);
joelvonrotz 0:a2a71c38065e 167 m_DEBUG_SERIAL.printf("\r\n\tAcceleraiton\t 0x%02X",id.accel);
joelvonrotz 0:a2a71c38065e 168 m_DEBUG_SERIAL.printf("\r\n\tMagnetometer\t 0x%02X",id.magneto);
joelvonrotz 0:a2a71c38065e 169 m_DEBUG_SERIAL.printf("\r\n\tGyroscope\t 0x%02X",id.gyro);
joelvonrotz 0:a2a71c38065e 170 m_DEBUG_SERIAL.printf("\r\n\tSofware Rev.\t 0x%04X",id.sw_rev);
joelvonrotz 0:a2a71c38065e 171 m_DEBUG_SERIAL.printf("\r\n\tBootloader\t 0x%02X",id.bl_rev);
joelvonrotz 0:a2a71c38065e 172 #endif
joelvonrotz 0:a2a71c38065e 173 }
joelvonrotz 0:a2a71c38065e 174
joelvonrotz 6:3dcf44b8289a 175 /**
joelvonrotz 6:3dcf44b8289a 176 * @brief Sets the Power Mode
joelvonrotz 6:3dcf44b8289a 177 *
joelvonrotz 6:3dcf44b8289a 178 * 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.
joelvonrotz 6:3dcf44b8289a 179 *
joelvonrotz 6:3dcf44b8289a 180 * <table>
joelvonrotz 6:3dcf44b8289a 181 * <caption>Power Modes</caption>
joelvonrotz 6:3dcf44b8289a 182 * <tr><th>Type <th>Description
joelvonrotz 6:3dcf44b8289a 183 * <tr><td>POWER_NORMAL <td>All sensors are activated
joelvonrotz 6:3dcf44b8289a 184 * <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.
joelvonrotz 6:3dcf44b8289a 185 * <tr><td>POWER_SUSPEND_MODE <td>System is paused and sensors and mcu is put into sleep mode.
joelvonrotz 6:3dcf44b8289a 186 * </table>
joelvonrotz 6:3dcf44b8289a 187 *
joelvonrotz 6:3dcf44b8289a 188 * @param new_power_mode New Power Mode
joelvonrotz 6:3dcf44b8289a 189 */
joelvonrotz 0:a2a71c38065e 190 void BNO055::setPowerMode(bno055_powermode_t new_power_mode)
joelvonrotz 0:a2a71c38065e 191 {
joelvonrotz 0:a2a71c38065e 192 if(new_power_mode != mode.power)
joelvonrotz 0:a2a71c38065e 193 {
joelvonrotz 0:a2a71c38065e 194 do
joelvonrotz 0:a2a71c38065e 195 {
joelvonrotz 0:a2a71c38065e 196 m_i2c_master.i2c_writeBits(m_bno055_address, PWR_MODE, new_power_mode, MASK_POWER_MODE);
joelvonrotz 0:a2a71c38065e 197 wait_ms(50.0);
joelvonrotz 0:a2a71c38065e 198 }
joelvonrotz 0:a2a71c38065e 199 while((m_i2c_master.i2c_read(m_bno055_address, PWR_MODE) & MASK_POWER_MODE) != new_power_mode);
joelvonrotz 0:a2a71c38065e 200 //and the old operation mode should only be updated, when there's a new mode.
joelvonrotz 0:a2a71c38065e 201 mode.power = new_power_mode;
joelvonrotz 0:a2a71c38065e 202
joelvonrotz 0:a2a71c38065e 203 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 204 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode new\t %3i", new_power_mode);
joelvonrotz 0:a2a71c38065e 205 #endif
joelvonrotz 0:a2a71c38065e 206 }
joelvonrotz 0:a2a71c38065e 207 else
joelvonrotz 0:a2a71c38065e 208 {
joelvonrotz 0:a2a71c38065e 209 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 210 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode\t no change");
joelvonrotz 0:a2a71c38065e 211 #endif
joelvonrotz 0:a2a71c38065e 212 }
joelvonrotz 0:a2a71c38065e 213 }
joelvonrotz 0:a2a71c38065e 214
joelvonrotz 6:3dcf44b8289a 215 /**
joelvonrotz 6:3dcf44b8289a 216 * @brief Returns the currently used Power Mode
joelvonrotz 6:3dcf44b8289a 217 *
joelvonrotz 6:3dcf44b8289a 218 * @return uint8_t Currently used Power Mode
joelvonrotz 6:3dcf44b8289a 219 */
joelvonrotz 0:a2a71c38065e 220 uint8_t BNO055::getPowerMode(void)
joelvonrotz 0:a2a71c38065e 221 {
joelvonrotz 0:a2a71c38065e 222 mode.power = m_i2c_master.i2c_read(m_bno055_address, PWR_MODE);
joelvonrotz 0:a2a71c38065e 223 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 224 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpwrMode current\t %3i",mode.power);
joelvonrotz 0:a2a71c38065e 225 #endif
joelvonrotz 0:a2a71c38065e 226 return mode.power;
joelvonrotz 0:a2a71c38065e 227 }
joelvonrotz 0:a2a71c38065e 228
joelvonrotz 0:a2a71c38065e 229 void BNO055::setOperationMode(bno055_opr_mode_t new_opr_mode)
joelvonrotz 0:a2a71c38065e 230 {
joelvonrotz 0:a2a71c38065e 231 //trying to write a mode, that is already being used, doesn't make any sense and can be ignored.
joelvonrotz 0:a2a71c38065e 232 if(new_opr_mode != mode.operation)
joelvonrotz 0:a2a71c38065e 233 {
joelvonrotz 0:a2a71c38065e 234 do
joelvonrotz 0:a2a71c38065e 235 {
joelvonrotz 0:a2a71c38065e 236 m_i2c_master.i2c_writeBits(m_bno055_address, OPR_MODE, new_opr_mode, MASK_OPERAITON_MODE);
joelvonrotz 0:a2a71c38065e 237 wait_ms(50.0);
joelvonrotz 0:a2a71c38065e 238 }
joelvonrotz 0:a2a71c38065e 239 while((m_i2c_master.i2c_read(m_bno055_address, OPR_MODE) & MASK_OPERAITON_MODE) != new_opr_mode);
joelvonrotz 0:a2a71c38065e 240 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 241 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode new\t %3i", new_opr_mode);
joelvonrotz 0:a2a71c38065e 242 #endif
joelvonrotz 0:a2a71c38065e 243
joelvonrotz 0:a2a71c38065e 244 //and the old operation mode should only be updated, when there's a new mode.
joelvonrotz 0:a2a71c38065e 245 mode.operation = new_opr_mode;
joelvonrotz 0:a2a71c38065e 246 }
joelvonrotz 0:a2a71c38065e 247 else
joelvonrotz 0:a2a71c38065e 248 {
joelvonrotz 0:a2a71c38065e 249 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 250 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode\t\t no change");
joelvonrotz 0:a2a71c38065e 251 #endif
joelvonrotz 0:a2a71c38065e 252 }
joelvonrotz 0:a2a71c38065e 253 }
joelvonrotz 0:a2a71c38065e 254
joelvonrotz 0:a2a71c38065e 255 uint8_t BNO055::getOperationMode(void)
joelvonrotz 0:a2a71c38065e 256 {
joelvonrotz 0:a2a71c38065e 257 mode.operation = m_i2c_master.i2c_read(m_bno055_address, OPR_MODE);
joelvonrotz 0:a2a71c38065e 258 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 259 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode current\t %3i", mode.operation);
joelvonrotz 0:a2a71c38065e 260 #endif
joelvonrotz 0:a2a71c38065e 261 return mode.operation;
joelvonrotz 0:a2a71c38065e 262 }
joelvonrotz 0:a2a71c38065e 263
joelvonrotz 0:a2a71c38065e 264 void BNO055::setPage(bno055_page_t new_page)
joelvonrotz 0:a2a71c38065e 265 {
joelvonrotz 0:a2a71c38065e 266 page = new_page;
joelvonrotz 0:a2a71c38065e 267
joelvonrotz 0:a2a71c38065e 268 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 269 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage set\t %3i", page);
joelvonrotz 0:a2a71c38065e 270 #endif
joelvonrotz 0:a2a71c38065e 271
joelvonrotz 0:a2a71c38065e 272 m_i2c_master.i2c_write(m_bno055_address, PAGE_ID, page);
joelvonrotz 0:a2a71c38065e 273 }
joelvonrotz 0:a2a71c38065e 274
joelvonrotz 0:a2a71c38065e 275 uint8_t BNO055::getPage(void)
joelvonrotz 0:a2a71c38065e 276 {
joelvonrotz 0:a2a71c38065e 277 page = m_i2c_master.i2c_read(m_bno055_address, PAGE_ID);
joelvonrotz 0:a2a71c38065e 278
joelvonrotz 0:a2a71c38065e 279 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 280 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage current\t %3i",page);
joelvonrotz 0:a2a71c38065e 281 #endif
joelvonrotz 0:a2a71c38065e 282
joelvonrotz 0:a2a71c38065e 283 return page;
joelvonrotz 0:a2a71c38065e 284 }
joelvonrotz 0:a2a71c38065e 285
joelvonrotz 0:a2a71c38065e 286 uint8_t BNO055::getSystemStatus(void)
joelvonrotz 0:a2a71c38065e 287 {
joelvonrotz 0:a2a71c38065e 288 system.status = m_i2c_master.i2c_read(m_bno055_address, SYS_STATUS);
joelvonrotz 0:a2a71c38065e 289 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 290 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysStatus\t %3i",system.status);
joelvonrotz 0:a2a71c38065e 291 #endif
joelvonrotz 0:a2a71c38065e 292 return system.status;
joelvonrotz 0:a2a71c38065e 293 }
joelvonrotz 0:a2a71c38065e 294
joelvonrotz 0:a2a71c38065e 295 uint8_t BNO055::getSystemError(void)
joelvonrotz 0:a2a71c38065e 296 {
joelvonrotz 0:a2a71c38065e 297 system.error = m_i2c_master.i2c_read(m_bno055_address, SYS_ERR);
joelvonrotz 0:a2a71c38065e 298 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 299 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysError\t %3i",system.error);
joelvonrotz 0:a2a71c38065e 300 #endif
joelvonrotz 0:a2a71c38065e 301 return system.error;
joelvonrotz 0:a2a71c38065e 302 }
joelvonrotz 0:a2a71c38065e 303
joelvonrotz 0:a2a71c38065e 304 void BNO055::useExternalOscillator(bool enabled)
joelvonrotz 0:a2a71c38065e 305 {
joelvonrotz 0:a2a71c38065e 306 char data = static_cast<char>(enabled) << 7;
joelvonrotz 0:a2a71c38065e 307 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 308 if(enabled)
joelvonrotz 0:a2a71c38065e 309 {
joelvonrotz 0:a2a71c38065e 310 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source\t active");
joelvonrotz 0:a2a71c38065e 311 }
joelvonrotz 0:a2a71c38065e 312 else
joelvonrotz 0:a2a71c38065e 313 {
joelvonrotz 0:a2a71c38065e 314 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source \tinactive");
joelvonrotz 0:a2a71c38065e 315 }
joelvonrotz 0:a2a71c38065e 316 #endif
joelvonrotz 0:a2a71c38065e 317 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, data, MASK_EXT_CLOCK);
joelvonrotz 0:a2a71c38065e 318 }
joelvonrotz 0:a2a71c38065e 319
joelvonrotz 0:a2a71c38065e 320 void BNO055::resetSW(void)
joelvonrotz 0:a2a71c38065e 321 {
joelvonrotz 0:a2a71c38065e 322 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 323 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSoftware Reset");
joelvonrotz 0:a2a71c38065e 324 #endif
joelvonrotz 0:a2a71c38065e 325
joelvonrotz 0:a2a71c38065e 326 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x20, MASK_SYSTEM_RESET);
joelvonrotz 0:a2a71c38065e 327
joelvonrotz 0:a2a71c38065e 328 //datasheet (p.13 of revision 14) indicate a Power-On-Reset-time of 650ms
joelvonrotz 0:a2a71c38065e 329 //from Reset to Config Mode. By constantly reading the chip id, it can be
joelvonrotz 0:a2a71c38065e 330 //determined, when the chip is fully functional again.
joelvonrotz 0:a2a71c38065e 331 while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID)
joelvonrotz 0:a2a71c38065e 332 {
joelvonrotz 0:a2a71c38065e 333 wait_ms(10.0);
joelvonrotz 0:a2a71c38065e 334 }
joelvonrotz 0:a2a71c38065e 335
joelvonrotz 0:a2a71c38065e 336 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x00, MASK_SYSTEM_RESET);
joelvonrotz 0:a2a71c38065e 337 }
joelvonrotz 0:a2a71c38065e 338
joelvonrotz 0:a2a71c38065e 339 void BNO055::resetHW(void)
joelvonrotz 0:a2a71c38065e 340 {
joelvonrotz 0:a2a71c38065e 341 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 342 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tHardware Reset");
joelvonrotz 0:a2a71c38065e 343 #endif
joelvonrotz 0:a2a71c38065e 344 m_ResetPin = 0;
joelvonrotz 0:a2a71c38065e 345 wait_ms(10.0);
joelvonrotz 0:a2a71c38065e 346 m_ResetPin = 1;
joelvonrotz 0:a2a71c38065e 347 while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID)
joelvonrotz 0:a2a71c38065e 348 {
joelvonrotz 0:a2a71c38065e 349 wait_ms(10.0);
joelvonrotz 0:a2a71c38065e 350 }
joelvonrotz 0:a2a71c38065e 351
joelvonrotz 0:a2a71c38065e 352 }
joelvonrotz 0:a2a71c38065e 353
joelvonrotz 0:a2a71c38065e 354 void BNO055::setOrientation(bno055_remap_options_t orientation_placement)
joelvonrotz 0:a2a71c38065e 355 {
joelvonrotz 0:a2a71c38065e 356 axis.map = (orientation_placement >> SHIFT_1BYTE) & 0xFF;
joelvonrotz 0:a2a71c38065e 357 axis.sign = (orientation_placement & 0xFF);
joelvonrotz 0:a2a71c38065e 358
joelvonrotz 0:a2a71c38065e 359 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 360 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap New\t 0x%02X", axis.map);
joelvonrotz 0:a2a71c38065e 361 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign New\t 0x%02X", axis.sign);
joelvonrotz 0:a2a71c38065e 362 #endif
joelvonrotz 0:a2a71c38065e 363
joelvonrotz 0:a2a71c38065e 364 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS);
joelvonrotz 0:a2a71c38065e 365 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS);
joelvonrotz 0:a2a71c38065e 366 }
joelvonrotz 0:a2a71c38065e 367
joelvonrotz 0:a2a71c38065e 368 void BNO055::getOrientation(void)
joelvonrotz 0:a2a71c38065e 369 {
joelvonrotz 0:a2a71c38065e 370 m_i2c_master.i2c_readSeries(m_bno055_address, AXIS_MAP_CONFIG,register_data,LENGTH_2_BYTES);
joelvonrotz 0:a2a71c38065e 371
joelvonrotz 0:a2a71c38065e 372 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 373 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap Current\t 0x%02X", axis.map);
joelvonrotz 0:a2a71c38065e 374 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign Current\t 0x%02X", axis.sign);
joelvonrotz 0:a2a71c38065e 375 #endif
joelvonrotz 0:a2a71c38065e 376
joelvonrotz 0:a2a71c38065e 377 axis.map = register_data[0];
joelvonrotz 0:a2a71c38065e 378 axis.sign = register_data[1];
joelvonrotz 0:a2a71c38065e 379 }
joelvonrotz 0:a2a71c38065e 380
joelvonrotz 0:a2a71c38065e 381 void BNO055::assignAxis(bno055_axis_t x_axis, bno055_axis_t y_axis, bno055_axis_t z_axis)
joelvonrotz 0:a2a71c38065e 382 {
joelvonrotz 0:a2a71c38065e 383 //check if multiple axis have the same remap-axis. If true, then this part can be skipped, as
joelvonrotz 0:a2a71c38065e 384 //it's useless (the chip reverts to the previous state) by the datasheet and doing it now saves some time
joelvonrotz 0:a2a71c38065e 385 if((x_axis == y_axis) || (x_axis == z_axis) || (z_axis == y_axis))
joelvonrotz 0:a2a71c38065e 386 {
joelvonrotz 0:a2a71c38065e 387 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 388 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis \tmultiple Axis");
joelvonrotz 0:a2a71c38065e 389 #endif
joelvonrotz 0:a2a71c38065e 390 }
joelvonrotz 0:a2a71c38065e 391 else
joelvonrotz 0:a2a71c38065e 392 {
joelvonrotz 0:a2a71c38065e 393
joelvonrotz 0:a2a71c38065e 394 axis.map = x_axis + (y_axis << 2) + (z_axis << 4);
joelvonrotz 0:a2a71c38065e 395 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 396 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis new \t%3i",axis.map);
joelvonrotz 0:a2a71c38065e 397 #endif
joelvonrotz 0:a2a71c38065e 398 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS);
joelvonrotz 0:a2a71c38065e 399 }
joelvonrotz 0:a2a71c38065e 400 }
joelvonrotz 0:a2a71c38065e 401
joelvonrotz 0:a2a71c38065e 402 void BNO055::setAxisSign(bno055_axis_sign_t x_sign, bno055_axis_sign_t y_sign, bno055_axis_sign_t z_sign)
joelvonrotz 0:a2a71c38065e 403 {
joelvonrotz 0:a2a71c38065e 404 axis.sign = x_sign + (y_sign << 1) + (z_sign << 2);
joelvonrotz 0:a2a71c38065e 405 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 406 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tAxis Sign new \t%3i",axis.sign);
joelvonrotz 0:a2a71c38065e 407 #endif
joelvonrotz 0:a2a71c38065e 408 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS);
joelvonrotz 0:a2a71c38065e 409 }
joelvonrotz 0:a2a71c38065e 410
joelvonrotz 0:a2a71c38065e 411 void BNO055::getCalibrationStatus(void)
joelvonrotz 0:a2a71c38065e 412 {
joelvonrotz 0:a2a71c38065e 413 calibration.status = m_i2c_master.i2c_read(m_bno055_address, CALIB_STAT);
joelvonrotz 0:a2a71c38065e 414 calibration.system = (calibration.status >> SHIFT_CALIB_SYSTEM) & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 415 calibration.gyro = (calibration.status >> SHIFT_CALIB_GYRO) & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 416 calibration.acceleration = (calibration.status >> SHIFT_CALIB_ACCEL) & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 417 calibration.magneto = calibration.status & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 418
joelvonrotz 0:a2a71c38065e 419 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 420 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[S] %1i [G] %1i [A] %1i [M] %1i\t[REG]%3X",calibration.system
joelvonrotz 0:a2a71c38065e 421 ,calibration.gyro
joelvonrotz 0:a2a71c38065e 422 ,calibration.acceleration
joelvonrotz 0:a2a71c38065e 423 ,calibration.magneto
joelvonrotz 0:a2a71c38065e 424 ,calibration.status
joelvonrotz 0:a2a71c38065e 425 );
joelvonrotz 0:a2a71c38065e 426 #endif
joelvonrotz 0:a2a71c38065e 427 }
joelvonrotz 0:a2a71c38065e 428
joelvonrotz 0:a2a71c38065e 429 void BNO055::getInterruptFlag(void)
joelvonrotz 0:a2a71c38065e 430 {
joelvonrotz 0:a2a71c38065e 431 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 432 interrupt.status = m_i2c_master.i2c_read(m_bno055_address, INT_STATUS);
joelvonrotz 0:a2a71c38065e 433
joelvonrotz 0:a2a71c38065e 434 interrupt.gyroscope.any_motion = ((interrupt.status & MASK_GYRO_ANY_MOTION ) == GYRO_ANY_MOTION );
joelvonrotz 0:a2a71c38065e 435 interrupt.gyroscope.high_rate = ((interrupt.status & MASK_GYRO_HIGH_RATE ) == GYRO_HIGH_RATE );
joelvonrotz 0:a2a71c38065e 436 interrupt.acceleration.high_g = ((interrupt.status & MASK_ACCEL_HIGH_G ) == ACCEL_HIGH_G );
joelvonrotz 0:a2a71c38065e 437 interrupt.acceleration.any_motion = ((interrupt.status & MASK_ACCEL_ANY_MOTION) == ACCEL_ANY_MOTION);
joelvonrotz 0:a2a71c38065e 438 interrupt.acceleration.no_motion = ((interrupt.status & MASK_ACCEL_NO_MOTION ) == ACCEL_NO_MOTION );
joelvonrotz 0:a2a71c38065e 439
joelvonrotz 0:a2a71c38065e 440 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 441 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",
joelvonrotz 0:a2a71c38065e 442 interrupt.gyroscope.any_motion,
joelvonrotz 0:a2a71c38065e 443 interrupt.gyroscope.high_rate,
joelvonrotz 0:a2a71c38065e 444 interrupt.acceleration.high_g,
joelvonrotz 0:a2a71c38065e 445 interrupt.acceleration.any_motion,
joelvonrotz 0:a2a71c38065e 446 interrupt.acceleration.no_motion,
joelvonrotz 0:a2a71c38065e 447 interrupt.status
joelvonrotz 0:a2a71c38065e 448 );
joelvonrotz 0:a2a71c38065e 449 #endif
joelvonrotz 0:a2a71c38065e 450
joelvonrotz 0:a2a71c38065e 451 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 452 }
joelvonrotz 0:a2a71c38065e 453
joelvonrotz 0:a2a71c38065e 454 uint8_t BNO055::getEnabledInterrupts(void)
joelvonrotz 0:a2a71c38065e 455 {
joelvonrotz 0:a2a71c38065e 456 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 457 uint8_t data_interrupt_enable = m_i2c_master.i2c_read(m_bno055_address, INT_EN);
joelvonrotz 0:a2a71c38065e 458 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 459
joelvonrotz 0:a2a71c38065e 460 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 461 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Enabled Interrupt\t0x%02X",data_interrupt_enable);
joelvonrotz 0:a2a71c38065e 462 #endif
joelvonrotz 0:a2a71c38065e 463
joelvonrotz 0:a2a71c38065e 464 return data_interrupt_enable;
joelvonrotz 0:a2a71c38065e 465 }
joelvonrotz 0:a2a71c38065e 466
joelvonrotz 0:a2a71c38065e 467 void BNO055::setEnableInterrupts(bno055_enable_t accel_no_motion,
joelvonrotz 0:a2a71c38065e 468 bno055_enable_t accel_any_motion,
joelvonrotz 0:a2a71c38065e 469 bno055_enable_t accel_high_g,
joelvonrotz 0:a2a71c38065e 470 bno055_enable_t gyro_high_rate,
joelvonrotz 0:a2a71c38065e 471 bno055_enable_t gyro_any_motion
joelvonrotz 0:a2a71c38065e 472 )
joelvonrotz 0:a2a71c38065e 473 {
joelvonrotz 0:a2a71c38065e 474 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 475 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);
joelvonrotz 0:a2a71c38065e 476 m_i2c_master.i2c_write(m_bno055_address, INT_EN, data_interrupt_enable);
joelvonrotz 0:a2a71c38065e 477
joelvonrotz 0:a2a71c38065e 478 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 479 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Enabled Interrupt\t0x%02X",data_interrupt_enable);
joelvonrotz 0:a2a71c38065e 480 #endif
joelvonrotz 0:a2a71c38065e 481
joelvonrotz 0:a2a71c38065e 482 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 483 }
joelvonrotz 0:a2a71c38065e 484
joelvonrotz 0:a2a71c38065e 485
joelvonrotz 0:a2a71c38065e 486
joelvonrotz 0:a2a71c38065e 487 void BNO055::setInterruptMask(bno055_enable_t accel_no_motion,
joelvonrotz 0:a2a71c38065e 488 bno055_enable_t accel_any_motion,
joelvonrotz 0:a2a71c38065e 489 bno055_enable_t accel_high_g,
joelvonrotz 0:a2a71c38065e 490 bno055_enable_t gyro_high_rate,
joelvonrotz 0:a2a71c38065e 491 bno055_enable_t gyro_any_motion
joelvonrotz 0:a2a71c38065e 492 )
joelvonrotz 0:a2a71c38065e 493 {
joelvonrotz 0:a2a71c38065e 494 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 495 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);
joelvonrotz 0:a2a71c38065e 496 m_i2c_master.i2c_write(m_bno055_address, INT_MSK, data_interrupt_mask);
joelvonrotz 0:a2a71c38065e 497
joelvonrotz 0:a2a71c38065e 498 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 499 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Interrupt Mask\t0x%02X",data_interrupt_mask);
joelvonrotz 0:a2a71c38065e 500 #endif
joelvonrotz 0:a2a71c38065e 501
joelvonrotz 0:a2a71c38065e 502 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 503 }
joelvonrotz 0:a2a71c38065e 504
joelvonrotz 0:a2a71c38065e 505 uint8_t BNO055::getInterruptMask(void)
joelvonrotz 0:a2a71c38065e 506 {
joelvonrotz 0:a2a71c38065e 507 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 508 uint8_t data_interrupt_mask = m_i2c_master.i2c_read(m_bno055_address, INT_MSK);
joelvonrotz 0:a2a71c38065e 509
joelvonrotz 0:a2a71c38065e 510 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 511 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Interrupt Mask\t0x%02X",data_interrupt_mask);
joelvonrotz 0:a2a71c38065e 512 #endif
joelvonrotz 0:a2a71c38065e 513
joelvonrotz 0:a2a71c38065e 514 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 515
joelvonrotz 0:a2a71c38065e 516 return data_interrupt_mask;
joelvonrotz 0:a2a71c38065e 517 }
joelvonrotz 0:a2a71c38065e 518
joelvonrotz 0:a2a71c38065e 519 void BNO055::configAccelerationInterrupt(bno055_config_int_axis_t high_axis,
joelvonrotz 0:a2a71c38065e 520 bno055_config_int_axis_t am_nm_axis,
joelvonrotz 0:a2a71c38065e 521 bno055_any_motion_sample_t am_dur
joelvonrotz 0:a2a71c38065e 522 )
joelvonrotz 0:a2a71c38065e 523 {
joelvonrotz 0:a2a71c38065e 524 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 525 uint8_t data = am_dur + (am_nm_axis << SHIFT_AM_NM_AXIS) + (high_axis << SHIFT_HIGH_AXIS);
joelvonrotz 0:a2a71c38065e 526 m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data);
joelvonrotz 0:a2a71c38065e 527
joelvonrotz 0:a2a71c38065e 528 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 529 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);
joelvonrotz 0:a2a71c38065e 530 #endif
joelvonrotz 0:a2a71c38065e 531
joelvonrotz 0:a2a71c38065e 532 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 533 }
joelvonrotz 0:a2a71c38065e 534
joelvonrotz 0:a2a71c38065e 535 void BNO055::configGyroscopeInterrupt(bno055_config_int_axis_t hr_axis,
joelvonrotz 0:a2a71c38065e 536 bno055_config_int_axis_t am_axis,
joelvonrotz 0:a2a71c38065e 537 bool high_rate_unfilt,
joelvonrotz 0:a2a71c38065e 538 bool any_motion_unfilt
joelvonrotz 0:a2a71c38065e 539 )
joelvonrotz 0:a2a71c38065e 540 {
joelvonrotz 0:a2a71c38065e 541 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 542 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);
joelvonrotz 0:a2a71c38065e 543 m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data);
joelvonrotz 0:a2a71c38065e 544
joelvonrotz 0:a2a71c38065e 545 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 546 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);
joelvonrotz 0:a2a71c38065e 547 #endif
joelvonrotz 0:a2a71c38065e 548
joelvonrotz 0:a2a71c38065e 549 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 550 }
joelvonrotz 0:a2a71c38065e 551
joelvonrotz 0:a2a71c38065e 552 //ACCELERATION
joelvonrotz 0:a2a71c38065e 553
joelvonrotz 0:a2a71c38065e 554 void BNO055::getAcceleration(void)
joelvonrotz 0:a2a71c38065e 555 {
joelvonrotz 0:a2a71c38065e 556 if(format.acceleration == ACCELERATION)
joelvonrotz 0:a2a71c38065e 557 {
joelvonrotz 0:a2a71c38065e 558 get(ACC_DATA_VECTOR, accel, ACCELERATION_FORMAT, false);
joelvonrotz 0:a2a71c38065e 559 }
joelvonrotz 0:a2a71c38065e 560 else if(format.acceleration == MILLI_G_FORCE)
joelvonrotz 0:a2a71c38065e 561 {
joelvonrotz 0:a2a71c38065e 562 get(ACC_DATA_VECTOR, accel, MILLI_G_FORCE_FORMAT, false);
joelvonrotz 0:a2a71c38065e 563 }
joelvonrotz 0:a2a71c38065e 564
joelvonrotz 0:a2a71c38065e 565 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 566 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);
joelvonrotz 0:a2a71c38065e 567 #endif
joelvonrotz 0:a2a71c38065e 568 }
joelvonrotz 0:a2a71c38065e 569
joelvonrotz 0:a2a71c38065e 570 //MAGNETOMETER
joelvonrotz 0:a2a71c38065e 571
joelvonrotz 0:a2a71c38065e 572 void BNO055::getMagnetometer(void)
joelvonrotz 0:a2a71c38065e 573 {
joelvonrotz 0:a2a71c38065e 574 get(MAG_DATA_VECTOR, magneto, MICRO_TESLA_FORMAT, false);
joelvonrotz 0:a2a71c38065e 575
joelvonrotz 0:a2a71c38065e 576 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 577 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);
joelvonrotz 0:a2a71c38065e 578 #endif
joelvonrotz 0:a2a71c38065e 579 }
joelvonrotz 0:a2a71c38065e 580
joelvonrotz 0:a2a71c38065e 581 //GYROSCOPE
joelvonrotz 0:a2a71c38065e 582
joelvonrotz 0:a2a71c38065e 583 void BNO055::getGyroscope(void)
joelvonrotz 0:a2a71c38065e 584 {
joelvonrotz 0:a2a71c38065e 585 if(format.gyroscope == DEGREE_PER_SEC)
joelvonrotz 0:a2a71c38065e 586 {
joelvonrotz 0:a2a71c38065e 587 get(GYR_DATA_VECTOR, gyro, GYRO_DEGREE_PER_SEC_FORMAT, false);
joelvonrotz 0:a2a71c38065e 588 }
joelvonrotz 0:a2a71c38065e 589 else if(format.gyroscope == RADIAN_PER_SEC)
joelvonrotz 0:a2a71c38065e 590 {
joelvonrotz 0:a2a71c38065e 591 get(GYR_DATA_VECTOR, gyro, GYRO_RADIAN_PER_SEC_FORMAT, false);
joelvonrotz 0:a2a71c38065e 592 }
joelvonrotz 0:a2a71c38065e 593 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 594 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);
joelvonrotz 0:a2a71c38065e 595 #endif
joelvonrotz 0:a2a71c38065e 596 }
joelvonrotz 0:a2a71c38065e 597
joelvonrotz 0:a2a71c38065e 598 //EULER-DEGREES
joelvonrotz 0:a2a71c38065e 599
joelvonrotz 0:a2a71c38065e 600 void BNO055::getEulerDegrees(void)
joelvonrotz 0:a2a71c38065e 601 {
joelvonrotz 0:a2a71c38065e 602 if(format.euler == DEGREES)
joelvonrotz 0:a2a71c38065e 603 {
joelvonrotz 0:a2a71c38065e 604 get(EUL_DATA_VECTOR, euler, EULER_DEGREES_FORMAT, true);
joelvonrotz 0:a2a71c38065e 605 }
joelvonrotz 0:a2a71c38065e 606 else if(format.euler == RADIANS)
joelvonrotz 0:a2a71c38065e 607 {
joelvonrotz 0:a2a71c38065e 608 get(EUL_DATA_VECTOR, euler, EULER_RADIANS_FORMAT, true);
joelvonrotz 0:a2a71c38065e 609 }
joelvonrotz 0:a2a71c38065e 610 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 611 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);
joelvonrotz 0:a2a71c38065e 612 #endif
joelvonrotz 0:a2a71c38065e 613 }
joelvonrotz 0:a2a71c38065e 614
joelvonrotz 0:a2a71c38065e 615 //QUATERNION
joelvonrotz 0:a2a71c38065e 616
joelvonrotz 0:a2a71c38065e 617 void BNO055::getQuaternion(void)
joelvonrotz 0:a2a71c38065e 618 {
joelvonrotz 0:a2a71c38065e 619 get(QUA_DATA_VECTOR, quaternion, QUATERNION_FORMAT);
joelvonrotz 0:a2a71c38065e 620
joelvonrotz 0:a2a71c38065e 621 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 622 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);
joelvonrotz 0:a2a71c38065e 623 #endif
joelvonrotz 0:a2a71c38065e 624 }
joelvonrotz 0:a2a71c38065e 625
joelvonrotz 0:a2a71c38065e 626 //LINEAR ACCELERATION
joelvonrotz 0:a2a71c38065e 627
joelvonrotz 0:a2a71c38065e 628 void BNO055::getLinearAcceleration(void)
joelvonrotz 0:a2a71c38065e 629 {
joelvonrotz 0:a2a71c38065e 630 if(format.acceleration == ACCELERATION)
joelvonrotz 0:a2a71c38065e 631 {
joelvonrotz 0:a2a71c38065e 632 get(LIA_DATA_VECTOR, linear_accel, ACCELERATION_FORMAT, false);
joelvonrotz 0:a2a71c38065e 633 }
joelvonrotz 0:a2a71c38065e 634 else if(format.acceleration == MILLI_G_FORCE)
joelvonrotz 0:a2a71c38065e 635 {
joelvonrotz 0:a2a71c38065e 636 get(LIA_DATA_VECTOR, linear_accel, MILLI_G_FORCE_FORMAT, false);
joelvonrotz 0:a2a71c38065e 637 }
joelvonrotz 0:a2a71c38065e 638 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 639 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);
joelvonrotz 0:a2a71c38065e 640 #endif
joelvonrotz 0:a2a71c38065e 641 }
joelvonrotz 0:a2a71c38065e 642
joelvonrotz 0:a2a71c38065e 643 //GRAVITY VECTOR
joelvonrotz 0:a2a71c38065e 644
joelvonrotz 0:a2a71c38065e 645 void BNO055::getGravityVector(void)
joelvonrotz 0:a2a71c38065e 646 {
joelvonrotz 0:a2a71c38065e 647 if(format.acceleration == ACCELERATION)
joelvonrotz 0:a2a71c38065e 648 {
joelvonrotz 0:a2a71c38065e 649 get(ACC_DATA_VECTOR, gravity_vector, ACCELERATION_FORMAT, false);
joelvonrotz 0:a2a71c38065e 650 }
joelvonrotz 0:a2a71c38065e 651 else if(format.acceleration == MILLI_G_FORCE)
joelvonrotz 0:a2a71c38065e 652 {
joelvonrotz 0:a2a71c38065e 653 get(ACC_DATA_VECTOR, gravity_vector, MILLI_G_FORCE_FORMAT, false);
joelvonrotz 0:a2a71c38065e 654 }
joelvonrotz 0:a2a71c38065e 655 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 656 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);
joelvonrotz 0:a2a71c38065e 657 #endif
joelvonrotz 0:a2a71c38065e 658 }
joelvonrotz 0:a2a71c38065e 659
joelvonrotz 0:a2a71c38065e 660 //TEMPERATURE
joelvonrotz 0:a2a71c38065e 661
joelvonrotz 0:a2a71c38065e 662 void BNO055::getTemperature(void)
joelvonrotz 0:a2a71c38065e 663 {
joelvonrotz 0:a2a71c38065e 664 m_i2c_master.i2c_readSeries(m_bno055_address, TEMP, register_data, LENGTH_1_BYTES);
joelvonrotz 0:a2a71c38065e 665 if(format.temperature == CELSIUS)
joelvonrotz 0:a2a71c38065e 666 {
joelvonrotz 0:a2a71c38065e 667 temperature = static_cast<float>(register_data[0]) * TEMPERATURE_CELSIUS_FORMAT;
joelvonrotz 0:a2a71c38065e 668 }
joelvonrotz 0:a2a71c38065e 669 if(format.temperature == FAHRENHEIT)
joelvonrotz 0:a2a71c38065e 670 {
joelvonrotz 0:a2a71c38065e 671 temperature = static_cast<float>(register_data[0]) * TEMPERATURE_FAHRENHEIT_FORMAT;
joelvonrotz 0:a2a71c38065e 672 }
joelvonrotz 0:a2a71c38065e 673 }
joelvonrotz 0:a2a71c38065e 674
joelvonrotz 0:a2a71c38065e 675 void BNO055::get(bno055_reg_t address, bno055_data_s& data, const float format, bool is_euler)
joelvonrotz 0:a2a71c38065e 676 {
joelvonrotz 0:a2a71c38065e 677 m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_6_BYTES);
joelvonrotz 0:a2a71c38065e 678
joelvonrotz 0:a2a71c38065e 679 for(arrayIndex = 0 ; arrayIndex < (LENGTH_6_BYTES/2) ; arrayIndex++)
joelvonrotz 0:a2a71c38065e 680 {
joelvonrotz 0:a2a71c38065e 681 sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE);
joelvonrotz 0:a2a71c38065e 682 }
joelvonrotz 0:a2a71c38065e 683
joelvonrotz 0:a2a71c38065e 684 if(is_euler)
joelvonrotz 0:a2a71c38065e 685 {
joelvonrotz 0:a2a71c38065e 686 data.z = format * static_cast<float>(sensor_data[0]);
joelvonrotz 0:a2a71c38065e 687 data.y = format * static_cast<float>(sensor_data[1]);
joelvonrotz 0:a2a71c38065e 688 data.x = format * static_cast<float>(sensor_data[2]);
joelvonrotz 0:a2a71c38065e 689 }
joelvonrotz 0:a2a71c38065e 690 else
joelvonrotz 0:a2a71c38065e 691 {
joelvonrotz 0:a2a71c38065e 692 data.x = format * static_cast<float>(sensor_data[0]);
joelvonrotz 0:a2a71c38065e 693 data.y = format * static_cast<float>(sensor_data[1]);
joelvonrotz 0:a2a71c38065e 694 data.z = format * static_cast<float>(sensor_data[2]);
joelvonrotz 0:a2a71c38065e 695 }
joelvonrotz 0:a2a71c38065e 696 }
joelvonrotz 0:a2a71c38065e 697
joelvonrotz 0:a2a71c38065e 698 void BNO055::get(bno055_reg_t address, bno055_quaternion_s& data, const float format)
joelvonrotz 0:a2a71c38065e 699 {
joelvonrotz 0:a2a71c38065e 700 m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_8_BYTES);
joelvonrotz 0:a2a71c38065e 701
joelvonrotz 0:a2a71c38065e 702 for(arrayIndex = 0 ; arrayIndex < (LENGTH_8_BYTES/2) ; arrayIndex++)
joelvonrotz 0:a2a71c38065e 703 {
joelvonrotz 0:a2a71c38065e 704 sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE);
joelvonrotz 0:a2a71c38065e 705 }
joelvonrotz 0:a2a71c38065e 706
joelvonrotz 0:a2a71c38065e 707 data.w = format * static_cast<float>(sensor_data[0]);
joelvonrotz 0:a2a71c38065e 708 data.x = format * static_cast<float>(sensor_data[1]);
joelvonrotz 0:a2a71c38065e 709 data.y = format * static_cast<float>(sensor_data[2]);
joelvonrotz 0:a2a71c38065e 710 data.z = format * static_cast<float>(sensor_data[3]);
joelvonrotz 0:a2a71c38065e 711 }
joelvonrotz 0:a2a71c38065e 712