LSM303C Tilt compensation

Other Hardware Sensors
1

I am making a digital compass that needs tilt compensation. I have calibrated the magnetometer and the accelerometer and am getting fairly accurate headings when the device is level. I have followed a couple of the past posts about this but I've gotten stuck on one problem. I can get pitch and roll angles that seem to be accurate but the compensation only seems to work for the positive pitch condition, i.e. when I roll the device towards the +90deg position around the y-axis. As soon as I tip the compass in any other direction the heading swings way off.

I used this article for my tilt information: https://www.nxp.com/files-static/sensors/doc/app_note/AN3461.pdf

///////////CALCULATE RAW ACCELERATION MAGNITUDE///////////
float Gx = getAcc(A_cal_x);
float Gy = getAcc(A_cal_y);
float Gz = getAcc(A_cal_z);

///////////CALCULATE PITCH AND ROLL/////////////////

float pitch = pitchCalc(Gx, Gy, Gz);

float roll = rollCalc(Gx, Gz, pitch);

//////////CALCULATE TILT ADJUSTED MAGNETIC FIELDS///////////////


float Mx_comp = M_cal_x*cos(pitch) + M_cal_z*sin(pitch);
float My_comp = M_cal_x*sin(pitch)*sin(roll) + M_cal_y*cos(roll) - M_cal_z*sin(roll)*cos(pitch);
float Mz_comp = -M_cal_x*cos(roll)*sin(pitch) + M_cal_y*sin(roll) + M_cal_z*cos(roll)*cos(pitch);

//////////ACCELERATION/////////////////////
float getAcc(float raw_acc){
  
  float Gacc = (raw_acc)*(0.0001220703125);//(raw)*(Range/(2^(resolution-1)) 2^15 = 32,768
  
  return Gacc;

}
//////////////PITCH CALCULATIONS/////////////////
float pitchCalc(float AGx, float AGy, float AGz){


 
 float p = atan2(AGy, sqrt(sq(AGx) + sq(AGz)));
 
 
  return p;
}


float rollCalc(float GxA, float GzA, float Pitch){

  
   float R = atan2(GxA, -GzA);
    

  if ((Pitch == 90) || (Pitch ==-90)){
    R = 0;
     }

return R;
 
}

I would appreciate it if someone spots my mistake!

2

A much easier and more robust approach to tilt compensation is the vector based code used by Pololu.

The code is part of a library they provide for their LSM303 boards: Pololu - LSM303DLHC 3D Compass and Accelerometer Carrier with Voltage Regulator.

The better you do with the calibration (for both the accelerometer and the magnetometer), the better the sensor will work as a compass. I recommend the sailboatinstruments approach described in this post.

3

If the compass is going to move around you might consider an auto calibration scheme.

I mounted a magnetometer 180 degrees out of phase with another magnetometer. I sum the readings for each magnetometer axis and then use that reading for magnetometer processing.

4

Post ALL your code. The snippet that you posted may have several problems.

How is Pitch defined? The atan2 function returns radians, not degrees.

   float R = atan2(GxA, -GzA);  //incorrect signs of arguments.
   
  if ((Pitch == 90) || (Pitch ==-90)){  //degrees??
    R = 0;
     }

There is no need to scale the accelerometer data:

  float Gacc = (raw_acc)*(0.0001220703125);//(raw)*(Range/(2^(resolution-1)) 2^15 = 32,768
5

Thanks for your responses. My full code is included bellow. Pitch is defined as the rotation around the the y-axis from -90deg to +90deg, in my code I have kept it in radians because I return that value to be used in calculating my compensated magnetic field components. I convert to degrees just to print it.

I'm planning on implementing auto-calibration for the accelerometer eventually, but wanted to get the tilt working to some degree before I added it.

//////////LSM303C//////////////

/*START signal
 next byte of data is the address of slave in first 7 bits
 8th bit indicates if it is read or write
 */
 
#include<Wire.h>
#include<math.h>

//mag sensor
#define Madd 0x1E//address of slave magnetic sensor
#define msad_read 0x3D // SAD read condition
#define msad_write 0x3C //SAD write

//Accel sensor
#define Aadd 0x1D//address of slave accelorometer sensor
#define asad_read 0x3B
#define asad_write 0x3A

void setup() {
  Wire.begin();
  Serial.begin(9600);

  
Serial.println("LSM303C Digital Magnetometer");

///////SET ACCELEROMETER REGISTERS////////////////  make a loop for all this. form: slave, sub, message
set_regs(Aadd, 0x20, 0xEF);// 1110 1111, high resolution, 800Hz ODR, enable all axis, bdu on to prevent diferent reads from different samples*
set_regs(Aadd, 0x21, 0xC0);// 1100 0000 LPF At ODR(800) / 400 = 2Hz, most aggressive filer for now
set_regs(Aadd, 0x22, 0x00);// 0000 0000 
set_regs(Aadd, 0x23, 0x04);// 0000 0100
set_regs(Aadd, 0x24, 0x00);// 0000 0000
set_regs(Aadd, 0x25, 0x00);// 0000 0000 
///////SET MAGNETOMETER REGISTERS//////////////////
set_regs(Madd, 0x20, 0x5C);// 0101 1100, ulta-high performance mode, ODR=80Hz CTRL_REG1_M
set_regs(Madd, 0x21, 0x60);// 0110 0000 full scale +-16G, and reset before operation CTRL_REG2_M
set_regs(Madd, 0x22, 0x00);// 0000 0000 CTRL_REG3_M
set_regs(Madd, 0x23, 0x08);// 0000 1000 z-axis ultra-high performance, lsb at lower address CTRL_REG4_M
set_regs(Madd, 0x24, 0x00);// 0000 0000 continous update of magnetic data, 0100 0000 (0x40) will wait until data has been read before updating CTRL_REG5_M

}

