Joel von Rotz
A library of the 9-Axis Sensor BNO055 from Bosch Sensortec.
Requires the I2C-Master library - Link
Diff: bno055.cpp
- Revision:
- 0:a2a71c38065e
- Child:
- 1:6c64ebf1f2b5
diff -r 000000000000 -r a2a71c38065e bno055.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/bno055.cpp Thu Jul 11 09:34:29 2019 +0000
@@ -0,0 +1,643 @@
+/**
+ * @brief Implementation of BNO055 Class.
+ *
+ * @file bno055.cpp
+ * @author Joel von Rotz
+ * @date 18.07.2018
+ */
+#include "mbed.h"
+#include "bno055.h"
+
+/* TODO ----------------------------------
+ * [+] comment blocks for all function
+ * [+] error handler
+ * [+] finish up documentation
+ * [~] rename variables to more understandable names
+ * [~] replace magic numbers with constants
+ * TODONE --------------------------------
+ *
+ * ---------------------------------------
+ */
+
+#ifdef DEBUGGING_ENABLED
+BNO055::BNO055(uint8_t slave_address, DoWi& dowi, DigitalOut& ResetPin, bool external_clk, Serial& DEBUG_SERIAL) :
+ m_i2c_master(dowi),
+ m_ResetPin(ResetPin),
+ m_DEBUG_SERIAL(DEBUG_SERIAL),
+ m_bno055_address(slave_address << 1)
+{
+ ResetPin = 1;
+ wait_ms(500);
+
+ #ifdef HARDWARE_RESET
+ resetHW();
+ #else
+ setOperationMode(OPERATION_MODE_CONFIGMODE);
+ resetSW();
+ #endif
+
+ setPage(PAGE_0);
+ getIDs();
+ setOperationMode(OPERATION_MODE_CONFIGMODE);
+ setPowerMode(POWER_NORMAL);
+ setUnitFormat(WINDOWS, CELSIUS, DEGREES, DEGREE_PER_SEC, ACCELERATION);
+
+ useExternalOscillator(external_clk);
+
+ setOrientation(REMAP_OPTION_P1);
+}
+#else
+BNO055::BNO055(uint8_t slave_address, I2C_Master& i2c_master, DigitalOut& ResetPin, bool external_clk) :
+ m_i2c_master(i2c_master),
+ m_ResetPin(ResetPin),
+ m_bno055_address(slave_address << 1)
+{
+ ResetPin = 1;
+ wait_ms(500);
+
+ #ifdef HARDWARE_RESET
+ resetHW();
+ #else
+ setOperationMode(OPERATION_MODE_CONFIGMODE);
+ resetSW();
+ #endif
+
+ setPage();
+ getIDs();
+ setOperationMode(OPERATION_MODE_CONFIGMODE);
+ setPowerMode(POWER_NORMAL);
+ setUnitFormat(WINDOWS, CELSIUS, DEGREES, DEGREE_PER_SEC, ACCELERATION);
+
+ useExternalOscillator(external_clk);
+
+ setOrientation(REMAP_OPTION_P1);
+}
+#endif
+
+void BNO055::setUnitFormat(bno055_orientation_t new_orientation_format,
+ bno055_temperature_t new_temperature_format,
+ bno055_euler_t new_euler_format,
+ bno055_gyro_t new_gyroscope_format,
+ bno055_acceleration_t new_acceleration_format)
+{
+ uint8_t new_unit_data = new_orientation_format + new_temperature_format + new_euler_format + new_gyroscope_format + new_acceleration_format;
+
+ format.units = new_unit_data;
+ format.orientation = new_orientation_format;
+ format.temperature = new_temperature_format;
+ format.euler = new_euler_format;
+ format.gyroscope = new_gyroscope_format;
+ format.acceleration = new_acceleration_format;
+
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tUnits Set\t 0x%02X",new_unit_data);
+ #endif
+ m_i2c_master.i2c_writeBits(m_bno055_address, UNIT_SEL, new_unit_data, MASK_UNIT_SEL);
+}
+
+void BNO055::getUnitFormat(void)
+{
+
+}
+
+void BNO055::getIDs(void)
+{
+ uint8_t dataArray[7];
+ m_i2c_master.i2c_readSeries(m_bno055_address, CHIP_ID, dataArray, 7);
+ id.chip = dataArray[0];
+ id.accel = dataArray[1];
+ id.magneto = dataArray[2];
+ id.gyro = dataArray[3];
+ id.sw_rev = dataArray[4] + (static_cast<uint16_t>(dataArray[5]) << 8);
+ id.bl_rev = dataArray[6];
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tIDs---------------------");
+ m_DEBUG_SERIAL.printf("\r\n\tChip\t\t 0x%02X",id.chip);
+ m_DEBUG_SERIAL.printf("\r\n\tAcceleraiton\t 0x%02X",id.accel);
+ m_DEBUG_SERIAL.printf("\r\n\tMagnetometer\t 0x%02X",id.magneto);
+ m_DEBUG_SERIAL.printf("\r\n\tGyroscope\t 0x%02X",id.gyro);
+ m_DEBUG_SERIAL.printf("\r\n\tSofware Rev.\t 0x%04X",id.sw_rev);
+ m_DEBUG_SERIAL.printf("\r\n\tBootloader\t 0x%02X",id.bl_rev);
+ #endif
+}
+
+void BNO055::setPowerMode(bno055_powermode_t new_power_mode)
+{
+ if(new_power_mode != mode.power)
+ {
+ do
+ {
+ m_i2c_master.i2c_writeBits(m_bno055_address, PWR_MODE, new_power_mode, MASK_POWER_MODE);
+ wait_ms(50.0);
+ }
+ while((m_i2c_master.i2c_read(m_bno055_address, PWR_MODE) & MASK_POWER_MODE) != new_power_mode);
+ //and the old operation mode should only be updated, when there's a new mode.
+ mode.power = new_power_mode;
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode new\t %3i", new_power_mode);
+ #endif
+ }
+ else
+ {
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpowerMode\t no change");
+ #endif
+ }
+}
+
+uint8_t BNO055::getPowerMode(void)
+{
+ mode.power = m_i2c_master.i2c_read(m_bno055_address, PWR_MODE);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpwrMode current\t %3i",mode.power);
+ #endif
+ return mode.power;
+}
+
+void BNO055::setOperationMode(bno055_opr_mode_t new_opr_mode)
+{
+ //trying to write a mode, that is already being used, doesn't make any sense and can be ignored.
+ if(new_opr_mode != mode.operation)
+ {
+ do
+ {
+ m_i2c_master.i2c_writeBits(m_bno055_address, OPR_MODE, new_opr_mode, MASK_OPERAITON_MODE);
+ wait_ms(50.0);
+ }
+ while((m_i2c_master.i2c_read(m_bno055_address, OPR_MODE) & MASK_OPERAITON_MODE) != new_opr_mode);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode new\t %3i", new_opr_mode);
+ #endif
+
+ //and the old operation mode should only be updated, when there's a new mode.
+ mode.operation = new_opr_mode;
+ }
+ else
+ {
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode\t\t no change");
+ #endif
+ }
+}
+
+uint8_t BNO055::getOperationMode(void)
+{
+ mode.operation = m_i2c_master.i2c_read(m_bno055_address, OPR_MODE);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\toprMode current\t %3i", mode.operation);
+ #endif
+ return mode.operation;
+}
+
+void BNO055::setPage(bno055_page_t new_page)
+{
+ page = new_page;
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage set\t %3i", page);
+ #endif
+
+ m_i2c_master.i2c_write(m_bno055_address, PAGE_ID, page);
+}
+
+uint8_t BNO055::getPage(void)
+{
+ page = m_i2c_master.i2c_read(m_bno055_address, PAGE_ID);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tpage current\t %3i",page);
+ #endif
+
+ return page;
+}
+
+uint8_t BNO055::getSystemStatus(void)
+{
+ system.status = m_i2c_master.i2c_read(m_bno055_address, SYS_STATUS);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysStatus\t %3i",system.status);
+ #endif
+ return system.status;
+}
+
+uint8_t BNO055::getSystemError(void)
+{
+ system.error = m_i2c_master.i2c_read(m_bno055_address, SYS_ERR);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tsysError\t %3i",system.error);
+ #endif
+ return system.error;
+}
+
+void BNO055::useExternalOscillator(bool enabled)
+{
+ char data = static_cast<char>(enabled) << 7;
+ #ifdef DEBUGGING_ENABLED
+ if(enabled)
+ {
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source\t active");
+ }
+ else
+ {
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tExt.Source \tinactive");
+ }
+ #endif
+ m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, data, MASK_EXT_CLOCK);
+}
+
+void BNO055::resetSW(void)
+{
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSoftware Reset");
+ #endif
+
+ m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x20, MASK_SYSTEM_RESET);
+
+ //datasheet (p.13 of revision 14) indicate a Power-On-Reset-time of 650ms
+ //from Reset to Config Mode. By constantly reading the chip id, it can be
+ //determined, when the chip is fully functional again.
+ while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID)
+ {
+ wait_ms(10.0);
+ }
+
+ m_i2c_master.i2c_writeBits(m_bno055_address, SYS_TRIGGER, 0x00, MASK_SYSTEM_RESET);
+}
+
+void BNO055::resetHW(void)
+{
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tHardware Reset");
+ #endif
+ m_ResetPin = 0;
+ wait_ms(10.0);
+ m_ResetPin = 1;
+ while(m_i2c_master.i2c_read(m_bno055_address, CHIP_ID) != BNO055_ID)
+ {
+ wait_ms(10.0);
+ }
+
+}
+
+void BNO055::setOrientation(bno055_remap_options_t orientation_placement)
+{
+ axis.map = (orientation_placement >> SHIFT_1BYTE) & 0xFF;
+ axis.sign = (orientation_placement & 0xFF);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap New\t 0x%02X", axis.map);
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign New\t 0x%02X", axis.sign);
+ #endif
+
+ m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS);
+ m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS);
+}
+
+void BNO055::getOrientation(void)
+{
+ m_i2c_master.i2c_readSeries(m_bno055_address, AXIS_MAP_CONFIG,register_data,LENGTH_2_BYTES);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tMap Current\t 0x%02X", axis.map);
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSign Current\t 0x%02X", axis.sign);
+ #endif
+
+ axis.map = register_data[0];
+ axis.sign = register_data[1];
+}
+
+void BNO055::assignAxis(bno055_axis_t x_axis, bno055_axis_t y_axis, bno055_axis_t z_axis)
+{
+ //check if multiple axis have the same remap-axis. If true, then this part can be skipped, as
+ //it's useless (the chip reverts to the previous state) by the datasheet and doing it now saves some time
+ if((x_axis == y_axis) || (x_axis == z_axis) || (z_axis == y_axis))
+ {
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis \tmultiple Axis");
+ #endif
+ }
+ else
+ {
+
+ axis.map = x_axis + (y_axis << 2) + (z_axis << 4);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tassignAxis new \t%3i",axis.map);
+ #endif
+ m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_CONFIG, axis.map, MASK_REMAP_AXIS);
+ }
+}
+
+void BNO055::setAxisSign(bno055_axis_sign_t x_sign, bno055_axis_sign_t y_sign, bno055_axis_sign_t z_sign)
+{
+ axis.sign = x_sign + (y_sign << 1) + (z_sign << 2);
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tAxis Sign new \t%3i",axis.sign);
+ #endif
+ m_i2c_master.i2c_writeBits(m_bno055_address, AXIS_MAP_SIGN, axis.sign, MASK_SIGN_AXIS);
+}
+
+void BNO055::getCalibrationStatus(void)
+{
+ calibration.status = m_i2c_master.i2c_read(m_bno055_address, CALIB_STAT);
+ calibration.system = (calibration.status >> SHIFT_CALIB_SYSTEM) & MASK_CALIBRATION_BITS;
+ calibration.gyro = (calibration.status >> SHIFT_CALIB_GYRO) & MASK_CALIBRATION_BITS;
+ calibration.acceleration = (calibration.status >> SHIFT_CALIB_ACCEL) & MASK_CALIBRATION_BITS;
+ calibration.magneto = calibration.status & MASK_CALIBRATION_BITS;
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\t[S] %1i [G] %1i [A] %1i [M] %1i\t[REG]%3X",calibration.system
+ ,calibration.gyro
+ ,calibration.acceleration
+ ,calibration.magneto
+ ,calibration.status
+ );
+ #endif
+}
+
+void BNO055::getInterruptFlag(void)
+{
+ setPage(PAGE_1);
+ interrupt.status = m_i2c_master.i2c_read(m_bno055_address, INT_STATUS);
+
+ interrupt.gyroscope.any_motion = ((interrupt.status & MASK_GYRO_ANY_MOTION ) == GYRO_ANY_MOTION );
+ interrupt.gyroscope.high_rate = ((interrupt.status & MASK_GYRO_HIGH_RATE ) == GYRO_HIGH_RATE );
+ interrupt.acceleration.high_g = ((interrupt.status & MASK_ACCEL_HIGH_G ) == ACCEL_HIGH_G );
+ interrupt.acceleration.any_motion = ((interrupt.status & MASK_ACCEL_ANY_MOTION) == ACCEL_ANY_MOTION);
+ interrupt.acceleration.no_motion = ((interrupt.status & MASK_ACCEL_NO_MOTION ) == ACCEL_NO_MOTION );
+
+ #ifdef DEBUGGING_ENABLED
+ 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",
+ interrupt.gyroscope.any_motion,
+ interrupt.gyroscope.high_rate,
+ interrupt.acceleration.high_g,
+ interrupt.acceleration.any_motion,
+ interrupt.acceleration.no_motion,
+ interrupt.status
+ );
+ #endif
+
+ setPage(PAGE_0);
+}
+
+uint8_t BNO055::getEnabledInterrupts(void)
+{
+ setPage(PAGE_1);
+ uint8_t data_interrupt_enable = m_i2c_master.i2c_read(m_bno055_address, INT_EN);
+ setPage(PAGE_0);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Enabled Interrupt\t0x%02X",data_interrupt_enable);
+ #endif
+
+ return data_interrupt_enable;
+}
+
+void BNO055::setEnableInterrupts(bno055_enable_t accel_no_motion,
+ bno055_enable_t accel_any_motion,
+ bno055_enable_t accel_high_g,
+ bno055_enable_t gyro_high_rate,
+ bno055_enable_t gyro_any_motion
+ )
+{
+ setPage(PAGE_1);
+ 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);
+ m_i2c_master.i2c_write(m_bno055_address, INT_EN, data_interrupt_enable);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Enabled Interrupt\t0x%02X",data_interrupt_enable);
+ #endif
+
+ setPage(PAGE_0);
+}
+
+
+
+void BNO055::setInterruptMask(bno055_enable_t accel_no_motion,
+ bno055_enable_t accel_any_motion,
+ bno055_enable_t accel_high_g,
+ bno055_enable_t gyro_high_rate,
+ bno055_enable_t gyro_any_motion
+ )
+{
+ setPage(PAGE_1);
+ 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);
+ m_i2c_master.i2c_write(m_bno055_address, INT_MSK, data_interrupt_mask);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tSet Interrupt Mask\t0x%02X",data_interrupt_mask);
+ #endif
+
+ setPage(PAGE_0);
+}
+
+uint8_t BNO055::getInterruptMask(void)
+{
+ setPage(PAGE_1);
+ uint8_t data_interrupt_mask = m_i2c_master.i2c_read(m_bno055_address, INT_MSK);
+
+ #ifdef DEBUGGING_ENABLED
+ m_DEBUG_SERIAL.printf("\r\n[DEBUG]\tCurrent Interrupt Mask\t0x%02X",data_interrupt_mask);
+ #endif
+
+ setPage(PAGE_0);
+
+ return data_interrupt_mask;
+}
+
+void BNO055::configAccelerationInterrupt(bno055_config_int_axis_t high_axis,
+ bno055_config_int_axis_t am_nm_axis,
+ bno055_any_motion_sample_t am_dur
+ )
+{
+ setPage(PAGE_1);
+ uint8_t data = am_dur + (am_nm_axis << SHIFT_AM_NM_AXIS) + (high_axis << SHIFT_HIGH_AXIS);
+ m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data);
+
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+
+ setPage(PAGE_0);
+}
+
+void BNO055::configGyroscopeInterrupt(bno055_config_int_axis_t hr_axis,
+ bno055_config_int_axis_t am_axis,
+ bool high_rate_unfilt,
+ bool any_motion_unfilt
+ )
+{
+ setPage(PAGE_1);
+ 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);
+ m_i2c_master.i2c_write(m_bno055_address, ACC_INT_SETTING,data);
+
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+
+ setPage(PAGE_0);
+}
+
+//ACCELERATION
+
+void BNO055::getAcceleration(void)
+{
+ if(format.acceleration == ACCELERATION)
+ {
+ get(ACC_DATA_VECTOR, accel, ACCELERATION_FORMAT, false);
+ }
+ else if(format.acceleration == MILLI_G_FORCE)
+ {
+ get(ACC_DATA_VECTOR, accel, MILLI_G_FORCE_FORMAT, false);
+ }
+
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//MAGNETOMETER
+
+void BNO055::getMagnetometer(void)
+{
+ get(MAG_DATA_VECTOR, magneto, MICRO_TESLA_FORMAT, false);
+
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//GYROSCOPE
+
+void BNO055::getGyroscope(void)
+{
+ if(format.gyroscope == DEGREE_PER_SEC)
+ {
+ get(GYR_DATA_VECTOR, gyro, GYRO_DEGREE_PER_SEC_FORMAT, false);
+ }
+ else if(format.gyroscope == RADIAN_PER_SEC)
+ {
+ get(GYR_DATA_VECTOR, gyro, GYRO_RADIAN_PER_SEC_FORMAT, false);
+ }
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//EULER-DEGREES
+
+void BNO055::getEulerDegrees(void)
+{
+ if(format.euler == DEGREES)
+ {
+ get(EUL_DATA_VECTOR, euler, EULER_DEGREES_FORMAT, true);
+ }
+ else if(format.euler == RADIANS)
+ {
+ get(EUL_DATA_VECTOR, euler, EULER_RADIANS_FORMAT, true);
+ }
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//QUATERNION
+
+void BNO055::getQuaternion(void)
+{
+ get(QUA_DATA_VECTOR, quaternion, QUATERNION_FORMAT);
+
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//LINEAR ACCELERATION
+
+void BNO055::getLinearAcceleration(void)
+{
+ if(format.acceleration == ACCELERATION)
+ {
+ get(LIA_DATA_VECTOR, linear_accel, ACCELERATION_FORMAT, false);
+ }
+ else if(format.acceleration == MILLI_G_FORCE)
+ {
+ get(LIA_DATA_VECTOR, linear_accel, MILLI_G_FORCE_FORMAT, false);
+ }
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//GRAVITY VECTOR
+
+void BNO055::getGravityVector(void)
+{
+ if(format.acceleration == ACCELERATION)
+ {
+ get(ACC_DATA_VECTOR, gravity_vector, ACCELERATION_FORMAT, false);
+ }
+ else if(format.acceleration == MILLI_G_FORCE)
+ {
+ get(ACC_DATA_VECTOR, gravity_vector, MILLI_G_FORCE_FORMAT, false);
+ }
+ #ifdef DEBUGGING_ENABLED
+ 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);
+ #endif
+}
+
+//TEMPERATURE
+
+void BNO055::getTemperature(void)
+{
+ m_i2c_master.i2c_readSeries(m_bno055_address, TEMP, register_data, LENGTH_1_BYTES);
+ if(format.temperature == CELSIUS)
+ {
+ temperature = static_cast<float>(register_data[0]) * TEMPERATURE_CELSIUS_FORMAT;
+ }
+ if(format.temperature == FAHRENHEIT)
+ {
+ temperature = static_cast<float>(register_data[0]) * TEMPERATURE_FAHRENHEIT_FORMAT;
+ }
+}
+
+void BNO055::get(bno055_reg_t address, bno055_data_s& data, const float format, bool is_euler)
+{
+ m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_6_BYTES);
+
+ for(arrayIndex = 0 ; arrayIndex < (LENGTH_6_BYTES/2) ; arrayIndex++)
+ {
+ sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE);
+ }
+
+ if(is_euler)
+ {
+ data.z = format * static_cast<float>(sensor_data[0]);
+ data.y = format * static_cast<float>(sensor_data[1]);
+ data.x = format * static_cast<float>(sensor_data[2]);
+ }
+ else
+ {
+ data.x = format * static_cast<float>(sensor_data[0]);
+ data.y = format * static_cast<float>(sensor_data[1]);
+ data.z = format * static_cast<float>(sensor_data[2]);
+ }
+}
+
+void BNO055::get(bno055_reg_t address, bno055_quaternion_s& data, const float format)
+{
+ m_i2c_master.i2c_readSeries(m_bno055_address, address, register_data, LENGTH_8_BYTES);
+
+ for(arrayIndex = 0 ; arrayIndex < (LENGTH_8_BYTES/2) ; arrayIndex++)
+ {
+ sensor_data[arrayIndex] = register_data[arrayIndex * 2] + (register_data[arrayIndex * 2 + 1] << SHIFT_1BYTE);
+ }
+
+ data.w = format * static_cast<float>(sensor_data[0]);
+ data.x = format * static_cast<float>(sensor_data[1]);
+ data.y = format * static_cast<float>(sensor_data[2]);
+ data.z = format * static_cast<float>(sensor_data[3]);
+}
+