Tilt Compensated Compass Using HMC5983 gives inconsistent output

Question

I've been trying to build a tilt compensated compass for an autonomous Arduino car I'm building. I'm having issues with the consistency of my data.

This is the code I'm using.

#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 connection successful") : F("MPU6050 connection failed"));
  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);
        //Serial.println();
        mpu.PrintActiveOffsets();
        mpu.setDMPEnabled(true);
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        //Serial.print(F("DMP Initialization failed (code "));
        //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(){
  //return compass heading
  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;

  //Calibrated manually
  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();

    }
}

When I have the below sensor orientation my data output(corrected_reading) varies as shown in the below graph on one complete rotation.I'm happy with this output as there is only very little inconsistency and the tilt compensation works fine too.

However keeping them in the above orientation within my car is not practically possible in my case. So I tried this orientation.

But no matter how I adjust my code to suit this orientation I always end up getting very inconsistent data like below.

I'm sure that there is some mistake i'm making in the second orientation i used.The biggest problem is that the raw magnetometer output(compass_heading) also becomes inconsistent in this case.

Please give me your suggestions as to what changes I should make in order to get a smooth output with the second orientation. Thanks in advance.

Microcontroller - Arduino Uno Magnetometer - HMC 5983 Accelerometer - MPU 6050

Answer 1

As it is not possible to reproduce the errors and analyse the program in detail, the thing which jumped at me that you have no gliding error correction (or its hidden in a library) for the compass (The MPU6050 has probably one as FIFO is called but you should look it up). The method is easy to explain:

• You take an array of lets say 20 measure points.
• You apply FIFO First value in is the last out
• you do gliding average over the first 15 values -> this is the orientation output
• the last 2 are used for value validation:
  • if you get a value out of a defined data range AND the next is in the range then ditch the before value (error corection)
  • if you get a value out of a defined data range AND the next 4 values are also in the range compared tothe first out of range this will cause the array to empty and start a new gliding average with these values. (Might not be the case when driving a car but in plane models this can happen -> 180 degree turns)

Its up to you how many values you take as a base reference and how many lead to an aprupt direction change. As we are in hobby electronics you can not expect high quality value delivery sowe have to use math to get to a desired result.
Regarding the changed orientation of the magnetometer.
EDIT
In my experience the turning of the magnetometer upside down resultedin weird behavior and more unstable readings. I guess this is due to the elctronics shielding now from the top rather than the bottom.
For reading and error handling I looked [into this library code]((https://github.com/keepworking/Mecha_QMC5883L/blob/master/MechaQMC5883.cpp) and adapted the read routine to my needs. Also lookat the examplesfor valuable input.