Joel von Rotz / BNO055

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

Requires the I2C-Master library - Link

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

Committer:
joelvonrotz
Date:
Thu Jul 11 11:05:39 2019 +0000
Revision:
3:ffc741483987
Parent:
2:d7baee646b4e
Child:
6:3dcf44b8289a
added table

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 3:ffc741483987 103 * <caption id="multi_row">Complex table</caption>
joelvonrotz 3:ffc741483987 104 * <tr><th>Type <th>Formats <th>Code
joelvonrotz 3:ffc741483987 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 0:a2a71c38065e 140 void BNO055::getUnitFormat(void)
joelvonrotz 0:a2a71c38065e 141 {
joelvonrotz 0:a2a71c38065e 142
joelvonrotz 0:a2a71c38065e 143 }
joelvonrotz 0:a2a71c38065e 144
joelvonrotz 0:a2a71c38065e 145 void BNO055::getIDs(void)
joelvonrotz 0:a2a71c38065e 146 {
joelvonrotz 0:a2a71c38065e 147 uint8_t dataArray[7];
joelvonrotz 0:a2a71c38065e 148 m_i2c_master.i2c_readSeries(m_bno055_address, CHIP_ID, dataArray, 7);
joelvonrotz 0:a2a71c38065e 149 id.chip = dataArray[0];
joelvonrotz 0:a2a71c38065e 150 id.accel = dataArray[1];
joelvonrotz 0:a2a71c38065e 151 id.magneto = dataArray[2];
joelvonrotz 0:a2a71c38065e 152 id.gyro = dataArray[3];
joelvonrotz 0:a2a71c38065e 153 id.sw_rev = dataArray[4] + (static_cast<uint16_t>(dataArray[5]) << 8);
joelvonrotz 0:a2a71c38065e 154 id.bl_rev = dataArray[6];
joelvonrotz 0:a2a71c38065e 155
joelvonrotz 0:a2a71c38065e 156 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 157 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tIDs---------------------");
joelvonrotz 0:a2a71c38065e 158 m_DEBUG_SERIAL.printf("\r\n\tChip\t\t 0x%02X",id.chip);
joelvonrotz 0:a2a71c38065e 159 m_DEBUG_SERIAL.printf("\r\n\tAcceleraiton\t 0x%02X",id.accel);
joelvonrotz 0:a2a71c38065e 160 m_DEBUG_SERIAL.printf("\r\n\tMagnetometer\t 0x%02X",id.magneto);
joelvonrotz 0:a2a71c38065e 161 m_DEBUG_SERIAL.printf("\r\n\tGyroscope\t 0x%02X",id.gyro);
joelvonrotz 0:a2a71c38065e 162 m_DEBUG_SERIAL.printf("\r\n\tSofware Rev.\t 0x%04X",id.sw_rev);
joelvonrotz 0:a2a71c38065e 163 m_DEBUG_SERIAL.printf("\r\n\tBootloader\t 0x%02X",id.bl_rev);
joelvonrotz 0:a2a71c38065e 164 #endif
joelvonrotz 0:a2a71c38065e 165 }
joelvonrotz 0:a2a71c38065e 166
joelvonrotz 0:a2a71c38065e 167 void BNO055::setPowerMode(bno055_powermode_t new_power_mode)
joelvonrotz 0:a2a71c38065e 168 {
joelvonrotz 0:a2a71c38065e 169 if(new_power_mode != mode.power)
joelvonrotz 0:a2a71c38065e 170 {
joelvonrotz 0:a2a71c38065e 171 do
joelvonrotz 0:a2a71c38065e 172 {
joelvonrotz 0:a2a71c38065e 173 m_i2c_master.i2c_writeBits(m_bno055_address, PWR_MODE, new_power_mode, MASK_POWER_MODE);
joelvonrotz 0:a2a71c38065e 174 wait_ms(50.0);
joelvonrotz 0:a2a71c38065e 175 }
joelvonrotz 0:a2a71c38065e 176 while((m_i2c_master.i2c_read(m_bno055_address, PWR_MODE) & MASK_POWER_MODE) != new_power_mode);
joelvonrotz 0:a2a71c38065e 177 //and the old operation mode should only be updated, when there's a new mode.
joelvonrotz 0:a2a71c38065e 178 mode.power = new_power_mode;
joelvonrotz 0:a2a71c38065e 179
joelvonrotz 0:a2a71c38065e 180 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 181 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode new\t %3i", new_power_mode);
joelvonrotz 0:a2a71c38065e 182 #endif
joelvonrotz 0:a2a71c38065e 183 }
joelvonrotz 0:a2a71c38065e 184 else
joelvonrotz 0:a2a71c38065e 185 {
joelvonrotz 0:a2a71c38065e 186 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 187 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode\t no change");
joelvonrotz 0:a2a71c38065e 188 #endif
joelvonrotz 0:a2a71c38065e 189 }
joelvonrotz 0:a2a71c38065e 190 }
joelvonrotz 0:a2a71c38065e 191
joelvonrotz 0:a2a71c38065e 192 uint8_t BNO055::getPowerMode(void)
joelvonrotz 0:a2a71c38065e 193 {
joelvonrotz 0:a2a71c38065e 194 mode.power = m_i2c_master.i2c_read(m_bno055_address, PWR_MODE);
joelvonrotz 0:a2a71c38065e 195 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 196 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpwrMode current\t %3i",mode.power);
joelvonrotz 0:a2a71c38065e 197 #endif
joelvonrotz 0:a2a71c38065e 198 return mode.power;
joelvonrotz 0:a2a71c38065e 199 }
joelvonrotz 0:a2a71c38065e 200
joelvonrotz 0:a2a71c38065e 201 void BNO055::setOperationMode(bno055_opr_mode_t new_opr_mode)
joelvonrotz 0:a2a71c38065e 202 {
joelvonrotz 0:a2a71c38065e 203 //trying to write a mode, that is already being used, doesn't make any sense and can be ignored.
joelvonrotz 0:a2a71c38065e 204 if(new_opr_mode != mode.operation)
joelvonrotz 0:a2a71c38065e 205 {
joelvonrotz 0:a2a71c38065e 206 do
joelvonrotz 0:a2a71c38065e 207 {
joelvonrotz 0:a2a71c38065e 208 m_i2c_master.i2c_writeBits(m_bno055_address, OPR_MODE, new_opr_mode, MASK_OPERAITON_MODE);
joelvonrotz 0:a2a71c38065e 209 wait_ms(50.0);
joelvonrotz 0:a2a71c38065e 210 }
joelvonrotz 0:a2a71c38065e 211 while((m_i2c_master.i2c_read(m_bno055_address, OPR_MODE) & MASK_OPERAITON_MODE) != new_opr_mode);
joelvonrotz 0:a2a71c38065e 212 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 213 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode new\t %3i", new_opr_mode);
joelvonrotz 0:a2a71c38065e 214 #endif
joelvonrotz 0:a2a71c38065e 215
joelvonrotz 0:a2a71c38065e 216 //and the old operation mode should only be updated, when there's a new mode.
joelvonrotz 0:a2a71c38065e 217 mode.operation = new_opr_mode;
joelvonrotz 0:a2a71c38065e 218 }
joelvonrotz 0:a2a71c38065e 219 else
joelvonrotz 0:a2a71c38065e 220 {
joelvonrotz 0:a2a71c38065e 221 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 222 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode\t\t no change");
joelvonrotz 0:a2a71c38065e 223 #endif
joelvonrotz 0:a2a71c38065e 224 }
joelvonrotz 0:a2a71c38065e 225 }
joelvonrotz 0:a2a71c38065e 226
joelvonrotz 0:a2a71c38065e 227 uint8_t BNO055::getOperationMode(void)
joelvonrotz 0:a2a71c38065e 228 {
joelvonrotz 0:a2a71c38065e 229 mode.operation = m_i2c_master.i2c_read(m_bno055_address, OPR_MODE);
joelvonrotz 0:a2a71c38065e 230 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 231 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode current\t %3i", mode.operation);
joelvonrotz 0:a2a71c38065e 232 #endif
joelvonrotz 0:a2a71c38065e 233 return mode.operation;
joelvonrotz 0:a2a71c38065e 234 }
joelvonrotz 0:a2a71c38065e 235
joelvonrotz 0:a2a71c38065e 236 void BNO055::setPage(bno055_page_t new_page)
joelvonrotz 0:a2a71c38065e 237 {
joelvonrotz 0:a2a71c38065e 238 page = new_page;
joelvonrotz 0:a2a71c38065e 239
joelvonrotz 0:a2a71c38065e 240 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 241 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage set\t %3i", page);
joelvonrotz 0:a2a71c38065e 242 #endif
joelvonrotz 0:a2a71c38065e 243
joelvonrotz 0:a2a71c38065e 244 m_i2c_master.i2c_write(m_bno055_address, PAGE_ID, page);
joelvonrotz 0:a2a71c38065e 245 }
joelvonrotz 0:a2a71c38065e 246
joelvonrotz 0:a2a71c38065e 247 uint8_t BNO055::getPage(void)
joelvonrotz 0:a2a71c38065e 248 {
joelvonrotz 0:a2a71c38065e 249 page = m_i2c_master.i2c_read(m_bno055_address, PAGE_ID);
joelvonrotz 0:a2a71c38065e 250
joelvonrotz 0:a2a71c38065e 251 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 252 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage current\t %3i",page);
joelvonrotz 0:a2a71c38065e 253 #endif
joelvonrotz 0:a2a71c38065e 254
joelvonrotz 0:a2a71c38065e 255 return page;
joelvonrotz 0:a2a71c38065e 256 }
joelvonrotz 0:a2a71c38065e 257
joelvonrotz 0:a2a71c38065e 258 uint8_t BNO055::getSystemStatus(void)
joelvonrotz 0:a2a71c38065e 259 {
joelvonrotz 0:a2a71c38065e 260 system.status = m_i2c_master.i2c_read(m_bno055_address, SYS_STATUS);
joelvonrotz 0:a2a71c38065e 261 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 262 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysStatus\t %3i",system.status);
joelvonrotz 0:a2a71c38065e 263 #endif
joelvonrotz 0:a2a71c38065e 264 return system.status;
joelvonrotz 0:a2a71c38065e 265 }
joelvonrotz 0:a2a71c38065e 266
joelvonrotz 0:a2a71c38065e 267 uint8_t BNO055::getSystemError(void)
joelvonrotz 0:a2a71c38065e 268 {
joelvonrotz 0:a2a71c38065e 269 system.error = m_i2c_master.i2c_read(m_bno055_address, SYS_ERR);
joelvonrotz 0:a2a71c38065e 270 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 271 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysError\t %3i",system.error);
joelvonrotz 0:a2a71c38065e 272 #endif
joelvonrotz 0:a2a71c38065e 273 return system.error;
joelvonrotz 0:a2a71c38065e 274 }
joelvonrotz 0:a2a71c38065e 275
joelvonrotz 0:a2a71c38065e 276 void BNO055::useExternalOscillator(bool enabled)
joelvonrotz 0:a2a71c38065e 277 {
joelvonrotz 0:a2a71c38065e 278 char data = static_cast<char>(enabled) << 7;
joelvonrotz 0:a2a71c38065e 279 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 280 if(enabled)
joelvonrotz 0:a2a71c38065e 281 {
joelvonrotz 0:a2a71c38065e 282 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source\t active");
joelvonrotz 0:a2a71c38065e 283 }
joelvonrotz 0:a2a71c38065e 284 else
joelvonrotz 0:a2a71c38065e 285 {
joelvonrotz 0:a2a71c38065e 286 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source \tinactive");
joelvonrotz 0:a2a71c38065e 287 }
joelvonrotz 0:a2a71c38065e 288 #endif
joelvonrotz 0:a2a71c38065e 289 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, data, MASK_EXT_CLOCK);
joelvonrotz 0:a2a71c38065e 290 }
joelvonrotz 0:a2a71c38065e 291
joelvonrotz 0:a2a71c38065e 292 void BNO055::resetSW(void)
joelvonrotz 0:a2a71c38065e 293 {
joelvonrotz 0:a2a71c38065e 294 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 295 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSoftware Reset");
joelvonrotz 0:a2a71c38065e 296 #endif
joelvonrotz 0:a2a71c38065e 297
joelvonrotz 0:a2a71c38065e 298 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x20, MASK_SYSTEM_RESET);
joelvonrotz 0:a2a71c38065e 299
joelvonrotz 0:a2a71c38065e 300 //datasheet (p.13 of revision 14) indicate a Power-On-Reset-time of 650ms
joelvonrotz 0:a2a71c38065e 301 //from Reset to Config Mode. By constantly reading the chip id, it can be
joelvonrotz 0:a2a71c38065e 302 //determined, when the chip is fully functional again.
joelvonrotz 0:a2a71c38065e 303 while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID)
joelvonrotz 0:a2a71c38065e 304 {
joelvonrotz 0:a2a71c38065e 305 wait_ms(10.0);
joelvonrotz 0:a2a71c38065e 306 }
joelvonrotz 0:a2a71c38065e 307
joelvonrotz 0:a2a71c38065e 308 m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x00, MASK_SYSTEM_RESET);
joelvonrotz 0:a2a71c38065e 309 }
joelvonrotz 0:a2a71c38065e 310
joelvonrotz 0:a2a71c38065e 311 void BNO055::resetHW(void)
joelvonrotz 0:a2a71c38065e 312 {
joelvonrotz 0:a2a71c38065e 313 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 314 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tHardware Reset");
joelvonrotz 0:a2a71c38065e 315 #endif
joelvonrotz 0:a2a71c38065e 316 m_ResetPin = 0;
joelvonrotz 0:a2a71c38065e 317 wait_ms(10.0);
joelvonrotz 0:a2a71c38065e 318 m_ResetPin = 1;
joelvonrotz 0:a2a71c38065e 319 while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID)
joelvonrotz 0:a2a71c38065e 320 {
joelvonrotz 0:a2a71c38065e 321 wait_ms(10.0);
joelvonrotz 0:a2a71c38065e 322 }
joelvonrotz 0:a2a71c38065e 323
joelvonrotz 0:a2a71c38065e 324 }
joelvonrotz 0:a2a71c38065e 325
joelvonrotz 0:a2a71c38065e 326 void BNO055::setOrientation(bno055_remap_options_t orientation_placement)
joelvonrotz 0:a2a71c38065e 327 {
joelvonrotz 0:a2a71c38065e 328 axis.map = (orientation_placement >> SHIFT_1BYTE) & 0xFF;
joelvonrotz 0:a2a71c38065e 329 axis.sign = (orientation_placement & 0xFF);
joelvonrotz 0:a2a71c38065e 330
joelvonrotz 0:a2a71c38065e 331 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 332 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap New\t 0x%02X", axis.map);
joelvonrotz 0:a2a71c38065e 333 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign New\t 0x%02X", axis.sign);
joelvonrotz 0:a2a71c38065e 334 #endif
joelvonrotz 0:a2a71c38065e 335
joelvonrotz 0:a2a71c38065e 336 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS);
joelvonrotz 0:a2a71c38065e 337 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS);
joelvonrotz 0:a2a71c38065e 338 }
joelvonrotz 0:a2a71c38065e 339
joelvonrotz 0:a2a71c38065e 340 void BNO055::getOrientation(void)
joelvonrotz 0:a2a71c38065e 341 {
joelvonrotz 0:a2a71c38065e 342 m_i2c_master.i2c_readSeries(m_bno055_address, AXIS_MAP_CONFIG,register_data,LENGTH_2_BYTES);
joelvonrotz 0:a2a71c38065e 343
joelvonrotz 0:a2a71c38065e 344 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 345 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap Current\t 0x%02X", axis.map);
joelvonrotz 0:a2a71c38065e 346 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign Current\t 0x%02X", axis.sign);
joelvonrotz 0:a2a71c38065e 347 #endif
joelvonrotz 0:a2a71c38065e 348
joelvonrotz 0:a2a71c38065e 349 axis.map = register_data[0];
joelvonrotz 0:a2a71c38065e 350 axis.sign = register_data[1];
joelvonrotz 0:a2a71c38065e 351 }
joelvonrotz 0:a2a71c38065e 352
joelvonrotz 0:a2a71c38065e 353 void BNO055::assignAxis(bno055_axis_t x_axis, bno055_axis_t y_axis, bno055_axis_t z_axis)
joelvonrotz 0:a2a71c38065e 354 {
joelvonrotz 0:a2a71c38065e 355 //check if multiple axis have the same remap-axis. If true, then this part can be skipped, as
joelvonrotz 0:a2a71c38065e 356 //it's useless (the chip reverts to the previous state) by the datasheet and doing it now saves some time
joelvonrotz 0:a2a71c38065e 357 if((x_axis == y_axis) || (x_axis == z_axis) || (z_axis == y_axis))
joelvonrotz 0:a2a71c38065e 358 {
joelvonrotz 0:a2a71c38065e 359 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 360 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis \tmultiple Axis");
joelvonrotz 0:a2a71c38065e 361 #endif
joelvonrotz 0:a2a71c38065e 362 }
joelvonrotz 0:a2a71c38065e 363 else
joelvonrotz 0:a2a71c38065e 364 {
joelvonrotz 0:a2a71c38065e 365
joelvonrotz 0:a2a71c38065e 366 axis.map = x_axis + (y_axis << 2) + (z_axis << 4);
joelvonrotz 0:a2a71c38065e 367 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 368 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis new \t%3i",axis.map);
joelvonrotz 0:a2a71c38065e 369 #endif
joelvonrotz 0:a2a71c38065e 370 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS);
joelvonrotz 0:a2a71c38065e 371 }
joelvonrotz 0:a2a71c38065e 372 }
joelvonrotz 0:a2a71c38065e 373
joelvonrotz 0:a2a71c38065e 374 void BNO055::setAxisSign(bno055_axis_sign_t x_sign, bno055_axis_sign_t y_sign, bno055_axis_sign_t z_sign)
joelvonrotz 0:a2a71c38065e 375 {
joelvonrotz 0:a2a71c38065e 376 axis.sign = x_sign + (y_sign << 1) + (z_sign << 2);
joelvonrotz 0:a2a71c38065e 377 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 378 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tAxis Sign new \t%3i",axis.sign);
joelvonrotz 0:a2a71c38065e 379 #endif
joelvonrotz 0:a2a71c38065e 380 m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS);
joelvonrotz 0:a2a71c38065e 381 }
joelvonrotz 0:a2a71c38065e 382
joelvonrotz 0:a2a71c38065e 383 void BNO055::getCalibrationStatus(void)
joelvonrotz 0:a2a71c38065e 384 {
joelvonrotz 0:a2a71c38065e 385 calibration.status = m_i2c_master.i2c_read(m_bno055_address, CALIB_STAT);
joelvonrotz 0:a2a71c38065e 386 calibration.system = (calibration.status >> SHIFT_CALIB_SYSTEM) & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 387 calibration.gyro = (calibration.status >> SHIFT_CALIB_GYRO) & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 388 calibration.acceleration = (calibration.status >> SHIFT_CALIB_ACCEL) & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 389 calibration.magneto = calibration.status & MASK_CALIBRATION_BITS;
joelvonrotz 0:a2a71c38065e 390
joelvonrotz 0:a2a71c38065e 391 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 392 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[S] %1i [G] %1i [A] %1i [M] %1i\t[REG]%3X",calibration.system
joelvonrotz 0:a2a71c38065e 393 ,calibration.gyro
joelvonrotz 0:a2a71c38065e 394 ,calibration.acceleration
joelvonrotz 0:a2a71c38065e 395 ,calibration.magneto
joelvonrotz 0:a2a71c38065e 396 ,calibration.status
joelvonrotz 0:a2a71c38065e 397 );
joelvonrotz 0:a2a71c38065e 398 #endif
joelvonrotz 0:a2a71c38065e 399 }
joelvonrotz 0:a2a71c38065e 400
joelvonrotz 0:a2a71c38065e 401 void BNO055::getInterruptFlag(void)
joelvonrotz 0:a2a71c38065e 402 {
joelvonrotz 0:a2a71c38065e 403 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 404 interrupt.status = m_i2c_master.i2c_read(m_bno055_address, INT_STATUS);
joelvonrotz 0:a2a71c38065e 405
joelvonrotz 0:a2a71c38065e 406 interrupt.gyroscope.any_motion = ((interrupt.status & MASK_GYRO_ANY_MOTION ) == GYRO_ANY_MOTION );
joelvonrotz 0:a2a71c38065e 407 interrupt.gyroscope.high_rate = ((interrupt.status & MASK_GYRO_HIGH_RATE ) == GYRO_HIGH_RATE );
joelvonrotz 0:a2a71c38065e 408 interrupt.acceleration.high_g = ((interrupt.status & MASK_ACCEL_HIGH_G ) == ACCEL_HIGH_G );
joelvonrotz 0:a2a71c38065e 409 interrupt.acceleration.any_motion = ((interrupt.status & MASK_ACCEL_ANY_MOTION) == ACCEL_ANY_MOTION);
joelvonrotz 0:a2a71c38065e 410 interrupt.acceleration.no_motion = ((interrupt.status & MASK_ACCEL_NO_MOTION ) == ACCEL_NO_MOTION );
joelvonrotz 0:a2a71c38065e 411
joelvonrotz 0:a2a71c38065e 412 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 413 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 414 interrupt.gyroscope.any_motion,
joelvonrotz 0:a2a71c38065e 415 interrupt.gyroscope.high_rate,
joelvonrotz 0:a2a71c38065e 416 interrupt.acceleration.high_g,
joelvonrotz 0:a2a71c38065e 417 interrupt.acceleration.any_motion,
joelvonrotz 0:a2a71c38065e 418 interrupt.acceleration.no_motion,
joelvonrotz 0:a2a71c38065e 419 interrupt.status
joelvonrotz 0:a2a71c38065e 420 );
joelvonrotz 0:a2a71c38065e 421 #endif
joelvonrotz 0:a2a71c38065e 422
joelvonrotz 0:a2a71c38065e 423 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 424 }
joelvonrotz 0:a2a71c38065e 425
joelvonrotz 0:a2a71c38065e 426 uint8_t BNO055::getEnabledInterrupts(void)
joelvonrotz 0:a2a71c38065e 427 {
joelvonrotz 0:a2a71c38065e 428 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 429 uint8_t data_interrupt_enable = m_i2c_master.i2c_read(m_bno055_address, INT_EN);
joelvonrotz 0:a2a71c38065e 430 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 431
joelvonrotz 0:a2a71c38065e 432 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 433 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Enabled Interrupt\t0x%02X",data_interrupt_enable);
joelvonrotz 0:a2a71c38065e 434 #endif
joelvonrotz 0:a2a71c38065e 435
joelvonrotz 0:a2a71c38065e 436 return data_interrupt_enable;
joelvonrotz 0:a2a71c38065e 437 }
joelvonrotz 0:a2a71c38065e 438
joelvonrotz 0:a2a71c38065e 439 void BNO055::setEnableInterrupts(bno055_enable_t accel_no_motion,
joelvonrotz 0:a2a71c38065e 440 bno055_enable_t accel_any_motion,
joelvonrotz 0:a2a71c38065e 441 bno055_enable_t accel_high_g,
joelvonrotz 0:a2a71c38065e 442 bno055_enable_t gyro_high_rate,
joelvonrotz 0:a2a71c38065e 443 bno055_enable_t gyro_any_motion
joelvonrotz 0:a2a71c38065e 444 )
joelvonrotz 0:a2a71c38065e 445 {
joelvonrotz 0:a2a71c38065e 446 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 447 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 448 m_i2c_master.i2c_write(m_bno055_address, INT_EN, data_interrupt_enable);
joelvonrotz 0:a2a71c38065e 449
joelvonrotz 0:a2a71c38065e 450 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 451 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Enabled Interrupt\t0x%02X",data_interrupt_enable);
joelvonrotz 0:a2a71c38065e 452 #endif
joelvonrotz 0:a2a71c38065e 453
joelvonrotz 0:a2a71c38065e 454 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 455 }
joelvonrotz 0:a2a71c38065e 456
joelvonrotz 0:a2a71c38065e 457
joelvonrotz 0:a2a71c38065e 458
joelvonrotz 0:a2a71c38065e 459 void BNO055::setInterruptMask(bno055_enable_t accel_no_motion,
joelvonrotz 0:a2a71c38065e 460 bno055_enable_t accel_any_motion,
joelvonrotz 0:a2a71c38065e 461 bno055_enable_t accel_high_g,
joelvonrotz 0:a2a71c38065e 462 bno055_enable_t gyro_high_rate,
joelvonrotz 0:a2a71c38065e 463 bno055_enable_t gyro_any_motion
joelvonrotz 0:a2a71c38065e 464 )
joelvonrotz 0:a2a71c38065e 465 {
joelvonrotz 0:a2a71c38065e 466 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 467 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 468 m_i2c_master.i2c_write(m_bno055_address, INT_MSK, data_interrupt_mask);
joelvonrotz 0:a2a71c38065e 469
joelvonrotz 0:a2a71c38065e 470 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 471 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Interrupt Mask\t0x%02X",data_interrupt_mask);
joelvonrotz 0:a2a71c38065e 472 #endif
joelvonrotz 0:a2a71c38065e 473
joelvonrotz 0:a2a71c38065e 474 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 475 }
joelvonrotz 0:a2a71c38065e 476
joelvonrotz 0:a2a71c38065e 477 uint8_t BNO055::getInterruptMask(void)
joelvonrotz 0:a2a71c38065e 478 {
joelvonrotz 0:a2a71c38065e 479 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 480 uint8_t data_interrupt_mask = m_i2c_master.i2c_read(m_bno055_address, INT_MSK);
joelvonrotz 0:a2a71c38065e 481
joelvonrotz 0:a2a71c38065e 482 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 483 m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Interrupt Mask\t0x%02X",data_interrupt_mask);
joelvonrotz 0:a2a71c38065e 484 #endif
joelvonrotz 0:a2a71c38065e 485
joelvonrotz 0:a2a71c38065e 486 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 487
joelvonrotz 0:a2a71c38065e 488 return data_interrupt_mask;
joelvonrotz 0:a2a71c38065e 489 }
joelvonrotz 0:a2a71c38065e 490
joelvonrotz 0:a2a71c38065e 491 void BNO055::configAccelerationInterrupt(bno055_config_int_axis_t high_axis,
joelvonrotz 0:a2a71c38065e 492 bno055_config_int_axis_t am_nm_axis,
joelvonrotz 0:a2a71c38065e 493 bno055_any_motion_sample_t am_dur
joelvonrotz 0:a2a71c38065e 494 )
joelvonrotz 0:a2a71c38065e 495 {
joelvonrotz 0:a2a71c38065e 496 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 497 uint8_t data = am_dur + (am_nm_axis << SHIFT_AM_NM_AXIS) + (high_axis << SHIFT_HIGH_AXIS);
joelvonrotz 0:a2a71c38065e 498 m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data);
joelvonrotz 0:a2a71c38065e 499
joelvonrotz 0:a2a71c38065e 500 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 501 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 502 #endif
joelvonrotz 0:a2a71c38065e 503
joelvonrotz 0:a2a71c38065e 504 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 505 }
joelvonrotz 0:a2a71c38065e 506
joelvonrotz 0:a2a71c38065e 507 void BNO055::configGyroscopeInterrupt(bno055_config_int_axis_t hr_axis,
joelvonrotz 0:a2a71c38065e 508 bno055_config_int_axis_t am_axis,
joelvonrotz 0:a2a71c38065e 509 bool high_rate_unfilt,
joelvonrotz 0:a2a71c38065e 510 bool any_motion_unfilt
joelvonrotz 0:a2a71c38065e 511 )
joelvonrotz 0:a2a71c38065e 512 {
joelvonrotz 0:a2a71c38065e 513 setPage(PAGE_1);
joelvonrotz 0:a2a71c38065e 514 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 515 m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data);
joelvonrotz 0:a2a71c38065e 516
joelvonrotz 0:a2a71c38065e 517 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 518 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 519 #endif
joelvonrotz 0:a2a71c38065e 520
joelvonrotz 0:a2a71c38065e 521 setPage(PAGE_0);
joelvonrotz 0:a2a71c38065e 522 }
joelvonrotz 0:a2a71c38065e 523
joelvonrotz 0:a2a71c38065e 524 //ACCELERATION
joelvonrotz 0:a2a71c38065e 525
joelvonrotz 0:a2a71c38065e 526 void BNO055::getAcceleration(void)
joelvonrotz 0:a2a71c38065e 527 {
joelvonrotz 0:a2a71c38065e 528 if(format.acceleration == ACCELERATION)
joelvonrotz 0:a2a71c38065e 529 {
joelvonrotz 0:a2a71c38065e 530 get(ACC_DATA_VECTOR, accel, ACCELERATION_FORMAT, false);
joelvonrotz 0:a2a71c38065e 531 }
joelvonrotz 0:a2a71c38065e 532 else if(format.acceleration == MILLI_G_FORCE)
joelvonrotz 0:a2a71c38065e 533 {
joelvonrotz 0:a2a71c38065e 534 get(ACC_DATA_VECTOR, accel, MILLI_G_FORCE_FORMAT, false);
joelvonrotz 0:a2a71c38065e 535 }
joelvonrotz 0:a2a71c38065e 536
joelvonrotz 0:a2a71c38065e 537 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 538 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 539 #endif
joelvonrotz 0:a2a71c38065e 540 }
joelvonrotz 0:a2a71c38065e 541
joelvonrotz 0:a2a71c38065e 542 //MAGNETOMETER
joelvonrotz 0:a2a71c38065e 543
joelvonrotz 0:a2a71c38065e 544 void BNO055::getMagnetometer(void)
joelvonrotz 0:a2a71c38065e 545 {
joelvonrotz 0:a2a71c38065e 546 get(MAG_DATA_VECTOR, magneto, MICRO_TESLA_FORMAT, false);
joelvonrotz 0:a2a71c38065e 547
joelvonrotz 0:a2a71c38065e 548 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 549 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 550 #endif
joelvonrotz 0:a2a71c38065e 551 }
joelvonrotz 0:a2a71c38065e 552
joelvonrotz 0:a2a71c38065e 553 //GYROSCOPE
joelvonrotz 0:a2a71c38065e 554
joelvonrotz 0:a2a71c38065e 555 void BNO055::getGyroscope(void)
joelvonrotz 0:a2a71c38065e 556 {
joelvonrotz 0:a2a71c38065e 557 if(format.gyroscope == DEGREE_PER_SEC)
joelvonrotz 0:a2a71c38065e 558 {
joelvonrotz 0:a2a71c38065e 559 get(GYR_DATA_VECTOR, gyro, GYRO_DEGREE_PER_SEC_FORMAT, false);
joelvonrotz 0:a2a71c38065e 560 }
joelvonrotz 0:a2a71c38065e 561 else if(format.gyroscope == RADIAN_PER_SEC)
joelvonrotz 0:a2a71c38065e 562 {
joelvonrotz 0:a2a71c38065e 563 get(GYR_DATA_VECTOR, gyro, GYRO_RADIAN_PER_SEC_FORMAT, false);
joelvonrotz 0:a2a71c38065e 564 }
joelvonrotz 0:a2a71c38065e 565 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 566 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 567 #endif
joelvonrotz 0:a2a71c38065e 568 }
joelvonrotz 0:a2a71c38065e 569
joelvonrotz 0:a2a71c38065e 570 //EULER-DEGREES
joelvonrotz 0:a2a71c38065e 571
joelvonrotz 0:a2a71c38065e 572 void BNO055::getEulerDegrees(void)
joelvonrotz 0:a2a71c38065e 573 {
joelvonrotz 0:a2a71c38065e 574 if(format.euler == DEGREES)
joelvonrotz 0:a2a71c38065e 575 {
joelvonrotz 0:a2a71c38065e 576 get(EUL_DATA_VECTOR, euler, EULER_DEGREES_FORMAT, true);
joelvonrotz 0:a2a71c38065e 577 }
joelvonrotz 0:a2a71c38065e 578 else if(format.euler == RADIANS)
joelvonrotz 0:a2a71c38065e 579 {
joelvonrotz 0:a2a71c38065e 580 get(EUL_DATA_VECTOR, euler, EULER_RADIANS_FORMAT, true);
joelvonrotz 0:a2a71c38065e 581 }
joelvonrotz 0:a2a71c38065e 582 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 583 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 584 #endif
joelvonrotz 0:a2a71c38065e 585 }
joelvonrotz 0:a2a71c38065e 586
joelvonrotz 0:a2a71c38065e 587 //QUATERNION
joelvonrotz 0:a2a71c38065e 588
joelvonrotz 0:a2a71c38065e 589 void BNO055::getQuaternion(void)
joelvonrotz 0:a2a71c38065e 590 {
joelvonrotz 0:a2a71c38065e 591 get(QUA_DATA_VECTOR, quaternion, QUATERNION_FORMAT);
joelvonrotz 0:a2a71c38065e 592
joelvonrotz 0:a2a71c38065e 593 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 594 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 595 #endif
joelvonrotz 0:a2a71c38065e 596 }
joelvonrotz 0:a2a71c38065e 597
joelvonrotz 0:a2a71c38065e 598 //LINEAR ACCELERATION
joelvonrotz 0:a2a71c38065e 599
joelvonrotz 0:a2a71c38065e 600 void BNO055::getLinearAcceleration(void)
joelvonrotz 0:a2a71c38065e 601 {
joelvonrotz 0:a2a71c38065e 602 if(format.acceleration == ACCELERATION)
joelvonrotz 0:a2a71c38065e 603 {
joelvonrotz 0:a2a71c38065e 604 get(LIA_DATA_VECTOR, linear_accel, ACCELERATION_FORMAT, false);
joelvonrotz 0:a2a71c38065e 605 }
joelvonrotz 0:a2a71c38065e 606 else if(format.acceleration == MILLI_G_FORCE)
joelvonrotz 0:a2a71c38065e 607 {
joelvonrotz 0:a2a71c38065e 608 get(LIA_DATA_VECTOR, linear_accel, MILLI_G_FORCE_FORMAT, false);
joelvonrotz 0:a2a71c38065e 609 }
joelvonrotz 0:a2a71c38065e 610 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 611 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 612 #endif
joelvonrotz 0:a2a71c38065e 613 }
joelvonrotz 0:a2a71c38065e 614
joelvonrotz 0:a2a71c38065e 615 //GRAVITY VECTOR
joelvonrotz 0:a2a71c38065e 616
joelvonrotz 0:a2a71c38065e 617 void BNO055::getGravityVector(void)
joelvonrotz 0:a2a71c38065e 618 {
joelvonrotz 0:a2a71c38065e 619 if(format.acceleration == ACCELERATION)
joelvonrotz 0:a2a71c38065e 620 {
joelvonrotz 0:a2a71c38065e 621 get(ACC_DATA_VECTOR, gravity_vector, ACCELERATION_FORMAT, false);
joelvonrotz 0:a2a71c38065e 622 }
joelvonrotz 0:a2a71c38065e 623 else if(format.acceleration == MILLI_G_FORCE)
joelvonrotz 0:a2a71c38065e 624 {
joelvonrotz 0:a2a71c38065e 625 get(ACC_DATA_VECTOR, gravity_vector, MILLI_G_FORCE_FORMAT, false);
joelvonrotz 0:a2a71c38065e 626 }
joelvonrotz 0:a2a71c38065e 627 #ifdef DEBUGGING_ENABLED
joelvonrotz 0:a2a71c38065e 628 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 629 #endif
joelvonrotz 0:a2a71c38065e 630 }
joelvonrotz 0:a2a71c38065e 631
joelvonrotz 0:a2a71c38065e 632 //TEMPERATURE
joelvonrotz 0:a2a71c38065e 633
joelvonrotz 0:a2a71c38065e 634 void BNO055::getTemperature(void)
joelvonrotz 0:a2a71c38065e 635 {
joelvonrotz 0:a2a71c38065e 636 m_i2c_master.i2c_readSeries(m_bno055_address, TEMP, register_data, LENGTH_1_BYTES);
joelvonrotz 0:a2a71c38065e 637 if(format.temperature == CELSIUS)
joelvonrotz 0:a2a71c38065e 638 {
joelvonrotz 0:a2a71c38065e 639 temperature = static_cast<float>(register_data[0]) * TEMPERATURE_CELSIUS_FORMAT;
joelvonrotz 0:a2a71c38065e 640 }
joelvonrotz 0:a2a71c38065e 641 if(format.temperature == FAHRENHEIT)
joelvonrotz 0:a2a71c38065e 642 {
joelvonrotz 0:a2a71c38065e 643 temperature = static_cast<float>(register_data[0]) * TEMPERATURE_FAHRENHEIT_FORMAT;
joelvonrotz 0:a2a71c38065e 644 }
joelvonrotz 0:a2a71c38065e 645 }
joelvonrotz 0:a2a71c38065e 646
joelvonrotz 0:a2a71c38065e 647 void BNO055::get(bno055_reg_t address, bno055_data_s& data, const float format, bool is_euler)
joelvonrotz 0:a2a71c38065e 648 {
joelvonrotz 0:a2a71c38065e 649 m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_6_BYTES);
joelvonrotz 0:a2a71c38065e 650
joelvonrotz 0:a2a71c38065e 651 for(arrayIndex = 0 ; arrayIndex < (LENGTH_6_BYTES/2) ; arrayIndex++)
joelvonrotz 0:a2a71c38065e 652 {
joelvonrotz 0:a2a71c38065e 653 sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE);
joelvonrotz 0:a2a71c38065e 654 }
joelvonrotz 0:a2a71c38065e 655
joelvonrotz 0:a2a71c38065e 656 if(is_euler)
joelvonrotz 0:a2a71c38065e 657 {
joelvonrotz 0:a2a71c38065e 658 data.z = format * static_cast<float>(sensor_data[0]);
joelvonrotz 0:a2a71c38065e 659 data.y = format * static_cast<float>(sensor_data[1]);
joelvonrotz 0:a2a71c38065e 660 data.x = format * static_cast<float>(sensor_data[2]);
joelvonrotz 0:a2a71c38065e 661 }
joelvonrotz 0:a2a71c38065e 662 else
joelvonrotz 0:a2a71c38065e 663 {
joelvonrotz 0:a2a71c38065e 664 data.x = format * static_cast<float>(sensor_data[0]);
joelvonrotz 0:a2a71c38065e 665 data.y = format * static_cast<float>(sensor_data[1]);
joelvonrotz 0:a2a71c38065e 666 data.z = format * static_cast<float>(sensor_data[2]);
joelvonrotz 0:a2a71c38065e 667 }
joelvonrotz 0:a2a71c38065e 668 }
joelvonrotz 0:a2a71c38065e 669
joelvonrotz 0:a2a71c38065e 670 void BNO055::get(bno055_reg_t address, bno055_quaternion_s& data, const float format)
joelvonrotz 0:a2a71c38065e 671 {
joelvonrotz 0:a2a71c38065e 672 m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_8_BYTES);
joelvonrotz 0:a2a71c38065e 673
joelvonrotz 0:a2a71c38065e 674 for(arrayIndex = 0 ; arrayIndex < (LENGTH_8_BYTES/2) ; arrayIndex++)
joelvonrotz 0:a2a71c38065e 675 {
joelvonrotz 0:a2a71c38065e 676 sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE);
joelvonrotz 0:a2a71c38065e 677 }
joelvonrotz 0:a2a71c38065e 678
joelvonrotz 0:a2a71c38065e 679 data.w = format * static_cast<float>(sensor_data[0]);
joelvonrotz 0:a2a71c38065e 680 data.x = format * static_cast<float>(sensor_data[1]);
joelvonrotz 0:a2a71c38065e 681 data.y = format * static_cast<float>(sensor_data[2]);
joelvonrotz 0:a2a71c38065e 682 data.z = format * static_cast<float>(sensor_data[3]);
joelvonrotz 0:a2a71c38065e 683 }
joelvonrotz 0:a2a71c38065e 684