Go Down

Topic: LSM303DLHC - Calibration, Pitch, Roll and Tilt Compensated Heading (Read 17176 times) previous topic - next topic

Scirus

In case anyone needs help calibrating the LSM303DLHC board (triple-axis accelerometer+magnetometer) and is looking for a tilt compensated heading, this could help them.
I'm relative new to Arduino and don't have prior programming experience, so probably the code isn't the fastest nor the most elegant, but it seems to work fine.
Any comments and improvements are welcome.

Calibration Software
Download the latest Magneto version from here: https://sites.google.com/site/sailboatinstruments1/home
Unzip executable.

LSM303 Libraries
Download LSM303 Libraries from here: https://github.com/pololu/lsm303-arduino/archive/master.zip
Unzip in the Arduino libraries folder.

Magnetometer Calibration
Copy and upload following code:

Code: [Select]

#include <Wire.h>
#include <LSM303.h>
LSM303 compass;

void setup()
{
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
Serial.println("Magnetometer Uncalibrated (Units in Nanotesla)");
}

void loop()
{
compass.read();
float Xm_print, Ym_print, Zm_print;

Xm_print = compass.m.x*(100000.0/1100.0); // Gain X [LSB/Gauss] for selected sensor input field range (1.3 in these case)
Ym_print = compass.m.y*(100000.0/1100.0) // Gain Y [LSB/Gauss] for selected sensor input field range
Zm_print = compass.m.z*(100000.0/980.0 );  // Gain Z [LSB/Gauss] for selected sensor input field range

Serial.print(Xm_print, 10); Serial.print(" "); Serial.print(Ym_print, 10); Serial.print(" "); Serial.println(Zm_print, 10);
delay(125);
}


