1

I am building a small device that also uses magnetometer data in order to calculate the compass heading. The LSM9DS0 IMU sensor works great if the heading is calculated as a yaw (if the sensor is on a flat surface).

I have 3D printed a shell in which i am going to assemble all the electronics. My problem is that it is poorly designed and the IMU sensor is not on a flat surface, but it has to stay on 90 degrees. So by this, the Z axis is no more my way to calculate the yaw (or heading), but it changed to Y.

In order to calculate the heading on Z, i was using this formula:

heading.value = atan2((float)dof.my, (float)dof.mx);
        if(heading.value < 0) heading.value += 2*PI;
        if(heading.value > 2*PI) heading.value -= 2*PI;
 heading.value *= 180/PI;

...where my is the magnetometer Y and mx the magnetometer X

Now, I don't know how to calculate the heading based on other axis.

Tudor Popa
  • 11
  • 1
  • 3

1 Answers1

0

I know this thread hasn't been active for a while, but during my search I came across this publication by NXP which explains the solution really nicely.

In brief:

  1. Align the accelerometer readings (G) and the magnetometer readings (B) so they follow the NED coordinate system with the x-axis pointing forward and the y and z-axis to the right and down, respectively.

  2. Calculate the roll and pitch

    // Using atan2 to restrict +/- PI const roll = Math.atan2(Gy, Gz)

    // Using atan to restrict to +/- PI/2 const pitch = Math.atan(-Gx / (Gy * Math.sin(roll) + Gz * Math.cos(roll)))

  3. Calculate the yaw / compass heading (here Vx, Vy, Vz correspond to the Hard-Iron effects that can be calculated separately as discussed in this publication):

    // Using atan2 to restring to +/- PI let yaw = Math.atan2( (Bz-Vz)*Math.sin(roll) - (By-Vy)*Math.cos(roll), (Bx-Vx)*Math.cos(pitch) + (By-Vy)*Math.sin(pitch)*Math.sin(roll) + (Bz-Vz)*Math.sin(pitch)*Math.cos(roll))

  4. Correct the heading to [0, 2*PI)

    if( yaw < 0 ) { yaw += 2*Math.PI }

Navid
  • 384
  • 2
  • 7
  • Hi navid, I don't understand the publication how I can create the "Vy, Vx and Vz". Do you have an example? So that I can test your whole suggested code. – haassiej May 06 '20 at 22:21
  • In the [second publication above](https://www.nxp.com/files-static/sensors/doc/app_note/AN4246.pdf) they explain how to calculate the hard-iron effects. But to get started you can just set them to zero. – Navid May 10 '20 at 14:00