LSM303 не может компенсировать наклон

Кажется, у меня все получилось! Большое спасибо за вашу помощь, я должен вам несколько пива. Для всех, кто следит и интересуется, вот мой код:

   LSM303DLHC including accelerometer to correct for position
  Vector calculation to be implemented
*/
#include <Wire.h>
#include <Math.h>




const int LSM303_ADDR = 0x19;
const int LSM303_ADDR_MAG = 0x1E;

// адреса регистров управления
const int CTRL_REG1_ADDR = 0x20;
const int CTRL_REG2_ADDR = 0x21;
const int CTRL_REG3_ADDR = 0x22;
const int CTRL_REG4_ADDR = 0x23;
const int CTRL_REG5_ADDR = 0x24;
const int CTRL_REG6_ADDR = 0x25;

const int CRA_REG_M_ADDR = 0x00;
const int CRB_REG_M_ADDR = 0x01;
const int MR_REG_M_ADDR = 0x02;

//регистр данных адресует сначала младший бит для акселератора, первый старший бит для магнето
const int Accelero_First_data_addr = 0x28;
const int Magneto_First_data_addr = 0x03;
const int Temp_out_data_addr = 0x31;// первые 12 бит MSB


int CTRL_REG1_A_value = 0x47; // 50Hz Low Power отключен, все оси включены
int CTRL_REG2_A_value = 0x00; //
int CTRL_REG3_A_value = 0x00; //нет прерываний или водяных знаков
int CTRL_REG4_A_value = 0x00; // Полная шкала 2G, высокое разрешение, режим SPI не выбран
int CTRL_REG5_A_value = 0x00; // ничего не включено
int CTRL_REG6_A_value = 0x00; // ничего не включено

int CRA_REG_M_value = 0x14; // Датчик температуры включен, скорость передачи данных 15 Гц
int CRB_REG_M_value = 0x20; // 2G — это данные 1100–980 G/LSB

int MR_REG_M_value = 0x00;

int16_t MagRaw_X_axis_value, MagRaw_Y_axis_value, MagRaw_Z_axis_value;
int16_t AccRaw_X_axis_value, AccRaw_Y_axis_value, AccRaw_Z_axis_value;

float pitch, roll;
float Xm_calibrated, Ym_calibrated, Zm_calibrated;
float Xm_norm, Ym_norm, Zm_norm, Xa_norm, Ya_norm, Za_norm;
float norm_a, norm_m;

void setup() {// поместите сюда код установки для однократного запуска:
  Serial.begin(9600);
  Wire.begin();
  setupLSM303();
  Serial.println("Setup complete..");
}

void loop() {// поместите сюда свой основной код для повторного запуска:
  MagnetoDataRead();
  AcceleroDataRead();

}



