#include "i2c.h"

#include "imu.h"

static const unsigned char IMU_Addr = 0x6A; // ACC GYR

static inline short Rev16(short v)
{
  asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
  return v;
}
//------------------------------------------------------------------------------

static inline void IMU_SetReg(unsigned char Reg, unsigned char Value)
{
  unsigned char reg[2];
  reg[0]=Reg; reg[1]=Value;
  I2C2_Write(IMU_Addr, reg, 2);
 I2C2_Stop();
}
//------------------------------------------------------------------------------

static inline unsigned char IMU_GetReg(unsigned char Reg)
{
  I2C2_Write(IMU_Addr, Reg);
  I2C2_Read(IMU_Addr, &Reg, 1);
  I2C2_Stop();
  return Reg;
}
//------------------------------------------------------------------------------

void IMU_Init() 
{ 
  I2C2_Init();
  
  for(int a=0; a<100000; a++) { asm volatile("NOP"); }

  unsigned char wai=IMU_GetReg(0x0F);
  
   IMU_SetReg(0x10, 0x38);
  IMU_SetReg(0x11, 0x48);
  IMU_SetReg(0x12, 0x06);
  

  
  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
}
//------------------------------------------------------------------------------

void IMU_Get(IMU_Data& Data)
{
  struct {short temp; short gx; short gy; short gz; short ax; short ay; short az; } data;

  I2C2_Write(IMU_Addr, 0x20);
  I2C2_Read(IMU_Addr, &data, sizeof(data));
  I2C2_Stop();

  Data.Temp  = 21+(((long)Rev16(data.temp))*128)/42735; // 21+(temperature / 333.87)
  
  
  //------------------------------ imu perevernuta, x z inverse
  Data.Acc.X = -Rev16(data.ax);
  Data.Acc.Y = Rev16(data.ay);
  Data.Acc.Z = -Rev16(data.az);
  
  Data.Gyr.X = -Rev16(data.gx);
  Data.Gyr.Y = Rev16(data.gy);
  Data.Gyr.Z = -Rev16(data.gz);
  
}
//------------------------------------------------------------------------------