-1

I am working on a tilt compensated compass with the LSM303DLHC. I manage to calibrate the magnetic and accelero part. I managed to calculate pitch and roll. But when it comes to calculate tilt compensation for the compass something goes wrong. My values on the serial monitor just don't make sense...If I calculate compass heading from calibrated magneto values it works using the Y axis as the magnetic vector. How would I be able to change that to the X-axis?

   LSM303DLHC including accelerometer to correct for position

*/
#include <Wire.h>


const int LSM303_ADDR = 0x19;
const int LSM303_ADDR_MAG = 0x1E;

//control register addresses
const int CTRL_REG1_ADDR = 0x20;
const int CTRL_REG2_ADDR = 0x21;
const int CTRL_REG3_ADDR = 0x22;
const int CTRL_REG4_ADDR = 0x23;
const int CTRL_REG5_ADDR = 0x24;
const int CTRL_REG6_ADDR = 0x25;

const int CRA_REG_M_ADDR = 0x00;
const int CRB_REG_M_ADDR = 0x01;
const int MR_REG_M_ADDR = 0x02;

//data register addresses LSB first for accelero, MSB first for magneto
const int Accelero_First_data_addr = 0x28;
const int Magneto_First_data_addr = 0x03;
const int Temp_out_data_addr = 0x31;//MSB first 12 bits


int CTRL_REG1_A_value = 0x47; //50Hz Low Power disable, all axes enabled
int CTRL_REG2_A_value = 0x00; //
int CTRL_REG3_A_value = 0x00; //no interrupt or watermark enabled
int CTRL_REG4_A_value = 0x00; // 2G full scale, high resolution, no SPI mode selected
int CTRL_REG5_A_value = 0x00; // nothing enabled
int CTRL_REG6_A_value = 0x00; // nothing enabled

int CRA_REG_M_value = 0x14; // Temp sens enabled, 15Hz data rate output
int CRB_REG_M_value = 0x20; // 2G is data 1100 - 980 G/LSB

int MR_REG_M_value = 0x00;

int16_t MagRaw_X_axis_value, MagRaw_Y_axis_value, MagRaw_Z_axis_value;
int16_t AccRaw_X_axis_value, AccRaw_Y_axis_value, AccRaw_Z_axis_value;

float pitch, roll;
float Xm_calibrated, Ym_calibrated, Zm_calibrated;
float Xm_norm, Ym_norm, Zm_norm, Xa_norm, Ya_norm, Za_norm;
float norm_a, norm_m;

void setup() {// put your setup code here, to run once:
  Serial.begin(9600);
  Wire.begin();
  setupLSM303();
  Serial.println("Setup complete..");
}

void loop() {// put your main code here, to run repeatedly:
  MagnetoDataRead();
  AcceleroDataRead();

}



