How to implement a tilt compensated compass?

Question

So i was trying to implement a tilt compensated compass, using HMC5883L and MPU6050. The problem i am facing is, there are 2 equations for tilt compensation

Tilt Compensated X: X,Y,Z are compass output (raw output)

new_X = X*cos(pitch) + Y*sin(roll)*sin(pitch) – Z*cos(roll)*sin(pitch)

Tilt Compensated Y:

new Y = Y*cos(roll) + Z*sin(roll)

Final Heading : Azimuth = atan2(Y, X) * RAD_TO_DEG

The angles which i am getting from the mpu are gyro-drift corrected angles which are coming through a kalman filter. Those angles are fed to these above equations and after atan2 i get the tilt compensated heading. But the issue/observation here is, when i rotate the body(on which the compass and mpu are) along the Z axis (YAW axis) the heading doesn't change(sort of locked on), but changes when i tilt the body in X/Y. I searched for thorough references, but wasn't able to find any!
Here is my code snippet (uses compass part form adafruit library):

double HEADING(double r, double p){
  sensors_event_t event; 
  mag.getEvent(&event);
  r = r * (PI/180);
  p = p * (PI/180);
  double x = (event.magnetic.x)*cos(p) + (event.magnetic.y)*sin(r)*sin(p) - (event.magnetic.z)*cos(r)*sin(p);
  double y = (event.magnetic.y)*cos(r) + (event.magnetic.z)*sin(r);
 
  float heading = atan2(y,x);
 
  float declinationAngle = 0.366667 * (PI/180);// (0°22')positve declination
  heading += declinationAngle;
  
  if(heading < 0)
    heading += 2*PI;
    
  if(heading > 2*PI)
    heading -= 2*PI;
   
  heading = heading * 180/PI; 
  
  return heading;
}

The HMC5883L data acquisition I wrote :

// i just need a direction reference not actual heading, just to keep my drone 
// steady on yaw axis with reference to that direction reference but it shall be Tilt compensated
#include<Wire.h>

#define MAG 0x1E

#define R 0
#define P 1
#define Y 2

float mag_raw[3],h;
float CMP_DEC = 0.36667 * (PI/180);



void setup(){
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);
  
  Wire.beginTransmission(MAG);
  Wire.write(0x02);
  Wire.write(0x00);
  Wire.endTransmission(true);
  
}

void loop(){
  Wire.beginTransmission(MAG);
  Wire.write(0x03);
  Wire.endTransmission(false);
  Wire.requestFrom(MAG,6,true);

  mag_raw[R] = Wire.read() << 8 | Wire.read();
  mag_raw[P] = Wire.read() << 8 | Wire.read();
  mag_raw[Y] = Wire.read() << 8 | Wire.read();

  h = atan2(mag_raw[P],mag_raw[R]);

//  h += CMP_DEC;

  if(h < 0) h+= (2*PI);
  if(h > (2*PI)) h -= (2*PI);
  h *= (180/PI);
  
//  Serial.print("X : ");
//  Serial.print(mag_raw[R]);
//  Serial.print(" Y : ");
//  Serial.print(mag_raw[P]);
//  Serial.print(" Z : ");
//  Serial.print(mag_raw[Y]);

  Serial.print(" H : ");
  Serial.println(h);
  
}

Some guidance will be very helpful!

Answer 1

Inspecting your code, it does not appear you are compensating the raw magnetometer values for soft and hard iron effects. These terms roughly refer to adjusting the range and magnitude of each of the individual magnetometers (X, Y and Z). Some magnetometer manufactures allow writing these compensation values into the chip. Even then the developer and user will still need to perform some form of calibration as the magnetometer can not predict under what conditions the magnetometer will be used. Here is another explanation using matrices of hard and soft iron compensation.

Once the compass produces expected results on a flat surface parallel to the surface of the earth (in a place on earth where the magnetic lines roughly align with true north). Consider adding tilt compensation. Accelerometers usually do not need to be compensated as extensively as magnetometers.

This can be done using trigonometric math and Euler angles. However, as with physical gambles, this can lead to a situation similar to gimbal lock. Where the trigonometric math goes to infinity.

To avoid a gimbal lock like situation, most tilt compensated compasses use quaternions. Quaternions use imaginary numbers and matrix math to describe 3 dimensional rotation.