//global variables
 int M_xl=0, M_yl=0, M_zl=0, M_xh=0, M_yh=0, M_zh=0;
 int Mx=0, My=0, Mz=0;
 int A_xl=0, A_yl=0, A_zl=0, A_xh=0, A_yh=0, A_zh=0;
float Ax=0, Ay=0, Az=0;
float Ax_Raw =0, Ay_Raw=0, Az_Raw=0;
 float A_cal_x=0, A_cal_y=0, A_cal_z=0;
 float M_cal_x =0, M_cal_y=0, M_cal_z=0;
  
void loop() {
///Check Status
int Mstat = getdata(Madd, 0x27);
int Astat = getdata(Aadd, 0x27);

//GET ACCELEROMETER DATA
///getdata(master, slave)////////
if ((Astat & 0x08) == 0x08){
A_xl = getdata(Aadd, 0x28);
A_xh = getdata(Aadd, 0x29);
A_yl = getdata(Aadd, 0x2A);
A_yh = getdata(Aadd, 0x2B);
A_zl = getdata(Aadd, 0x2C);
A_zh = getdata(Aadd, 0x2D);
}

//GET MAGNETOMETER DATA
if ((Mstat & 0x08) == 0x08){
M_xl = getdata(Madd, 0x28);//
M_xh = getdata(Madd, 0x29);//
M_yl = getdata(Madd, 0x2A);//
M_yh = getdata(Madd, 0x2B);//
M_zl = getdata(Madd, 0x2C);//
M_zh = getdata(Madd, 0x2D);//
}

//WHO AM I
int WhoM = getdata(Madd, 0x0F); 
int WhoA = getdata(Aadd, 0x0F);

//combine lsb and msb
Mx = ((M_xh<<8) | M_xl)*(0.0464);//[0.58mG/LSB], ODR=80Hz, converts output to gauss
My = ((M_yh<<8) | M_yl)*(0.0464);//0.00058*80=0.0464
Mz = ((M_zh<<8) | M_zl)*(0.0464);


Ax = ((int16_t)(A_xh<<8) | A_xl)*(0.000061);//0.000061 for 2g, 0.000122 for 4g, 0.000244 for 8g
Ay = ((int16_t)(A_yh<<8) | A_yl)*(0.000061);//
Az = ((int16_t)(A_zh<<8) | A_zl)*(0.000061);


//GET ACC OFFSET VALUES

// apply offset bias 
  /*offsets calcualted by Mageneto 1.2*/
Mx = Mx - 2878.527880;
My = My - 183.571936;
Mz = Mz - 1506.301507;

Ax = Ax - 0.040266;
Ay = Ay - 0.067500;
Az = Az - 0.004826;

//apply combined scaled factors
  /* calculated by magneto 1.2*/

M_cal_x = Mx*(0.003499) + My*(0.000001) + Mz*(0.000001);
M_cal_y = Mx*(0.000001) + My*(0.003501) - Mz*(0.000001);
M_cal_z = Mx*(0.000001) - My*(0.000001) + Mz*(0.000339);

A_cal_x =  Ax*(1.108194) - Ay*(0.010513) - Az*(0.006387);
A_cal_y = -Ax*(0.010513) + Ay*(1.163415) + Az*(0.011668);
A_cal_z = -Ax*(0.006387) + Ay*(0.011558) + Az*(0.919439);

//normalize results
float A_norm = sqrt(pow(A_cal_x, 2.0) + pow(A_cal_y, 2.0) + pow(A_cal_z, 2.0));
float M_norm = sqrt(pow(A_cal_x, 2.0) + pow(A_cal_y, 2.0) + pow(A_cal_z, 2.0));

A_cal_x = A_cal_x/A_norm;
A_cal_y = A_cal_y/A_norm;
A_cal_z = A_cal_z/A_norm;

M_cal_x = M_cal_x/M_norm;
M_cal_y = M_cal_y/M_norm;
M_cal_z = M_cal_z/M_norm;

A_cal_y = -1.0*A_cal_y;//because chip defines +Z as up
A_cal_z = -1.0*A_cal_z;//to keep the correct right-hand coordinate system

//digital low-pass filter
/* set in the control register for ACC slave */
float alpha = 0.15; //smoothing factor do not set too low or it will take too long to stabilize and real-time results will be lost, 0<alpha<1

A_cal_x = (alpha*A_cal_x) + (1-alpha)*(A_cal_x);
A_cal_y = (alpha*A_cal_y) + (1-alpha)*(A_cal_y);
A_cal_z = (alpha*A_cal_z) + (1-alpha)*(A_cal_z);

//CALCULATE RAW ACCELERATION MAGNITUDE
float Gx = getAcc(A_cal_x);
float Gy = getAcc(A_cal_y);
float Gz = getAcc(A_cal_z);

//CALCULATE PITCH AND ROLL
//float pitch = pitchCalc(A_cal_x, A_cal_y, A_cal_z);
float pitch = pitchCalc(Gx, Gy, Gz);
//float roll = rollCalc(A_cal_x, A_cal_y, A_cal_z, pitch);
float roll = rollCalc(Gx, Gz, pitch);

//CALCULATE TILT ADJUSTED MAGNETIC FIELDS
//only need x and y for heading calc

float Mx_comp = M_cal_x*cos(pitch) + M_cal_z*sin(pitch);
float My_comp = M_cal_x*sin(pitch)*sin(roll) + M_cal_y*cos(roll) - M_cal_z*sin(roll)*cos(pitch);
float Mz_comp = -M_cal_x*cos(roll)*sin(pitch) + M_cal_y*sin(roll) + M_cal_z*cos(roll)*cos(pitch);

//float Mx_comp = M_cal_x*cos(pitch) + M_cal_y*sin(roll)*sin(pitch) + M_cal_z*sin(pitch)*cos(roll);
//float My_comp = M_cal_y*cos(roll) - M_cal_z*sin(roll);

//float Mx_comp = M_cal_x*cos(pitch) + M_cal_z*sin(pitch);
//float My_comp = M_cal_y*cos(roll) - M_cal_z*sin(roll);

//CALCULATE HEADING W/ TILT CORRECTION 

float Heading = headingCalc(Mx_comp, My_comp);

//CALCULATE FIELD STRENGTH
float Fstrength = sqrt(pow(M_cal_x, 2.0) + pow(M_cal_y, 2.0) + pow(M_cal_z, 2.0));



printOut(Heading,Fstrength, M_cal_x, M_cal_y, M_cal_z, A_cal_x, A_cal_y, A_cal_z, WhoM, WhoA);

//TEST PRINTS
 Serial.print("Pitch:");Serial.println(pitch*(180/PI));
 Serial.print("Roll:");Serial.println(roll*(180/PI));
 
 delay(1000);
}