void setupLSM303() {
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG1_ADDR);
  Wire.write(CTRL_REG1_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG2_ADDR);
  Wire.write(CTRL_REG2_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG3_ADDR);
  Wire.write(CTRL_REG3_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG4_ADDR);
  Wire.write(CTRL_REG4_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG5_ADDR);
  Wire.write(CTRL_REG5_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG6_ADDR);
  Wire.write(CTRL_REG6_A_value);
  Wire.endTransmission();
  //magneto register setup
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(CRA_REG_M_ADDR);
  Wire.write(CRA_REG_M_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(CRB_REG_M_ADDR);
  Wire.write(CRB_REG_M_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(MR_REG_M_ADDR);
  Wire.write(MR_REG_M_value);
  Wire.endTransmission();
}


void MagnetoDataRead() {
  Wire.beginTransmission(LSM303_ADDR_MAG );
  Wire.write(Magneto_First_data_addr);
  Wire.endTransmission();

  Wire.requestFrom(LSM303_ADDR_MAG , 6);
  if (Wire.available() <= 6) {
    MagRaw_X_axis_value = (Wire.read() << 8) | (Wire.read());
    MagRaw_Y_axis_value = (Wire.read() << 8) | (Wire.read());
    MagRaw_Z_axis_value = (Wire.read() << 8) | (Wire.read());

    float X_mag_cal = MagRaw_X_axis_value / 1100.0;
    float Y_mag_cal = MagRaw_Y_axis_value / 1100.0;
    float Z_mag_cal = MagRaw_Z_axis_value / 980.0;

    //input calibration factors
    float Xm_offset_corrected = MagRaw_X_axis_value - 0.041423;
    float Ym_offset_corrected = MagRaw_Y_axis_value + 0.022448;
    float Zm_offset_corrected = MagRaw_Z_axis_value + 0.078354;

    Xm_calibrated = 94.622065 * Xm_offset_corrected + 0.831029 * Ym_offset_corrected + 1.647934 * Zm_offset_corrected;
    Ym_calibrated = 0.831029 * Xm_offset_corrected + 93.462985 * Ym_offset_corrected + 3.416885 * Zm_offset_corrected;
    Zm_calibrated = 1.647934 * Xm_offset_corrected + 3.416885 * Ym_offset_corrected + 76.477691 * Zm_offset_corrected;

    //normalization formula
    norm_m = sqrt(sq(Xm_calibrated) + sq(Ym_calibrated) + sq(Zm_calibrated));
    Xm_norm = Xm_calibrated / norm_m;
    Ym_norm = Ym_calibrated / norm_m;
    Zm_norm = Zm_calibrated / norm_m;

    //float Heading = ((atan2(Ym_calibrated, Xm_calibrated)) * 180 ) / PI;
    //if (Heading < 0) {
    //  Heading = 360 + Heading;
    //}
    /*
        Serial.print(X_mag_cal, 10);
        Serial.print("\t");
        Serial.print(Y_mag_cal, 10);
        Serial.print("\t");
        Serial.print(Z_mag_cal, 10);
        Serial.println("\t");
    */

    delay(50);

  }
}

void AcceleroDataRead() {
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(Accelero_First_data_addr | 0x80);
  Wire.endTransmission();

  Wire.requestFrom(LSM303_ADDR, 6);
  if (Wire.available() <= 6) {
    AccRaw_X_axis_value = (Wire.read()) | (Wire.read() << 8);
    AccRaw_Y_axis_value = (Wire.read()) | (Wire.read() << 8);
    AccRaw_Z_axis_value = (Wire.read()) | (Wire.read() << 8);

    float X_Acc =  (AccRaw_X_axis_value >> 4);
    float Y_Acc =  (AccRaw_Y_axis_value >> 4);
    float Z_Acc =  (AccRaw_Z_axis_value >> 4);
//calibrate offset
    float X_acc_offset_corrected = X_Acc + 6.676443;
    float Y_acc_offset_corrected = Y_Acc - 6.622381;
    float Z_acc_offset_corrected = Z_Acc + 55.408804;
//calibrate
    float X_acc_calibrated = 0.977273 * X_acc_offset_corrected - 0.003458 * Y_acc_offset_corrected + 0.012639 * Z_acc_offset_corrected;
    float Y_acc_calibrated = -0.003458 * X_acc_offset_corrected + 0.979674 * Y_acc_offset_corrected - 0.001130 * Z_acc_offset_corrected;
    float Z_acc_calibrated = 0.012639 * X_acc_offset_corrected - 0.001130 * Y_acc_offset_corrected + 0.897083 * Z_acc_offset_corrected;
//normalize
    norm_a = sqrt(sq(X_acc_calibrated) + sq(Y_acc_calibrated) + sq(Z_acc_calibrated));
    Xa_norm = X_acc_calibrated / norm_a;
    Ya_norm = Y_acc_calibrated / norm_a;
    Za_norm = Z_acc_calibrated / norm_a;

//calculate pitch and roll
    pitch = asin(-Xa_norm) * 180 / PI;
    roll = (atan2(Ya_norm, Za_norm) * 180) / PI;
//calculate vectors fo tilt compensation
    float Xh = Xm_norm * cos(pitch * PI / 180) + Zm_norm * sin(pitch * PI / 180);
    float Yh = Xm_norm * sin(roll * PI / 180) * sin(pitch * PI / 180) + Ym_norm * cos(roll * PI / 180) - Zm_norm * sin(roll * PI / 180) * cos(pitch * PI / 180);
//calculate tilt compensated heading
    float Heading = ((atan2(Yh, Xh)) * 180.0 ) / PI;
    if (Heading < 0) {
      Heading = 360 + Heading;
    }

    Serial.print("Pitch \t");
    Serial.print(pitch);
    Serial.print("\t roll \t");
    Serial.print(roll);
    Serial.print("\t");
    Serial.println(Heading);

    delay(50);
  }
}

I have been stuck on this for a few days now. Any help is appreciated!

  • Are you developing on an STMicro evaluation kit? If so you might be able to you STMicro MotionMC and MotionEC (warning, no source code!) libraries to implement a tilt compass. Ug, my bad, this the the Arduino forum - of course you are not using an STMicro development kit! – st2000 May 15 at 17:32
  • Also, are you wanting to tilt your compass beyond normal? As in more than, say, 30 degrees? If you want to hold you compass at 90 degrees WRT the local surface of the Earth you will have to do more than just implement a tilt compass. – st2000 May 15 at 17:34
  • Hi! Thank you for commenting! I am developing on a NodeMcu with the Arduino IDE. My compass will not be tilted beyond these kind of angles. Formulas used in my code are ectracted from the lsm303 app notes – sanrays10 May 15 at 18:27
  • Hmm, in another STMicro chip (lsm9ds1) one of the acc vectors is backwards WRT the mag. I don't think that's a problem here w/the LSM303. Could you be having problem converting to/from degrees/radians? That can get confusing depending on where you are getting your trig function from. Also, (just curious) are you keeping everything in Euler angles or are you using quaternions? – st2000 May 15 at 18:49
  • I have read somewhere that for the dlhc that the earths magnetic field is aligned along the y axis. Let me re-check the source though. Eventhough its just partially the problem I have. – sanrays10 May 15 at 19:24
0

Found it...Magneto data is coming out XZY not XYZ...when I changed this it works! Sorry, RTFMA would have given me the answer straight away.

| improve this answer | |
  • Glad to hear it. But you posted the answer 10 hours ago and made a comment that it still does not work 8 hours ago. Which is it? – st2000 May 16 at 19:30
  • Sorry for the confusion. The sensor data for magneto and accelero independently work. But when I have to combine the 2 for tilt compensation...it does not compensate... – sanrays10 May 16 at 22:51
  • Try changing the sign of your Z-normal value before using it in your Xh and Yh equations (about 15 lines before the end of your program). If it works, I'll post an answer explaining why. – st2000 May 17 at 2:01
  • I have tried Za_norm = -Z_acc_calibrated/norm_a; but no luck. I also tried to change the sign in the Xh formula...also no luck... – sanrays10 May 17 at 10:25
  • Bummer. I'm also wondering about the pitch calculation. It is different than the STMicro paper I'm looking at. Are you using this stmicro 2010 documet? I can not even find that one on the STMicro web site. Try this stmicro 2018 document. It appears the Pitch calculation is different. You might want to print out your Pitch and Roll values to check. – st2000 May 17 at 13:52
0

It seems I got it working! Thanks a lot for your help, I owe you a few beers. For anyone following and interested here is my code:

   LSM303DLHC including accelerometer to correct for position
  Vector calculation to be implemented
*/
#include <Wire.h>
#include <Math.h>




const int LSM303_ADDR = 0x19;
const int LSM303_ADDR_MAG = 0x1E;

//control register addresses
const int CTRL_REG1_ADDR = 0x20;
const int CTRL_REG2_ADDR = 0x21;
const int CTRL_REG3_ADDR = 0x22;
const int CTRL_REG4_ADDR = 0x23;
const int CTRL_REG5_ADDR = 0x24;
const int CTRL_REG6_ADDR = 0x25;

const int CRA_REG_M_ADDR = 0x00;
const int CRB_REG_M_ADDR = 0x01;
const int MR_REG_M_ADDR = 0x02;

//data register addresses LSB first for accelero, MSB first for magneto
const int Accelero_First_data_addr = 0x28;
const int Magneto_First_data_addr = 0x03;
const int Temp_out_data_addr = 0x31;//MSB first 12 bits


int CTRL_REG1_A_value = 0x47; //50Hz Low Power disable, all axes enabled
int CTRL_REG2_A_value = 0x00; //
int CTRL_REG3_A_value = 0x00; //no interrupt or watermark enabled
int CTRL_REG4_A_value = 0x00; // 2G full scale, high resolution, no SPI mode selected
int CTRL_REG5_A_value = 0x00; // nothing enabled
int CTRL_REG6_A_value = 0x00; // nothing enabled

int CRA_REG_M_value = 0x14; // Temp sens enabled, 15Hz data rate output
int CRB_REG_M_value = 0x20; // 2G is data 1100 - 980 G/LSB

int MR_REG_M_value = 0x00;

int16_t MagRaw_X_axis_value, MagRaw_Y_axis_value, MagRaw_Z_axis_value;
int16_t AccRaw_X_axis_value, AccRaw_Y_axis_value, AccRaw_Z_axis_value;

float pitch, roll;
float Xm_calibrated, Ym_calibrated, Zm_calibrated;
float Xm_norm, Ym_norm, Zm_norm, Xa_norm, Ya_norm, Za_norm;
float norm_a, norm_m;

void setup() {// put your setup code here, to run once:
  Serial.begin(9600);
  Wire.begin();
  setupLSM303();
  Serial.println("Setup complete..");
}

void loop() {// put your main code here, to run repeatedly:
  MagnetoDataRead();
  AcceleroDataRead();

}



void setupLSM303() {
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG1_ADDR);
  Wire.write(CTRL_REG1_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG2_ADDR);
  Wire.write(CTRL_REG2_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG3_ADDR);
  Wire.write(CTRL_REG3_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG4_ADDR);
  Wire.write(CTRL_REG4_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG5_ADDR);
  Wire.write(CTRL_REG5_A_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(CTRL_REG6_ADDR);
  Wire.write(CTRL_REG6_A_value);
  Wire.endTransmission();
  //magneto register setup
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(CRA_REG_M_ADDR);
  Wire.write(CRA_REG_M_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(CRB_REG_M_ADDR);
  Wire.write(CRB_REG_M_value);
  Wire.endTransmission();
  Wire.beginTransmission(LSM303_ADDR_MAG);
  Wire.write(MR_REG_M_ADDR);
  Wire.write(MR_REG_M_value);
  Wire.endTransmission();
}


void MagnetoDataRead() {
  Wire.beginTransmission(LSM303_ADDR_MAG );
  Wire.write(Magneto_First_data_addr);
  Wire.endTransmission();

  Wire.requestFrom(LSM303_ADDR_MAG , 6);
  if (Wire.available() <= 6) {
    MagRaw_X_axis_value = (Wire.read() << 8) | (Wire.read());
    MagRaw_Z_axis_value = (Wire.read() << 8) | (Wire.read());
    MagRaw_Y_axis_value = (Wire.read() << 8) | (Wire.read());

    float X_mag_cal = MagRaw_X_axis_value / 1100.0;
    float Y_mag_cal = MagRaw_Y_axis_value / 1100.0;
    float Z_mag_cal = MagRaw_Z_axis_value / 980.0;

    //input calibration factors
    float Xm_offset_corrected = X_mag_cal - 0.032874;
    float Ym_offset_corrected = Y_mag_cal + 0.047876;
    float Zm_offset_corrected = Z_mag_cal + 0.039649;

    Xm_calibrated = 92.387197 * Xm_offset_corrected - 0.307374 * Ym_offset_corrected + 1.151625 * Zm_offset_corrected;
    Ym_calibrated = -0.307374 * Xm_offset_corrected + 85.002110 * Ym_offset_corrected + 1.169755 * Zm_offset_corrected;
    Zm_calibrated = 1.151625 * Xm_offset_corrected + 1.169755 * Ym_offset_corrected + 80.276037 * Zm_offset_corrected;

    //normalization formula
    norm_m = sqrt(sq(Xm_calibrated) + sq(Ym_calibrated) + sq(Zm_calibrated));
    Xm_norm = Xm_calibrated / norm_m;
    Ym_norm = Ym_calibrated / norm_m;
    Zm_norm = Zm_calibrated / norm_m;

    /*
        Serial.print(X_mag_cal, 10);
        Serial.print("\t");
        Serial.print(Y_mag_cal, 10);
        Serial.print("\t");
        Serial.print(Z_mag_cal, 10);
        Serial.println("\t");
    */

    //float Heading = ((atan2(Ym_norm, Xm_norm)) * 180.0 ) / PI;
    //if (Heading < 0) {
    // Heading = 360 + Heading;
    //}
    //Serial.println(Heading);
    //delay(50);

  }
}

void AcceleroDataRead() {
  Wire.beginTransmission(LSM303_ADDR);
  Wire.write(Accelero_First_data_addr | 0x80);
  Wire.endTransmission();

  Wire.requestFrom(LSM303_ADDR, 6);
  if (Wire.available() <= 6) {
    AccRaw_X_axis_value = (Wire.read()) | (Wire.read() << 8);
    AccRaw_Y_axis_value = (Wire.read()) | (Wire.read() << 8);
    AccRaw_Z_axis_value = (Wire.read()) | (Wire.read() << 8);

    float X_Acc =  (AccRaw_X_axis_value >> 4);
    float Y_Acc =  (AccRaw_Y_axis_value >> 4);
    float Z_Acc =  (AccRaw_Z_axis_value >> 4);

    //calibrate offset
    float X_acc_offset_corrected = X_Acc + 6.676443;
    float Y_acc_offset_corrected = Y_Acc - 6.622381;
    float Z_acc_offset_corrected = Z_Acc + 55.408804;

    //calibrate
    float X_acc_calibrated = 0.977273 * X_acc_offset_corrected - 0.003458 * Y_acc_offset_corrected + 0.012639 * Z_acc_offset_corrected;
    float Y_acc_calibrated = -0.003458 * X_acc_offset_corrected + 0.979674 * Y_acc_offset_corrected - 0.001130 * Z_acc_offset_corrected;
    float Z_acc_calibrated = 0.012639 * X_acc_offset_corrected - 0.001130 * Y_acc_offset_corrected + 0.897083 * Z_acc_offset_corrected;
    //normalize
    norm_a = sqrt(sq(X_acc_calibrated) + sq(Y_acc_calibrated) + sq(Z_acc_calibrated));
    Xa_norm = X_acc_calibrated / norm_a;
    Ya_norm = Y_acc_calibrated / norm_a;
    Za_norm = Z_acc_calibrated / norm_a;

    //calculate pitch and roll
    /*
      pitch = asin(-Xa_norm) * 180 / PI;
      roll = asin(Ya_norm / cos(pitch * PI / 180)) * 180 / PI;
    */
    roll = atan2(Ya_norm, Za_norm);
    float  Gz2 = Ya_norm * sin(roll) + Za_norm * cos(roll);
    pitch = atan(-Xa_norm / Gz2);
    float By2 = Zm_norm * sin(roll) - Ym_norm * cos(roll);
    float Bz2 = Ym_norm * sin(roll) + Zm_norm * cos(roll);
    float Bx3 = Xm_norm * cos(pitch) + Bz2 * sin(pitch);
    float yaw = (atan2(By2, Bx3)*180)/PI;
    if (yaw < 0) {
      yaw = 360 + yaw;
    }

    float tilt = atan2(sqrt((sq(Xa_norm)+(sq(Ya_norm)))),Za_norm);

    //calculate vectors fo tilt compensation
    float Xh = Xm_norm * cos(pitch) + Zm_norm * sin(pitch);
    float Yh = Xm_norm * sin(roll ) * sin(pitch) + Ym_norm * cos(roll) - Zm_norm * sin(roll) * cos(pitch);

        // calculate tilt compensated heading
        float Heading = ((atan2(Yh, Xh)) * 180.0 ) / PI;
        if (Heading < 0) {
          Heading = 360 + Heading;
        }


    Serial.print("Pitch \t");
    Serial.print(pitch * 180 / PI);
    Serial.print("\t roll \t");
    Serial.print(roll * 180 / PI);
    Serial.print("\t tilt \t");
    Serial.print(tilt*180/PI);
    Serial.print("\t");
    Serial.println(Heading);
    delay(500);
  }
} 
| improve this answer | |

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.