Joel von Rotz / BNO055

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

Requires the I2C-Master library - Link

bno055.cpp@2:d7baee646b4e, 2019-07-11 (annotated)

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