void setupLSM303() {
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG1_ADDR);
  Wire.write(CTRL_REG1_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG2_ADDR);
  Wire.write(CTRL_REG2_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG3_ADDR);
  Wire.write(CTRL_REG3_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG4_ADDR);
  Wire.write(CTRL_REG4_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG5_ADDR);
  Wire.write(CTRL_REG5_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG6_ADDR);
  Wire.write(CTRL_REG6_A_value);
  Wire.endTransmission();
  //настройка магниторегистра
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(CRA_REG_M_ADDR);
  Wire.write(CRA_REG_M_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(CRB_REG_M_ADDR);
  Wire.write(CRB_REG_M_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(MR_REG_M_ADDR);
  Wire.write(MR_REG_M_value);
  Wire.endTransmission();
}


void MagnetoDataRead() {
  Wire.beginTransmission(LSM303_ADDR_MAG );
  Wire.write(Magneto_First_data_addr);
  Wire.endTransmission();

  Wire.requestFrom(LSM303_ADDR_MAG , 6);
  if (Wire.available() <= 6) {
    MagRaw_X_axis_value = (Wire.read() << 8) | (Wire.read());
    MagRaw_Z_axis_value = (Wire.read() << 8) | (Wire.read());
    MagRaw_Y_axis_value = (Wire.read() << 8) | (Wire.read());

    float X_mag_cal = MagRaw_X_axis_value / 1100.0;
    float Y_mag_cal = MagRaw_Y_axis_value / 1100.0;
    float Z_mag_cal = MagRaw_Z_axis_value / 980.0;

    //вводим коэффициенты калибровки
    float Xm_offset_corrected = X_mag_cal - 0.032874;
    float Ym_offset_corrected = Y_mag_cal + 0.047876;
    float Zm_offset_corrected = Z_mag_cal + 0.039649;

    Xm_calibrated = 92.387197 * Xm_offset_corrected - 0.307374 * Ym_offset_corrected + 1.151625 * Zm_offset_corrected;
    Ym_calibrated = -0.307374 * Xm_offset_corrected + 85.002110 * Ym_offset_corrected + 1.169755 * Zm_offset_corrected;
    Zm_calibrated = 1.151625 * Xm_offset_corrected + 1.169755 * Ym_offset_corrected + 80.276037 * Zm_offset_corrected;

    //формула нормализации
    norm_m = sqrt(sq(Xm_calibrated) + sq(Ym_calibrated) + sq(Zm_calibrated));
    Xm_norm = Xm_calibrated / norm_m;
    Ym_norm = Ym_calibrated / norm_m;
    Zm_norm = Zm_calibrated / norm_m;

    /*
        Serial.print(X_mag_cal, 10);
        Serial.print("\t");
        Serial.print(Y_mag_cal, 10);
        Serial.print("\t");
        Serial.print(Z_mag_cal, 10);
        Serial.println("\t");
    */

    //заголовок с плавающей запятой = ((atan2(Ym_norm, Xm_norm)) * 180.0) / PI;
    //если (Заголовок < 0) {
    // Заголовок = 360 + Заголовок;
    //}
    //Serial.println(Заголовок);
    // задержка (50);

  }
}

void AcceleroDataRead() {
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(Accelero_First_data_addr | 0x80);
  Wire.endTransmission();

  Wire.requestFrom(LSM303_ADDR, 6);
  if (Wire.available() <= 6) {
    AccRaw_X_axis_value = (Wire.read()) | (Wire.read() << 8);
    AccRaw_Y_axis_value = (Wire.read()) | (Wire.read() << 8);
    AccRaw_Z_axis_value = (Wire.read()) | (Wire.read() << 8);

    float X_Acc =  (AccRaw_X_axis_value >> 4);
    float Y_Acc =  (AccRaw_Y_axis_value >> 4);
    float Z_Acc =  (AccRaw_Z_axis_value >> 4);

    //калибровать смещение
    float X_acc_offset_corrected = X_Acc + 6.676443;
    float Y_acc_offset_corrected = Y_Acc - 6.622381;
    float Z_acc_offset_corrected = Z_Acc + 55.408804;

    //калибровать
    float X_acc_calibrated = 0.977273 * X_acc_offset_corrected - 0.003458 * Y_acc_offset_corrected + 0.012639 * Z_acc_offset_corrected;
    float Y_acc_calibrated = -0.003458 * X_acc_offset_corrected + 0.979674 * Y_acc_offset_corrected - 0.001130 * Z_acc_offset_corrected;
    float Z_acc_calibrated = 0.012639 * X_acc_offset_corrected - 0.001130 * Y_acc_offset_corrected + 0.897083 * Z_acc_offset_corrected;
    // нормализовать
    norm_a = sqrt(sq(X_acc_calibrated) + sq(Y_acc_calibrated) + sq(Z_acc_calibrated));
    Xa_norm = X_acc_calibrated / norm_a;
    Ya_norm = Y_acc_calibrated / norm_a;
    Za_norm = Z_acc_calibrated / norm_a;

    // вычисляем тангаж и крен
    /*
      pitch = asin(-Xa_norm) * 180 / PI;
      roll = asin(Ya_norm / cos(pitch * PI / 180)) * 180 / PI;
    */
    roll = atan2(Ya_norm, Za_norm);
    float  Gz2 = Ya_norm * sin(roll) + Za_norm * cos(roll);
    pitch = atan(-Xa_norm / Gz2);
    float By2 = Zm_norm * sin(roll) - Ym_norm * cos(roll);
    float Bz2 = Ym_norm * sin(roll) + Zm_norm * cos(roll);
    float Bx3 = Xm_norm * cos(pitch) + Bz2 * sin(pitch);
    float yaw = (atan2(By2, Bx3)*180)/PI;
    if (yaw < 0) {
      yaw = 360 + yaw;
    }

    float tilt = atan2(sqrt((sq(Xa_norm)+(sq(Ya_norm)))),Za_norm);

    //рассчитываем векторы для компенсации наклона
    float Xh = Xm_norm * cos(pitch) + Zm_norm * sin(pitch);
    float Yh = Xm_norm * sin(roll ) * sin(pitch) + Ym_norm * cos(roll) - Zm_norm * sin(roll) * cos(pitch);

        // расчет курса с компенсацией наклона
        float Heading = ((atan2(Yh, Xh)) * 180.0 ) / PI;
        if (Heading < 0) {
          Heading = 360 + Heading;
        }


    Serial.print("Pitch \t");
    Serial.print(pitch * 180 / PI);
    Serial.print("\t roll \t");
    Serial.print(roll * 180 / PI);
    Serial.print("\t tilt \t");
    Serial.print(tilt*180/PI);
    Serial.print("\t");
    Serial.println(Heading);
    delay(500);
  }
}