Pages: 1 [2]   Go Down
Author Topic: Selfmade Tilt compensated Compass Pitch, Roll, Yaw  (Read 9113 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Edison Member
*
Karma: 20
Posts: 1980
Wo ein Wille ist, da ist auch ein Weg !
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

@BastiKay

What do you think about this solution ?

Code:
#include <hmc.h>

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

void loop()
{
  delay(1000); // There will be new values every 100ms
  HMC.getValues(&x,&y,&z);
  //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(" :<-degree");
  Serial.print("x:");
  Serial.print(x);
  Serial.print(" y:");
  Serial.print(y);
  Serial.print(" z:");
  Serial.println(z);
}
« Last Edit: November 09, 2010, 04:19:45 am by Megaionstorm » Logged

Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

0
Offline Offline
Newbie
*
Karma: 0
Posts: 3
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
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)))); //
}
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 38
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Edison Member
*
Karma: 20
Posts: 1980
Wo ein Wille ist, da ist auch ein Weg !
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I think it's good to have the additional information, for the future !
Logged

Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

0
Offline Offline
Newbie
*
Karma: 0
Posts: 38
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

This is the doc i was referring to:

http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf

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)

and

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 smiley
Logged

0
Offline Offline
Edison Member
*
Karma: 20
Posts: 1980
Wo ein Wille ist, da ist auch ein Weg !
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks !

@Viki

How can i get the values for f and q ?
« Last Edit: November 12, 2010, 05:47:21 am by Megaionstorm » Logged

Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

0
Offline Offline
Newbie
*
Karma: 0
Posts: 38
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Edison Member
*
Karma: 20
Posts: 1980
Wo ein Wille ist, da ist auch ein Weg !
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

@Viki

Is it possible to get the f and q values from a Breakout Board - ADXL321 ?
Logged

Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

0
Offline Offline
Newbie
*
Karma: 0
Posts: 38
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 1
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

0
Offline Offline
Edison Member
*
Karma: 20
Posts: 1980
Wo ein Wille ist, da ist auch ein Weg !
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Good idea to post it !
Logged

Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

0
Offline Offline
Newbie
*
Karma: 0
Posts: 49
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi,

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?

Andreas
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 49
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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,
  getValues(&fx,&fy,&fz);
  
  // 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!

Andreas
Logged

Pages: 1 [2]   Go Up
Jump to: