# How to implement a tilt compensated compass?

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 declinationAngle = 0.366667 * (PI/180);// (0°22')positve declination

}
``````

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,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);

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!

• Are you writing all the code? (Not using libraries?) I ask as you did not mention how you were compensating the magnetometer's output for hard and soft iron errors. Creating a tilt compass is hard enough. But I find it better, if writing most or all of the code from scratch, to create a normal (use it flat on a table) compass 1st. Sep 5, 2020 at 18:52
• @st2000 I tried writing my own code (the general approach for i2C Wire lib). Sometimes i got a solid output (no change in data even if i move in different axis), sometimes data came but the range remained almost around 170 for X , 65000 for Y and 56 for Z axes. I didn't understand what was going on so i started using libraries :( can you give me some relevant references ? Sep 6, 2020 at 4:29
• Are the pitch and roll angles small when the body sits flat on a table? Do the raw values of the magnetic field components change when you rotate it? Sep 6, 2020 at 8:38
• @EdgarBonet i'll answer according to what i understood; So the R&P angles coming from the MPU are 0±0.1° when kept on the flat surface. Now when I rotate the compass along Z axis, there is no change in the values(actually it is there but it changed about 3 points when i rotated it 90°!). For X,Y axes, the values change a lot !almost ±120 units. ill just add my from scratch written code in the question. Have a look to it, maybe something wrong can be seen... Sep 6, 2020 at 11:57
• @st2000 yeah let me work more on magnetometer thoroughly. Can you send me some references for it ? Possibly not using the libraries (I find it easy without libraries 😅). Sep 6, 2020 at 14:21