Код для использования Акселерометра LIS302DLTR в MultiWii

Убил блин почти целый день, но сделал таки кусок кода для применения данного датчика:

// ************************************************************************
// LIS302DL I2C Accelerometer
// ************************************************************************
#if defined(LIS302DL)
#define LIS302  0x1D
#define LIS302_CTRL_XEN 0
#define LIS302_CTRL_YEN 1
#define LIS302_CTRL_ZEN 2
#define LIS302_CTRL_STM 3
#define LIS302_CTRL_STP 4
#define LIS302_CTRL_FS 5
#define LIS302_CTRL_PD 6
#define LIS302_CTRL_DR 7


void ACC_init(){
  i2c_writeReg(LIS302 ,0x20 , _BV(LIS302_CTRL_PD) | _BV(LIS302_CTRL_XEN) | _BV(LIS302_CTRL_YEN) | _BV(LIS302_CTRL_ZEN) );
  acc_1G = 56; // Странно что не 64, но практика показала, что именно 56
}

void ACC_getADC(){
  TWBR = ((F_CPU / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  i2c_getSixRawADC(LIS302,0x28+0x80);

  ACC_ORIENTATION( int8_t(rawADC[1]),
                   int8_t(rawADC[3]),
                   int8_t(rawADC[5]));
  ACC_Common();
}
#endif