Download LSM303DLHC datasheet: http://www.st.com/web/en/resource/technical/document/datasheet/DM00027543.pdf
In the LSM303.cpp file (https://github.com/pololu/lsm303-arduino/blob/master/LSM303/LSM303.cpp), the default value for the CRB_REG_M register is 0x20.
If you convert that hexadecimal value to binary, you get 00100000.
Checking table N°75 on page number 38 in the datasheet, you'll notice the default sensor input field range is +/- 1.3 gauss.
Adjust LSB/Gauss if necessary in the sketch.

Open serial monitor and move board for several minutes in all directions.
Copy values in a textfile (txt) and save file.

Execute Magneto.
Load textfile with raw readings.

For the norm of magnetic field, visit this page: http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
Enter your location and calculate magnetic field.
Copy the value to Magneto (units are the same).



Click calibrate.

Copy and replace the combined bias values and combined scale factors in this sketchbook (check signs; by default, bias is substracted from raw values):

Code: [Select]

#include <Wire.h>
#include <LSM303.h>
LSM303 compass;

void setup()
{
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
Serial.println("Magnetometer Calibrated (Units in Nanotesla)");
}

void loop()
{
compass.read();
float Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal;

Xm_off = compass.m.x*(100000.0/1100.0) - 8397.862881; //X-axis combined bias (Non calibrated data - bias)
Ym_off = compass.m.y*(100000.0/1100.0) - 3307.507492; //Y-axis combined bias (Default: substracting bias)
Zm_off = compass.m.z*(100000.0/980.0 ) + 2718.831179; //Z-axis combined bias

Xm_cal =  0.949393*Xm_off + 0.006185*Ym_off + 0.015063*Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
Ym_cal =  0.006185*Xm_off + 0.950124*Ym_off + 0.003084*Zm_off; //Y-axis correction for combined scale factors
Zm_cal =  0.015063*Xm_off + 0.003084*Ym_off + 0.880435*Zm_off; //Z-axis correction for combined scale factors

Serial.print(Xm_cal, 10); Serial.print(" "); Serial.print(Ym_cal, 10); Serial.print(" "); Serial.println(Zm_cal, 10);
delay(125);
}


Upload code, open serial monitor and move board in all directions.
Copy values in a textfile (txt) and save file.
Load in Magneto textfile with calibrated values.
Click calibrate.

Combined bias should now be closer to 0.



Accelerometer Calibration
Copy and upload following code:

Code: [Select]

#include <Wire.h>
#include <LSM303.h>
LSM303 compass;

void setup()
{
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
Serial.println("Accelerometer Uncalibrated (Units in mGal)");
}

void loop()
{
compass.read();
float Xa_print, Ya_print, Za_print;

Xa_print = compass.a.x/16.0; //Acceleration data registers contain a left-aligned 12-bit number, so values should be shifted right by 4 bits (divided by 16)
Ya_print = compass.a.y/16.0;
Za_print = compass.a.z/16.0;

Serial.print(Xa_print); Serial.print(" "); Serial.print(Ya_print); Serial.print(" "); Serial.println(Za_print);
delay(125);
}


Open serial monitor and move board for several minutes along all axes.
Copy values in a textfile (txt) and save file.

Execute Magneto.
Load textfile with raw readings.

For the norm of gravitational field, enter 1000 (since raw units are in milli-Galileo).

In the LSM303.cpp file, the default value for the CTRL_REG4_A register is 0x08.
If you convert that hexadecimal value to binary, you get 00001000.
Checking table N°27 in the datasheet, you'll notice the accelerometer has a scale selection of +/- 2G.
And checking table N°3 for that scale selection, the linear acceleration sensitivity is 1 mg/LSB.

Click calibrate.

Copy the combined bias values and combined scale factors in this sketchbook:

Code: [Select]

#include <Wire.h>
#include <LSM303.h>
LSM303 compass;

void setup()
{
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
Serial.println("Accelerometer Calibrated (Units in mGal)");
}

void loop()
{
compass.read();
float Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal;

Xa_off = compass.a.x/16.0 + 6.008747; //X-axis combined bias (Non calibrated data - bias)
Ya_off = compass.a.y/16.0 - 18.648762; //Y-axis combined bias (Default: substracting bias)
Za_off = compass.a.z/16.0 + 10.808316; //Z-axis combined bias

Xa_cal =  0.980977*Xa_off + 0.001993*Ya_off - 0.004377*Za_off; //X-axis correction for combined scale factors (Default: positive factors)
Ya_cal =  0.001993*Xa_off + 0.998259*Ya_off - 0.000417*Za_off; //Y-axis correction for combined scale factors
Za_cal = -0.004377*Xa_off - 0.000417*Ya_off + 0.942771*Za_off; //Z-axis correction for combined scale factors

Serial.print(Xa_cal); Serial.print(" "); Serial.print(Ya_cal); Serial.print(" "); Serial.println(Za_cal);
delay(125);
}


Upload code, repeat procedure and check if bias decreased.

Pitch, Roll and Heading
Use following code for printing pitch, roll and tilt compensated heading (code includes low-pass filter, adjustable with the alpha value).
Replace calibration values.

Code: [Select]

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

const float alpha = 0.15;
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;

void setup() {
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
}

void loop()
{
compass.read();
float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;

// Accelerometer calibration
Xa_off = compass.a.x/16.0 + 6.008747;
Ya_off = compass.a.y/16.0 - 18.648762;
Za_off = compass.a.z/16.0 + 10.808316;
Xa_cal =  0.980977*Xa_off + 0.001993*Ya_off - 0.004377*Za_off;
Ya_cal =  0.001993*Xa_off + 0.998259*Ya_off - 0.000417*Za_off;
Za_cal = -0.004377*Xa_off - 0.000417*Ya_off + 0.942771*Za_off;

// Magnetometer calibration
Xm_off = compass.m.x*(100000.0/1100.0) - 8397.862881;
Ym_off = compass.m.y*(100000.0/1100.0) - 3307.507492;
Zm_off = compass.m.z*(100000.0/980.0 ) + 2718.831179;
Xm_cal =  0.949393*Xm_off + 0.006185*Ym_off + 0.015063*Zm_off;
Ym_cal =  0.006185*Xm_off + 0.950124*Ym_off + 0.003084*Zm_off;
Zm_cal =  0.015063*Xm_off + 0.003084*Ym_off + 0.880435*Zm_off;

// Low-Pass filter accelerometer
fXa = Xa_cal * alpha + (fXa * (1.0 - alpha));
fYa = Ya_cal * alpha + (fYa * (1.0 - alpha));
fZa = Za_cal * alpha + (fZa * (1.0 - alpha));

// Low-Pass filter magnetometer
fXm = Xm_cal * alpha + (fXm * (1.0 - alpha));
fYm = Ym_cal * alpha + (fYm * (1.0 - alpha));
fZm = Zm_cal * alpha + (fZm * (1.0 - alpha));

// Pitch and roll
roll  = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
roll_print = roll*180.0/M_PI;
pitch_print = pitch*180.0/M_PI;

// Tilt compensated magnetic sensor measurements
fXm_comp = fXm*cos(pitch)+fZm*sin(pitch);
fYm_comp = fXm*sin(roll)*sin(pitch)+fYm*cos(roll)-fZm*sin(roll)*cos(pitch);

// Arctangent of y/x
Heading = (atan2(fYm_comp,fXm_comp)*180.0)/M_PI;
if (Heading < 0)
Heading += 360;

Serial.print("Pitch (X): "); Serial.print(pitch_print); Serial.print("  ");
Serial.print("Roll (Y): "); Serial.print(roll_print); Serial.print("  ");
Serial.print("Heading: "); Serial.println(Heading);
delay(250);
}

moeburn

I tried running the magneto calibration, but I am getting massive bias values, way larger than yours. 12721 22126 and 51590 respectively.  And my correction factors are nowhere near 0.9 like yours are, mine are more like 0.17 and 0.18 (the rest are closer to zero though).  Is this normal?

jremington

Quote
Is this normal?
Those numbers depend on what value you use for the "norm" of the magnetic field, and the sensitivity of the magnetometer.

You can use an arbitrary number for the norm, such as the average value of all the measurements. Then the correction factors will be reasonably sized numbers.

moeburn

Oh yes I looked up the magnetic field norm for my city, it's about 53,921nT.  I think the problem is the conversion factor, this code is written for a sensor input field range of 1.3 gauss, but there are 3 different types of LSM303 sensors, and I believe mine is outputting to a default of a range of 4 gauss.  Problem is, I have no idea how the author of this code got this:

Code: [Select]
Xm_print = compass.m.x*(100000.0/1100.0); // Gain X [LSB/Gauss] for selected sensor input field range (1.3 in these case)
Ym_print = compass.m.y*(100000.0/1100.0); // Gain Y [LSB/Gauss] for selected sensor input field range
Zm_print = compass.m.z*(100000.0/980.0 );  // Gain Z [LSB/Gauss] for selected sensor input field range


From 1.3, or I'd change it myself.  The 100,000/1100 values.  Nor do I understand why Z gets its own unique value of 980 instead of 1100.  Maybe it would be easier to just figure out how to get my LSM303 to output in a range of 1.3 gauss.

EDIT:  Yes, confirmed, the above code is written for the LSM303DLHC, which can set the gauss range from 1.3 to 8.1 gauss.  I've detected that I'm running an LSM303D, which can set the gauss range from from 2 to 12 gauss.  So I can't set mine to output at a range of 1.3 gauss, which is what this code was written for, so I need to figure out how to rewrite this math to work with an output of 4 gauss.  I assume it is outputting a 16bit signed value, where +32767 = 4 gauss, and -32767 = -4 gauss.  I just can't understand how the author got "multiply this 16 bit integer by 90.90 for x and y, 102.04 for z" to get gauss out of the 16 bit value.

EDIT2:  So I did some different math.  Outputted the raw 16 bit values of the magnetometer.  Took my local norm of 53,921nT, found out that equals exactly 0.53921 gauss.  If 16 bits = 32767 = 4 gauss, then 0.53921gauss = 4417.0735175.  If I enter that as the magnetic norm in Magneto, I should be able to use the raw 16 bit outputs of the magnetometer in magneto as well, so everything is in the same scale.  Did that, copied all the bias and correction factors into this guy's code:

https://github.com/mikeshub/Pololu_Open_IMU/blob/master/Pololu_Open_IMU.ino

(also using his raw magneto output, which also uses 4 gauss range, just to make sure I haven't mismatched any axes)

And I'm still getting visible drift of about 0.1 degrees per second.

Maybe a range of 4 gauss means -2 to +2?  Let's try entering the magnetic norm of 8834.147035, double what we entered before.  Recalculated values, re-entered them into code...

Exact same amount of drift, about 0.1 degrees per second.  It's not massive, but it makes the IMU useless for any real applications.  

I tried plotting the raw magneto values on plot.ly, is it just me, or do they look WAY further off center than anyone else's raw magneto values?



I know the point of these calibrations is to move each of these 3 circles to the center, and then "squish" them into perfect circle shapes, could the fact that my magneto is so far off in the first place explain why the calibration can't seem to eliminate drift?

jremington

Magneto works.

If the corrected magnetometer data points do not lie (roughly) on the surface of a sphere centered on the origin, then you applied the Magneto scale matrix and offsets incorrectly.

adam_g

Hi there,

I've attempted calibrate my Adafruit LMS303DLHC several times using the instructions above, however I have not yet been able to get the tilt-compensation to actually work.

I've done some research on the topic, which involved thoroughly reading through this post, as well as Ed Mallon's calibration blog post, and sixD's tilt compensation thread on the Pololu forums (which has not yet been resolved). While it appears I'm calibrating properly, when I run the Pitch, Roll and Heading script, the heading will change significantly when any pitch or roll is experienced by the sensor.

Using Magneto and plot.ly for visualization, my uncalibrated values appeared as below:




After calibration, they improved to the following:




Accelerometer calibration was quite good to start off with, but after calibration bias was even closer to 0:



Now, while it seems I have the correct calibration values to calculate the pitch, heading, and roll, the tilt-compensation appears to be effectively not working. The only code I have changed in any of the steps has been to replaced the calibrated bias values. In the pitch, roll, and heading script they appear as follows:

Code: [Select]

// Accelerometer calibration
Xa_off = compass.a.x/16.0 + 17.224598; //X-axis combined bias (Non calibrated data - bias)
Ya_off = compass.a.y/16.0 + 12.282323; //Y-axis combined bias (Default: substracting bias)
Za_off = compass.a.z/16.0 - 3.162899; //Z-axis combined bias
Xa_cal =  0.949813*Xa_off + 0.009282*Ya_off + 0.004454*Za_off; //X-axis correction for combined scale factors (Default: positive factors)
Ya_cal =  0.009282*Xa_off + 0.983771*Ya_off - 0.001821*Za_off; //Y-axis correction for combined scale factors
Za_cal =  0.004454*Xa_off - 0.001821*Ya_off + 0.980313*Za_off; //Z-axis correction for combined scale factors

// Magnetometer calibration
Xm_off = compass.m.x*(100000.0/1100.0) + 683.481495; //X-axis combined bias (Non calibrated data - bias)
Ym_off = compass.m.y*(100000.0/1100.0) + 8449.523695; //Y-axis combined bias (Default: substracting bias)
Zm_off = compass.m.z*(100000.0/980.0 ) + 6823.709607; //Z-axis combined bias
Xm_cal =  0.882879*Xm_off + 0.024205*Ym_off + 0.016546*Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
Ym_cal =  0.024205*Xm_off + 0.863208*Ym_off + 0.007950*Zm_off; //Y-axis correction for combined scale factors
Zm_cal =  0.016546*Xm_off + 0.007950*Ym_off + 0.829441*Zm_off; //Z-axis correction for combined scale factors




I'm very curious as to whether I may have made a glaring mistake somewhere that has escaped notice. If anyone might have any thoughts on where I could have gone wrong, I would certainly appreciate it!  :)

Cheers,
A


jremington

What you have posted is generally the correct approach. However, I ALWAYS double check my corrected values to make sure I did not apply the corrections in the wrong direction.  Do that by outputting the corrected values and run magneto again, to see that the offsets are approximately zero and the rotation matrix is approximately the identity matrix.

Hint: it is much, much easier to work with normalized magnetometer and acceleration vectors.


Quote
the tilt-compensation appears to be effectively not working.
You have supplied no data and no code to support this conclusion. Are we supposed to guess what you did?

BTW the simplest and most straightforward tilt compensation algorithm was put in the public domain by the Pololu engineers and is posted on their site. Here is my version of the code, to which I added an unsophisticated magnetometer calibration upon startup.

It works very well.

Code: [Select]

#include <Wire.h>
#include <LSM303.h>
#include "vector.c"
// last update 2/6/2018
/*
   Tilt compensated compass by Pololu engineers, with mods by SJR
   This program assumes that an LSM303D carrier is oriented with X pointing
   North, Y pointing East, and Z pointing down (toward the ground).
   The code compensates for tilts of up to 90 degrees away from horizontal.
   Vector p, the "pointing vector" should be defined as pointing forward, parallel
   to the ground, with coordinates {X, Y, Z} in the sensor frame of reference.
*/
vector p = {1, 0, 0};

LSM303 compass;

char report[80];

// magnetometer offsets, uninitialized
// can be replaced by the output of the calibration step, once the sensor is in place

vector m_max = { -320000, -32000, -32000};
vector m_min = {32000, 32000, 32000};

void setup()
{
  vector a, m;

  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  unsigned long start = millis();
  Serial.println("Calibrating...");

  // calibration,
  // rotate sensor through all possible orientations, accumulating max and min
  // loop until nothing changes for 5 seconds

  while (millis() - start < 5000UL) {
    read_data(&a, &m);

    if (m.x < m_min.x) {
      m_min.x = m.x;
      start = millis();
    }
    if (m.x > m_max.x) {
      m_max.x = m.x;
      start = millis();
    }
    if (m.y < m_min.y) {
      m_min.y = m.y;
      start = millis();
    }
    if (m.y > m_max.y) {
      m_max.y = m.y;
      start = millis();
    }
    if (m.z < m_min.z) {
      m_min.z = m.z;
      start = millis();
    }
    if (m.z > m_max.z) {
      m_max.z = m.z;
      start = millis();
    }
  }
  Serial.println("magnetometer offset constants X, Y, Z");
  Serial.print(m_min.x);
  Serial.print(", ");
  Serial.print(m_max.x);
  Serial.print(", ");
  Serial.print(m_min.y);
  Serial.print(", ");
  Serial.print(m_max.y);
  Serial.print(", ");
  Serial.print(m_min.z);
  Serial.print(", ");
  Serial.println(m_max.z);
}
// Returns a set of acceleration and magnetic readings from the cmp01c.
void read_data(vector *a, vector *m)
{
  compass.read();
  a->x = compass.a.x;
  a->y = compass.a.y;
  a->z = compass.a.z;
  m->x = compass.m.x;
  m->y = compass.m.y;
  m->z = compass.m.z;
}

// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p (global).
int get_heading(const vector *a, const vector *m)
{
  vector E;
  vector N;

  // D X M = E, cross acceleration vector Down with M (magnetic north + inclination) to produce "East"
  vector_cross(a, m, &E);
  vector_normalize(&E);

  // E X D = N, cross "East" with "Down" to produce "North" (parallel to the ground)
  vector_cross(&E, a, &N);
  vector_normalize(&N);

  // compute heading, get Y and X components of heading from E dot p and N dot p
  int heading = round(atan2(vector_dot(&E, &p), vector_dot(&N, &p)) * 180 / M_PI);
  if (heading < 0)
    heading += 360;
  return heading;
}

void loop()
{
  vector a, m;
  read_data(&a, &m);
  // shift and scale magnetometer data
  m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
  m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
  m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
  // same for accelerometer. Should also consider offsets and scale here...
  a.x /= 16384.;
  a.y /= 16384.;
  a.z /= 16384.;

  /* for the curious...
    Serial.print(m.x);
    Serial.print(", ");
    Serial.print(m.y);
    Serial.print(", ");
    Serial.print(m.z);
    Serial.print(", ");
    Serial.print(a.x);
    Serial.print(", ");
    Serial.print(a.y);
    Serial.print(", ");
    Serial.println(a.z);
  */

  Serial.print("Heading: ");
  Serial.println(get_heading(&a, &m));
  delay(1000);
}



vector.c

Code: [Select]
#include "vector.h"
#include <math.h>

void vector_cross(const vector *a, const vector *b, vector *out)
{
 out->x = a->y * b->z - a->z * b->y;
 out->y = a->z * b->x - a->x * b->z;
 out->z = a->x * b->y - a->y * b->x;
}

float vector_dot(const vector *a, const vector *b)
{
  return a->x * b->x + a->y * b->y + a->z * b->z;
}

void vector_normalize(vector *a)
{
 float mag = sqrt(vector_dot(a, a));
 a->x /= mag;
 a->y /= mag;
 a->z /= mag;
}


vector.h

Code: [Select]
typedef struct vector
{
 float x, y, z;
} vector;

extern void vector_cross(const vector *a, const vector *b, vector *out);
extern float vector_dot(const vector *a, const vector *b);
extern void vector_normalize(vector *a);





adam_g

Hi Jim,

Thanks for your reply and I apologize for not providing the code and data earlier.

My code was copy pasted from above, with only my specific calibration values added.

Code: [Select]


#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

const float alpha = 0.15;
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
}

void loop()
{
  compass.read();
  float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;

  // Accelerometer calibration
  Xa_off = compass.a.x / 16.0 + 17.224598; //X-axis combined bias (Non calibrated data - bias)
  Ya_off = compass.a.y / 16.0 + 12.282323; //Y-axis combined bias (Default: substracting bias)
  Za_off = compass.a.z / 16.0 - 3.162899; //Z-axis combined bias
  Xa_cal =  0.949813 * Xa_off + 0.009282 * Ya_off + 0.004454 * Za_off; //X-axis correction for combined scale factors (Default: positive factors)
  Ya_cal =  0.009282 * Xa_off + 0.983771 * Ya_off - 0.001821 * Za_off; //Y-axis correction for combined scale factors
  Za_cal =  0.004454 * Xa_off - 0.001821 * Ya_off + 0.980313 * Za_off; //Z-axis correction for combined scale factors

  // Magnetometer calibration
  Xm_off = compass.m.x * (100000.0 / 1100.0) + 683.481495; //X-axis combined bias (Non calibrated data - bias)
  Ym_off = compass.m.y * (100000.0 / 1100.0) + 8449.523695; //Y-axis combined bias (Default: substracting bias)
  Zm_off = compass.m.z * (100000.0 / 980.0 ) + 6823.709607; //Z-axis combined bias
  Xm_cal =  0.882879 * Xm_off + 0.024205 * Ym_off + 0.016546 * Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
  Ym_cal =  0.024205 * Xm_off + 0.863208 * Ym_off + 0.007950 * Zm_off; //Y-axis correction for combined scale factors
  Zm_cal =  0.016546 * Xm_off + 0.007950 * Ym_off + 0.829441 * Zm_off; //Z-axis correction for combined scale factors


  // Low-Pass filter accelerometer
  fXa = Xa_cal * alpha + (fXa * (1.0 - alpha));
  fYa = Ya_cal * alpha + (fYa * (1.0 - alpha));
  fZa = Za_cal * alpha + (fZa * (1.0 - alpha));

  // Low-Pass filter magnetometer
  fXm = Xm_cal * alpha + (fXm * (1.0 - alpha));
  fYm = Ym_cal * alpha + (fYm * (1.0 - alpha));
  fZm = Zm_cal * alpha + (fZm * (1.0 - alpha));

  // Pitch and roll
  roll  = atan2(fYa, sqrt(fXa * fXa + fZa * fZa));
  pitch = atan2(fXa, sqrt(fYa * fYa + fZa * fZa));
  roll_print = roll * 180.0 / M_PI;
  pitch_print = pitch * 180.0 / M_PI;

  // Tilt compensated magnetic sensor measurements
  fXm_comp = fXm * cos(pitch) + fZm * sin(pitch);
  fYm_comp = fXm * sin(roll) * sin(pitch) + fYm * cos(roll) - fZm * sin(roll) * cos(pitch);

  // Arctangent of y/x
  Heading = (atan2(fYm_comp, fXm_comp) * 180.0) / M_PI;
  if (Heading < 0)
    Heading += 360;

  Serial.print("Pitch (X): "); Serial.print(pitch_print); Serial.print("  ");
  Serial.print("Roll (Y): "); Serial.print(roll_print); Serial.print("  ");
  Serial.print("Heading: "); Serial.println(Heading);
  delay(250);
}


I did a quick test with keeping the heading steady and alternately increasing the pitch and roll to about 90°. My results are shown below:
 


It seems when in my attempts at calibration, I've always experienced similar results. This leads me to believe there is definitely a systematic error on my part somewhere. I confirmed the signs of the corrected values, and will spend some more time with Magneto to make sure I continue to get offsets approaching zero.

I have had very good results with the tilt compensation calibration script from Pololu, and look forward to testing out your enhanced version.

Thanks again for the help!
Adam

jremington

Code: [Select]
  roll  = atan2(fYa, sqrt(fXa * fXa + fZa * fZa));
  pitch = atan2(fXa, sqrt(fYa * fYa + fZa * fZa));

One of the two equations for pitch and roll is wrong, so it is not surprising that the correction is wrong. I did not check further. Here is a reference to one possible set of correct functions to use in tilt compensation.

rokenbuzz

Code: [Select]
 roll  = atan2(fYa, sqrt(fXa * fXa + fZa * fZa));
  pitch = atan2(fXa, sqrt(fYa * fYa + fZa * fZa));

One of the two equations for pitch and roll is wrong, so it is not surprising that the correction is wrong.
I'm confused why you say something is wrong here.  It's straight from the OP.  Is it wrong there too?

EDIT: I guess I should mention, I am struggling with getting the OP's calibration and code working for me.

jremington

Quote
I'm confused why you say something is wrong here.  It's straight from the OP.  Is it wrong there too?
Yes, wrong there too. Those equations give the correct answer only if one of the two angles is zero.

See the reference linked in reply #8, or for roll and pitch use equations 25&26 or 28&29  from this link. The latter two depend on which rotation is regarded to be applied first.

I strongly prefer Pololu's vector method of tilt compensation, which gets around defining pitch and roll.

The confusing aspect of angular orientations is that there are six different angular system definitions in common use, partly with respect to the order in which the rotations are applied, and there is no universal convention for what defines a "positive" angle.

For compass and accelerometer calibration, here is an excellent, comprehensive overview.

adam_g

I'm confused why you say something is wrong here.  It's straight from the OP.  Is it wrong there too?

EDIT: I guess I should mention, I am struggling with getting the OP's calibration and code working for me.
Hi rokenbuzz,

I also struggled with the calibration and tilt compensation method provided by Scrius. It does appear that his equations for pitch and roll are incorrect.

I abandoned this method and also ended up using the Pololu LSM303 library. There is a code example provided by Kevin on the Pololu forums that was super helpful in finally getting my tilt-compensation working: https://forum.pololu.com/t/lsm303d-tilt-compensation-problem/11611/4

For my specific application (iceberg tracking beacon), I've implemented the following function to obtain pitch, roll and tilt-compensated heading. I apply a simple low-pass filter to smooth out the readings.

Cheers,
Adam

Code: [Select]
// Read pitch, roll and tilt-compensated heading from LSM303
void readLsm303()
{
  const float alpha = 0.10;   // Alpha
  float fXa, fYa, fZa = 0;    // Low-pass filtered accelerometer variables

  if (lsm303.init()) {
    lsm303.enableDefault();
    /*
      Calibration values: the default values of +/-32767 for each axis lead to an assumed
      magnetometer bias of 0. Use the Pololu LSM303 library Calibrate example program to
      determine appropriate values for each LSM303 sensor.

    */
    lsm303.m_min = (LSM303::vector<int16_t>) {
      -334, -792, -713  
    };
    lsm303.m_max = (LSM303::vector<int16_t>) {
      +918, +450, +512  
    };

    // Apply low-pass filter to accelerometer data
    for (byte i = 0; i < 30; i++) {
      lsm303.read();   // Read accelerometer and magnetometer data
      fXa = lsm303.a.x * alpha + (fXa * (1.0 - alpha));
      fYa = lsm303.a.y * alpha + (fYa * (1.0 - alpha));
      fZa = lsm303.a.z * alpha + (fZa * (1.0 - alpha));
      delay(5);
    }

    // Calculate orientation data
    float pitch = (atan2(-fXa, sqrt((long)fYa * fYa + (long)fZa * fZa)) * 180) / PI;
    float roll = (atan2(fYa, fZa) * 180) / PI;
    float heading = lsm303.heading((LSM303::vector<int>) {
      1, 0, 0   // PCB orientation
    });


    Serial.print(F("pitch: ")); Serial.println(pitch);
    Serial.print(F("roll: ")); Serial.println(roll);
    Serial.print(F("heading: ")); Serial.println(heading);

  }
  else {
    Serial.println(F("Error: Could not initialize LSM303!"));
  }
}



Go Up