#include "qt_pi.h"

void initIMU(){
    
    Wire.begin();

    uint8_t buf;

    I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
    
    I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250);
    I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2);
    
    I2Cdev::writeBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false);
    
    I2Cdev::readBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &buf);
    
    I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XA_OFFS_H, 620);
    I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_YA_OFFS_H, -2300);
    I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZA_OFFS_H, 1500);
    I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_XG_OFFS_USRH, 29);
    I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_YG_OFFS_USRH, 0);
    I2Cdev::writeWord(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ZG_OFFS_USRH, 0);
  
}

uint8_t bufimu[14 + com_preamble_len];

void sendIMU() { 
  
  I2Cdev::readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14, bufimu + DATASTRB);
  
  bufimu[COMB] = IMU_DATA;
  bufimu[CIDB] = BID;

  sendData(14 + com_preamble_len, bufimu);

}
