Компас с компенсацией наклона Использование HMC5983 дает непоследовательный вывод

Я пытался создать компас с компенсацией наклона для автономного автомобиля Arduino, который собираю. У меня проблемы с непротиворечивостью моих данных.

Это код, который я использую.

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps_V6_12.h>
#define Magnetometer_addr 0x1E
MPU6050 mpu;

bool dmpReady = false;  
uint8_t mpuIntStatus;   
uint8_t devStatus;      
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64]; 
float prevoius_corrected_reading,corrected_reading;

Quaternion q;           
VectorFloat gravity;   
float ypr[3];         

bool mpuInterrupt = false;  

float Xerror = 0.01;
float Yerror = -0.385;
float Zerror = -0.17;

int X0, X1, Z0, Z1,Y0, Y1;
float X, Y, Z;
float compass_heading;



void setup() {
  Serial.begin(9600);
  Wire.begin();

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x01);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x02);
  Wire.write(0x00);
  Wire.endTransmission();

  mpu.initialize();  
  //Serial.println(mpu.testConnection() ? F("Успешное подключение к MPU6050") : F("Ошибка подключения к MPU6050"));
  devStatus = mpu.dmpInitialize();

    mpu.setXGyroOffset(51);
    mpu.setYGyroOffset(8);
    mpu.setZGyroOffset(21);
    mpu.setXAccelOffset(1150); 
    mpu.setYAccelOffset(-50); 
    mpu.setZAccelOffset(1060); 

    if (devStatus == 0) {

        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
        //Серийный.println();
        mpu.PrintActiveOffsets();
        mpu.setDMPEnabled(true);
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        //Serial.print(F("Ошибка инициализации DMP (код "));
        //Serial.print(devStatus);
        //Serial.println(F(")"));
    }

  delay(20);

}

void loop() {

  compass_heading = request_compass_data();

  Serial.print("Raw compass heading is \t");
  Serial.print(compass_heading);

  Serial.print("\t\t");

  request_accel_data();

  Serial.print("YAW\t:");
  Serial.print(ypr[0] * 180/M_PI);
  Serial.print("\tROLL\t:");
  Serial.print(ypr[1] * 180/M_PI);
  Serial.print("\tPITCH\t:");
  Serial.print(ypr[2] * 180/M_PI);

  prevoius_corrected_reading = corrected_reading;

  Serial.print("\tCorrected compass heading is \t");
  float Xhor =  X*cos(ypr[2]) + Z*sin(ypr[2]);
  float Yhor =  Y*cos(ypr[1]) - Z*sin(ypr[1])*cos(ypr[2]) + X*sin(ypr[1])*sin(ypr[2]);

  corrected_reading = rad_to_deg(rad_ang(Yhor, Xhor));
  Serial.print(corrected_reading);

  Serial.print("\tFiltered compass heading is \t");

  Serial.print(0.1 * corrected_reading + 0.9 * prevoius_corrected_reading);
  Serial.println();



}

float rad_ang(float a, float b){
  float ang = atan2(a, b);
  if(ang<0){ang += 2*PI;}
  if(ang>2*PI){ang -= 2*PI;}
  return ang;
}

float rad_to_deg(float rad_in){
  return rad_in / PI * 180;
}

float request_compass_data(){
  //возвращаем направление по компасу
  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x03);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  X0 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x04);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  X1 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x05);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Z0 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x06);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Z1 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x07);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Y0 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x08);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Y1 = Wire.read();
  }

  X0 = X0<<8;
  Z0 = Z0<<8;
  Y0 = Y0<<8;

  X = X1 + X0;
  X -= Xerror;
  Y = Y1 + Y0;
  Y -= Yerror;
  Z = Z1 + Z0;
  Z -= Zerror;

  //Калибровка вручную
  X = X * 0.00092 - Xerror;
  Y = Y * 0.00092 - Yerror;
  Z = Z * 0.00092 - Zerror;
  /*
  Serial.print("XYZ\t");
  Serial.print(X);
  Serial.print("\t");
  Serial.print(Y);
  Serial.print("\t");
  Serial.println(Z);
*/
  float dir = rad_to_deg(rad_ang(Y, X));

  return dir;

}

void request_accel_data(){
        if (!dmpReady) return;
    mpuInterrupt = true;

    while (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount < packetSize) {
          fifoCount = mpu.getFIFOCount();
        }  

    }

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    //Serial.print("INT STATUS:\t");
    //Serial.print(mpuIntStatus);
    fifoCount = mpu.getFIFOCount();
    //Serial.print("\tFIFO COUNT:\t");
    //Serial.print(fifoCount);

    if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {

        mpu.resetFIFO();
        fifoCount = mpu.getFIFOCount();
        Serial.println(F("FIFO overflow!"));

    } 

    else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  
        mpu.resetFIFO();

    }
}

Когда у меня есть следующая ориентация датчика, мои выходные данные (corrected_reading) варьируются, как показано на приведенном ниже графике, за один полный оборот. Я доволен этим выходом, поскольку существует очень мало несоответствий, и компенсация наклона тоже работает нормально.

Однако в моем случае сохранить их в указанном выше положении в машине практически невозможно. Поэтому я попробовал эту ориентацию.

Но как бы я ни корректировал свой код, чтобы он соответствовал этой ориентации, я всегда получаю очень противоречивые данные, как показано ниже.

Я уверен, что во второй ориентации, которую я использовал, есть какая-то ошибка. Самая большая проблема заключается в том, что необработанные выходные данные магнитометра (compass_heading) также становятся несогласованными в этом случае.

Пожалуйста, дайте мне ваши предложения относительно того, какие изменения я должен внести, чтобы получить плавный вывод со второй ориентацией. Заранее спасибо.

Микроконтроллер - Arduino Uno Магнитометр - HMC 5983 Акселерометр - MPU 6050