Go Down

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

#### Megaionstorm

#15
##### Nov 09, 2010, 10:19 amLast Edit: Nov 09, 2010, 10:19 am by Megaionstorm Reason: 1
@BastiKay

Code: [Select]
`#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);} `
Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

#### BastiKay

#16
##### Nov 09, 2010, 10:52 am
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)))); // }`

#### VIKI68

#17
##### Nov 11, 2010, 11:53 pm
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.

#### Megaionstorm

#18
##### Nov 12, 2010, 12:01 am
I think it's good to have the additional information, for the future !
Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

#### VIKI68

#19
##### Nov 12, 2010, 01:56 am
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

#### Megaionstorm

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

@Viki

How can i get the values for f and q ?
Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

#### VIKI68

#21
##### Nov 12, 2010, 04:44 pm
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.

#### Megaionstorm

#22
##### Nov 12, 2010, 06:10 pm
@Viki

Is it possible to get the f and q values from a Breakout Board - ADXL321 ?
Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

#### VIKI68

#23
##### Nov 12, 2010, 07:22 pm
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.

#### varuna

#24
##### Nov 17, 2010, 01:59 pm
Hey i could interface a sure electronics digital compass..do anyone need code i will provide

#### Megaionstorm

#25
##### Nov 17, 2010, 02:03 pm
Good idea to post it !
Mein Arduino Projekte Blog:
http://ardu-megatank.blogspot.de/

#### newbee

#26
##### Nov 17, 2010, 10:50 pm
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

#### newbee

#27
##### Nov 27, 2010, 05:23 pm
void loop() {

// Accelerometer
ACx = AccelX();
ACy = AccelY();
ACz = AccelZ();
pitch = calcPitch();
roll = calcRoll();

// HMC5843
float fx,fy,fz;
getValues(&ix,&iy,&iz); // Returned HMC values are scaled to common max, not calculated to rotation,
getValues(&fx,&fy,&fz);

Xcomp = getCompX(fx, fy, fz);
Ycomp = getCompY(fx, fy, fz);
}
//-----------------------------------------------------------
float calcPitch(){
return atan2(-ACx, sqrt(ACy * ACy + ACz * ACz));
}
float calcRoll(){
return atan2(ACy,sqrt(ACx * ACx + ACz * ACz));
}
}

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