File content as of revision 0:a2a71c38065e:

/**
 * @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]);
}