//SET REGISTERS//
////this function will be used to set all the control registers in setup
void set_regs(int slave,int sub, int message){
  
Wire.beginTransmission(slave);
Wire.write(sub); 
Wire.write(message); 
Wire.endTransmission();

}

//GET DATA//


int getdata(int slave, int sub){
  
  Wire.beginTransmission(slave);
  Wire.write(sub);
  Wire.endTransmission();
  Wire.requestFrom(slave,1);
  int data = Wire.read();

return (data);  

}

//HEADING CALCULATIONS
float headingCalc(float M_cal_x, float M_cal_y){
  float head;
  
     if ((M_cal_x == 0) && (M_cal_y < 0)){
     head = 90; 
 } else if ((M_cal_x ==0) && (M_cal_y>0)){
  head = 0;
 }
 head = ((atan2(M_cal_y, M_cal_x)) * (180/PI))-17.14; //magnetic declination is -17.14, for true north delete

 if (head < 0){
   head = head + 360;
  } else if (head > 360){
    head = head - 360;
  }

  return head;
  
}
//ACCELERATION
float getAcc(float raw_acc){
  
  float Gacc = (raw_acc)*(0.0001220703125);//(raw)*(Range/(2^(resolution-1)) 2^15 = 32,768
  
  return Gacc;

}
//PITCH CALCULATIONS
float pitchCalc(float AGx, float AGy, float AGz){

// pitchCalc(float Ax_cal, float Ay_cal, float Az_cal){

 //float p = atan2(Ax_cal, sqrt(sq(Ay_cal) + sq(Az_cal)));
 float p = atan2(AGy, sqrt(sq(AGx) + sq(AGz)));
 //float p = asin(Ax_cal);
 
  return p;
}


float rollCalc(float GxA, float GzA, float Pitch){
//float rollCalc (float Ax_cal, float Ay_cal, float Az_cal,float Pitch){

  //float R = atan2(Ay_cal, sqrt(sq(Ax_cal) + sq(Az_cal)));
   float R = atan2(GxA, -GzA);
    
 //float R = -asin(Ay_cal/cos(Pitch));
 
  if ((Pitch == 90) || (Pitch ==-90)){
    R = 0;
     }

return R;
 
}

//PRINT OUTS
void printOut(float Heading, float Fstrength, float M_cal_x, float M_cal_y, float M_cal_z, float A_cal_x, float A_cal_y, float A_cal_z, int WhoM, int WhoA){

// Serial.println("LSM303 Outputs");
// Serial.println("  ");
Serial.print("Heading: "); Serial.println(Heading);
// Serial.print("Field Strength: "); Serial.print(Fstrength);Serial.println("[G]");

}

Note that for the pitch and roll functions I have multiple versions I have been trying that are commented out. It looks a bit confusing.