Joel von Rotz / BNO055

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

Requires the I2C-Master library - Link

bno055.cpp@0:a2a71c38065e, 2019-07-11 (annotated)

Committer:
joelvonrotz
Date:
Thu Jul 11 09:34:29 2019 +0000
Revision:
0:a2a71c38065e
Child:
1:6c64ebf1f2b5
BNO055 library

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