Go Down

Topic: Selfmade Tilt compensated Compass Pitch, Roll, Yaw (Read 27812 times) previous topic - next topic


Nov 09, 2010, 10:19 am Last Edit: Nov 09, 2010, 10:19 am by Megaionstorm Reason: 1

What do you think about this solution ?

Code: [Select]

#include <hmc.h>

void setup()
 double x, y, z, degree;

void loop()
 delay(1000); // There will be new values every 100ms
 //x = -298;
 //y = 143;
 degree = ((-180) * atan(y/x))/ 3,14159265;
 if ((x > 0) and (y > 0)) degree = degree + 0;   / 1. Quadrant
 if ((x < 0) and (y > 0)) degree = degree + 180; / 2. Quadrant
 if ((x < 0) and (y < 0)) degree = degree + 180; / 3. Quadrant
 if ((x > 0) and (y < 0)) degree = 360 + degree; / 4. Quadrant
 if ((x > 0) and (y == 0)) degree = 0;
 if ((x == 0) and (y > 0)) degree = 90;
 if ((x < 0) and ((y == 0) degree = 180;
 if ((x == 0) and (y < 0)) degree = 270;
 Serial.print(" :<-degree");
 Serial.print(" y:");
 Serial.print(" z:");
Mein Arduino Projekte Blog:


I think you can calculate the heading by using just the function atan() but from what I have read atan2() is easier to use. You do not have to make any exceptions. But maybe I am wrong!
My float values for atan2(fy,fx) from north - east - south are all >=0 and from south - west - north <=0

Code: [Select]
if ((atan2(fy,fx))>=0)
   Serial.println((atan2(fy,fx))*180/M_PI); //
 if ((atan2(fy,fx))<=0)
   Serial.println(360-((atan2(fy,fx))*((-180/M_PI)))); //


This only works if the sensor is somewhat close to horizontal (maybe even depending on where you're at), otherwise you need to do more calculations. There was a link somewhere in the docs or something, can't find it now. If you need it, let us know.

Atan2 is indeed easier and faster, it doesn't check for overflows and stuff.


I think it's good to have the additional information, for the future !
Mein Arduino Projekte Blog:


This is the doc i was referring to:


In the previous listed code the z vector-component wasn't taken in account so in normal writing you'd need:

Xh = X*cos(f) + Y*sin(q)*sin(f) - Z*cos(q)*sin(f)
Yh = Y*cos(q) + Z*sin(q)


Heading for (Xh <0) = 180 - arcTan(Yh/Xh)
for (Xh >0, Yh <0) = - arcTan(Yh/Xh)
for (Xh >0, Yh >0) = 360 - arcTan(Yh/Xh)
for (Xh =0, Yh <0) = 90
for (Xh =0, Yh >0) = 270

This is almost like combining both codes listed :)


Nov 12, 2010, 11:39 am Last Edit: Nov 12, 2010, 11:47 am by Megaionstorm Reason: 1
Thanks !


How can i get the values for f and q ?
Mein Arduino Projekte Blog:


Ok, interesting... those are roll and pitch, but we don't get these values from this sensor.. so that doc/description isn't just usable, but it does show that without z-vector you won't get a good heading. Once my robot is mobile and in one piece again i might dig into this again.



Is it possible to get the f and q values from a Breakout Board - ADXL321 ?
Mein Arduino Projekte Blog:


Yeah that ought to work, although some proper math should do the trick without it (can't be that hard). An accelerometer has one downside, it's always relative.. so you'd need to be certain of a certain state, moving or non-moving.. which is already relative by itself. So if your GPS-module tells you you're not moving you could calibrate the accelerometer, so you know the speed and state relative to the earth.


Hey i could interface a sure electronics digital compass..do anyone need code i will provide


Mein Arduino Projekte Blog:



I want to build that kind of compass,too.

But what I understand from the lowcost paper by Mr. Caruso is that only roll and pitch are necessary for calculating heading, NOT yaw. Which is natural enough, yaw being the degree of freedom that coincides with that of magnetic heading, of course.

So one needs a 2 axis accelerometer and a 3 axis magnetometer.
As accelerometers aren't drifting that much it may be enough to calibrate it once and save and retrieve the offsets in eeprom which are subtracted or added to the reading values.

The GPS speed is no guarantee for a stillstand because below a speed of 2 knots the GPS will display speed = 0. So the best way for calibration of the accel may be to calibrate it once and store the offsets.

However, it may be an interesting idea to correct the calculated heading by the GPS track ( if one is going on a straight line) and to store the correction values. However, it would require to give input to the compass and set it into and out of calibration mode. This is, by the way, how modern autopilots from RAYMARINE are calibrating themselves. They need a GPS connection for that. Once calibrated they steer autonomously by magnetic heading.

Has anyone managed to get the magnetic calibration to work?



void loop() {

 // Accelerometer
 ACx = AccelX();
 ACy = AccelY();
 ACz = AccelZ();
 pitch = calcPitch();
 roll = calcRoll();
 // HMC5843
 int ix,iy,iz, heading;
 float fx,fy,fz;  
 getValues(&ix,&iy,&iz); // Returned HMC values are scaled to common max, not calculated to rotation,
 // get tilt compensated heading
 Xcomp = getCompX(fx, fy, fz);
 Ycomp = getCompY(fx, fy, fz);
 heading = getCompHeading(Xcomp, Ycomp);
float calcPitch(){                                    
 return atan2(-ACx, sqrt(ACy * ACy + ACz * ACz));    
float calcRoll(){                                      
 return atan2(ACy,sqrt(ACx * ACx + ACz * ACz));      
int deg(float rad){    
 return (int)round(rad * 180/M_PI);

// tilt compensation of magnetic vectors

float getCompX(float fx, float fy, float fz){  
 return fx * cos(pitch) + fy * sin(roll) * sin(pitch) - fz * cos(roll) * sin(pitch);
float getCompY(float fx, float fy, float fz){  
 return fy * cos(roll) + fz * sin(pitch);
int getCompHeading(float Xcomp, float Ycomp){  
 return  round(deg(atan2(Ycomp, Xcomp) + M_PI));

Can someone find out why these functions don't work to compensate the heading for tilt?

Thanks for your comments!


Go Up