I don't use that concept at all, and I think the logic and methodology for it is spurious.
The basic feature of the geomagnetic field is that it isn't parallel to the ground, as you would
think when you have a magnetic compass sitting on a map on a table.It is a vector field shooting down into the ground at a diagonal angle, which is more or less
constant over short distances and which you can easily find out what it is for your region.The 3-axis magnetometer is going to give you a vector in 3D which is going to show you the
direction of that field. Since you already know the direction of the field, you can determine
the orientation of the magnetometer, with the exception of rotation about the vector direction.If you also have an accelerometer, and you assume your device is stationary or moving at a constant
velocity ( so it has no actual acceleration ), you can fully determine the orientation of your device.
So, how i can solve tilt compensation for yaw Axis? By the way, this is my complete code:
// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>
// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;
int accelResult[3];
float timeStep = 0.02; //20ms. Need a time step value for integration of gyro angle from angle/sec
float biasAccelX, biasAccelY, biasAccelZ;
float pitchAccel = 0;
float rollAccel = 0;
float Xh;
float Yh;
float realHeading;
float pitchAccelXh;
float rollAccelYh;
float XM;
float YM;
float ZM;
unsigned long timer;
//Penjabaran fungsi writeTo sebagai Fungsi untuk writing byte ke alamat device pada I2C
void writeTo(byte device, byte toAddress, byte val) {
Wire.beginTransmission(device);
Wire.write(toAddress);
Wire.write(val);
Wire.endTransmission();
}
//Penjabaran fungsi readFrom sebagai Fungsi untuk membaca num bytes dari alamat pada device I2C
void readFrom(byte device, byte fromAddress, int num, byte result[]) {
Wire.beginTransmission(device);
Wire.write(fromAddress);
Wire.endTransmission();
Wire.requestFrom((int)device, num);
int i = 0;
while(Wire.available()) {
result[i] = Wire.read();
i++;
}
}
//Fungsi untuk mmebaca nilai Accelerometer
void getAccelerometerReadings(int accelResult[]) {
byte buffer[6];
readFrom(0x40,0x02,6,buffer);
accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0]; //Yes, byte order different from gyros'
accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}
// Out setup routine, here we will configure the microcontroller and compass.
void setup()
{
int totalAccelXValues = 0;
int totalAccelYValues = 0;
int totalAccelZValues = 0;
int i;
// Initialize the serial port.
Serial.begin(115200);
Serial.println("Starting the I2C interface.");
Wire.begin(); // Start the I2C interface.
Serial.println("Constructing new HMC5883L");
compass = HMC5883L(); // Construct a new HMC5883 compass.
Serial.println("Setting scale to +/- 1.3 Ga");
error = compass.SetScale(1.3); // Set the scale of the compass.
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
Serial.println("Setting measurement mode to continous.");
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
writeTo(0x40,0x10,0xB6); //Soft_reset accelerometer BMA180
writeTo(0x40,0x0D,0x10); //set fungsi ee_w
// Determine zero bias for all axes of both sensors by averaging 50 measurements
delay(100); //wait for gyro to "spin" up
for (i = 0; i < 50; i += 1) {
getAccelerometerReadings(accelResult);
totalAccelXValues += accelResult[0];
totalAccelYValues += accelResult[1];
totalAccelZValues += accelResult[2];
delay(50);
}
biasAccelX = totalAccelXValues / 50;
biasAccelY = totalAccelYValues / 50;
biasAccelZ = (totalAccelZValues / 50) - 256; //Don't compensate gravity away! We would all (float)!
}
// Our main program loop.
void loop()
{
// Retrive the raw values from the compass (not scaled).
MagnetometerRaw raw = compass.ReadRawAxis();
// Retrived the scaled values from the compass (scaled to the configured scale).
MagnetometerScaled scaled = compass.ReadScaledAxis();
// Values are accessed like so:
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(scaled.YAxis, scaled.XAxis);
// Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
// Find yours here: http://www.magnetic-declination.com/
// Mine is: 2� 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
// If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
float declinationAngle = 0.55;
heading += declinationAngle;
// Correct for when signs are reversed.
if(heading < 0)
heading += 2*PI;
// Check for wrap due to addition of declination.
if(heading > 2*PI)
heading -= 2*PI;
// Convert radians to degrees for readability.
float headingDegrees = heading * 180/M_PI;
// Output the data via the serial port.
//Output(raw, scaled, heading, headingDegrees);
// Normally we would delay the application by 66ms to allow the loop
// to run at 15Hz (default bandwidth for the HMC5883L).
// However since we have a long serial out (104ms at 9600) we will let
// it run at its natural speed.
// delay(66);
timer = millis(); //get a start value to determine the time the loop takes
getAccelerometerReadings(accelResult);
pitchAccel = atan2((accelResult[1] - biasAccelY) / 1024, (accelResult[2] - biasAccelZ) / 1024) * 360.0 / (2*PI);
rollAccel = atan2((accelResult[0] - biasAccelX) / 1024, (accelResult[2] - biasAccelZ) / 1024) * 360.0 / (2*PI);
//---------------Tilt Compensated Begin----------------------------------------
pitchAccelXh = atan2((accelResult[1] - biasAccelY) / 1024, (accelResult[2] - biasAccelZ) / 1024);
rollAccelYh = atan2((accelResult[0] - biasAccelX) / 1024, (accelResult[2] - biasAccelZ) / 1024);
//pitchAccelXh = constrain(pitchAccelXh, -1.57, 1.57);
//pitchAccelXh = map(pitchAccelXh, -3.14, 3.14, 0, 6.28);
//rollAccelYh = constrain(rollAccelYh, -1.57, 1.57);
//rollAccelYh = map(rollAccelYh, -3.14, 3.14, 0, 6.28);
float cos_roll= cos(rollAccelYh);
float sin_roll = sin(rollAccelYh);
float cos_pitch = cos(pitchAccelXh);
float sin_pitch = sin(pitchAccelXh);
XM = scaled.XAxis;
YM = scaled.YAxis;
ZM = scaled.ZAxis;
// The tilt compensation algorithem.
Yh = YM * cos_roll - ZM * sin_roll;
Xh = XM * cos_pitch + YM * sin_roll * sin_pitch + ZM * cos_roll * sin_pitch;
realHeading = atan2(-Yh, Xh) * 360.0 / (2*PI);
//--------------------Tilt Compensated End--------------------------------------
Serial.print(pitchAccel);
Serial.print(" pitch \t");
Serial.print(rollAccel);
Serial.print(" roll \t");
//Serial.print(pitchAccelXh);
//Serial.print(" pitchXH ");
//Serial.print(rollAccelYh);
//Serial.print(" rollYH");
//Serial.print(raw.XAxis);
//Serial.print("RawX:\t");
//Serial.print(" ");
//Serial.print(raw.YAxis);
//Serial.print(" ");
//Serial.print(raw.ZAxis);
//Serial.print(" \tScaled:\t");
//Serial.print(scaled.XAxis);
//Serial.print(" X ");
//Serial.print(scaled.YAxis);
//Serial.print(" Y ");
//Serial.print(scaled.ZAxis);
//Serial.print(" Z ");
Serial.print(realHeading);
Serial.print(" DegreeReal \t");
Serial.print(headingDegrees);
Serial.println(" Degrees \t");
delay(200);
//timer = millis() - timer; //how long did the loop take?
//timer = (timeStep * 1000) - timer; //how much time to add to the loop to make it last time step msec
//delay(timer); //make one loop last time step msec
}