Tilt Compensated Compass

18,581

54

27

Introduction: Tilt Compensated Compass

About: 55+ years in electronics, computers, and teaching ... now retired.

This instructable explains how to make a tilt compensated compass using an Arduino UNO R3, an LCD display, and an IvenSense MPU-9250 multi-chip-module that contains an MPU-6050 accelerometer / gyro and an AK8963 magnetometer within the same package.

The LCD simultaneously displays the Heading, (P)itch, and (R)oll.

The heading accuracy is within 2 degrees depending on how well the compass has been calibrated.

Without tilt compensation the compass headings vary significantly ... sometimes by as much as 100 degrees.

When stabilised, the tilted compass headings only vary by one or two degrees ... the improvement is amazing.

The tilt stabilization may be disabled by placing a jumper wire between Arduino pins A0 and GND.

The estimated cost for this project is $20 USD.

Images

The opening photo shows the tilt compensated compass resting on a 30-60 degree set square. The pitch angle is approximately 30 degrees, the roll angle is zero, and the heading is 100 degrees magnetic.

The video shows the compass both “with” and “without” tilt compensation.

Warning

Do not use this compass in situations involving safety to life, such as navigation at sea, as violent shaking (rapid movement) can affect the gyro accuracy requiring a system reset.

Step 1: Circuit Diagram

Photo 1 shows how the Arduino UNO R3, the LCD display, and the MPU-9250 accelerometer/gyro/magnetometer are wired together.

The tilt stabilization may be disabled by placing a jumper wire between Arduino pins A0 and GND.

The MPU-9250 must:

  • be flat when the compass is placed on a level surface.
  • be clear of any ferrous metals.

Step 2: Parts List

The following parts where obtained from https://www.aliexpress.com/

  • 1 only Arduino UNO R3 and USB cable
  • 1 only serial LCD display
  • 1 only MPU-9250 accelerometer/gyro/magnetometer
  • 2 only Arduino female-to-female jumper cables
  • 6 only Arduino male-to-female jumper cables

The following parts were obtained locally:

  • 9 volt battery
  • 9 volt battery clip/leads
  • Scrap plastic sheet for base
  • 12 only threaded nylon spacers
  • 20 only M3 x 5mm bolts

The estimated cost for this project is $20 USD.

Step 3: Theory

Tilt compensated compass

A conceptual view of a “tilt compensated compass” is shown in photo 1.

The InvenSense MPU-9250 chip contains two separate integrated circuits within the same package:

  • an MPU-6050 gyro / accelerometer and
  • an AK8963 magnetometer from Asahi/Kasei

Calculating pitch and roll

Assume that:

  • We are in an aircraft that is heading north (up the page) along the gyro X axis.
  • The Z (yaw) axis is pointing down, and
  • the X (pitch) and Y (roll) axes are both horizontal
  • The pitch and roll readings are both zero

Let’s now raise the aircraft nose by 45 degrees:

  • The roll reading remains at zero since there is no rotation about the roll axis.
  • The pitch reading becomes 45 degrees due to the rotation about the pitch axis.
  • The yaw axis is now tilted by 45 degrees.

Let’s now yaw the aircraft by 90 degrees:

  • The Yaw axis remains tilted .
  • The pitch should now read zero as the aircraft body is now horizontal.
  • The roll should now read 45 degrees as the wings are now tilted.

Since there was no rotation about either the pitch or roll axes, the pitch angle still reads 45 degrees, and the roll angle is still reads zero ... we have a problem!

Enter the accelerometer which is able to measure acceleration about each of the XYZ axes. As the aircraft yaws the accelerometer allows us to gradually transfer the pitch reading to the roll reading, and vice versa.

The Accelerometer is also able to correct for gyro drift ... our pitch and roll readings are now correct!!

The following two Youtube videos explain how this is done:

Determining the compass heading

The X and Y outputs from the AK8963 magnetometer allow us to determine the compass “heading” using the formula:

  • Heading = atan2(Y, X) * RAD_TO_DEG;............................................................................. (1)

This “heading” is only valid while the compass is level ... any tilt and the reading will change dramatically. This variation can be greater than 100 degrees !!!!

The solution is to fool the compass into thinking that it is always horizontal.

We do this by plugging the “pitch” and “roll” values from the MPU-6050 into equations (2) and (3) and using these new (compensated) values in equation (1):

  • Xhorizontal = X*cos(pitch) + Y*sin(roll)*sin(pitch) – Z*cos(roll)*sin(pitch) ............................(2)
  • Yhorizontal = Y*cos(roll) + Z*sin(roll) .................................................................................. (3)

The above formulas are found in several of the reference papers below.

When applying these formulas, the orientation of the MPU-6050 and AK8963 XYZ axes within the MPU-9250 package must be taken into consideration.

From the magnetometers point of view:

  • X equates to the gyro Y
  • Y equates to the gyro X
  • and, since the Z axes point in opposite directions, the pitch is negative.

This is fixed by preceding the above equations with the following code:

  • Mag_pitch = -Gyro_roll_output * DEG_TO_RAD; ............................................................... (4)
  • Mag_roll = Gyro_pitch_output * DEG_TO_RAD; ................................................................ (5)

Theoretically, any variations in the compass heading due to tilt will disappear. In practice there is still a small variation of one or two degrees as shown in photo 2.

Inspiration

The “pitch” and “roll” code for this compass is modeled on the MPU-6050 IMU (Inertial Management Unit) described by Joop Brokking, http://www.brokking.net/imu.html.

The algorithm for calibrating the magnetometer is described in the following article by Kris Winer, https://github.com/kriswiner/MPU6050/wiki/Simple-a...

The remaining code is my own.

References

Step 4: Software Installation

It is essential that you perform this step BEFORE uploading the compass code to your Arduino

Editing your I2C Wire Library.

According to the breakout board schematic (photo 1), the MPU-9250 chip has 10K ohm pull-up resistors connected to 3.3 volts on each of the SDA (data) and SCL (clock) lines.

The Arduino, however, has internal pull-up resistors to 5 volts. These pull-up resistors are not required and should be disabled to prevent the I2C lines from rising above 3.3 volts and damaging the MPU-9250.

I recommend editing lines 75 ,76, & 77 in file “C:Users\...\Documents\Arduino\libraries\Wire\utility\twi.c” to read:

  • // deactivate internal pullups for twi.
  • digitalWrite(SDA, 0);
  • digitalWrite(SCL, 0);

These commands could be placed inside the Arduino setup() function following the Wire.begin() function but it is still possible for the I2C lines to rise above their safe voltage level until the code lines are run.

Use a text editor such as Notepad++ when editing any files. Do NOT use a word processor.

Installing the compass code

  • Disconnect the I2C lines from your Arduino [1]
  • Edit your I2C Wire library as described above.
  • Copy the contents of the attached “"tilt_comp_compass.ino"” file into an Arduino sketch.
  • Save the Arduino sketch as “"tilt_comp_compass” (without the quotes).
  • Compile and upload the sketch to your Arduino.
  • Unplug the Arduino.
  • Connect the Arduino I2C lines.
  • Apply power to your project.

Notes

[1]

The reason for disconnecting the I2C SDA (data), and SCL ( lines is that Arduino pins A4 & A5 may be in an output “high” state from a previous project. Disconnecting these wires eliminates the possibility of 5 volts damaging the MPU-9250.

[2]

16 September 2019

The code "tilt_comp_compass_v1.01.ino" contains a minor bug-fix.

Step 5: Calibrating

Depending on their orientation with respect to the Earth’s magnetic field, the XYZ outputs from the magnetometer change from +ve to -ve (positive to negative) as the magnetometer is rotated.

If you rotate the MPU-9250 about each axis, the XYZ outputs should each plot a perfect circle centered about the 3D XYZ coordinate (0,0,0).

Hard-iron distortion

In practice these circles are NOT centered over the 3D coordinate (0,0,0) but are displaced either up or down, or to the left or right.

These displacements are due to “Hard-iron” distortion from, say, a magnetised object such as a speaker. Such distortions are always additive and the offsets can be calculated (then subtracted) using the following code: [1]

  • Mag_x_offset = (mag_x_max + mag_x_min) / 2;
  • Mag_y_offset = (mag_y_max + mag_y_min) / 2;
  • Mag_z_offset = (mag_z_max + mag_z_min) / 2;

Soft-iron distortion

There is also another form of distortion called “Soft-iron” distortion” that arises from the proximity of ferrous, and other materials, that disturb the earth’s magnetic field.

The effect of “soft-iron” distortion is to turn the ideal circles into ellipses which has the effect of altering the compass heading.

The solution to this problem is to scale the X and Y readings in such a way as to form perfect circles. This is achieved using the following code: [1]

  • chord_x = ((float)(mag_x_max - mag_x_min)) / 2;
  • chord_y = ((float)(mag_y_max - mag_y_min)) / 2;
  • chord_z = ((float)(mag_z_max - mag_z_min)) / 2;
  • chord_average = (chord_x + chord_y + chord_z) / 3;
  • Mag_x_scale = chord_average / chord_x;
  • Mag_y_scale = chord_average / chord_y;
  • Mag_z_scale = chord_average / chord_z;

The “calibrate_magnetometer()” function does this for you and MUST be run before you can use the compass. Theoretically this function is not required again unless you change your location.

Instructions for doing this are given in the “tilt_comp_compass.ino” code header.

Simply change the line “bool Record_data = false;” to read “bool Record_data = true;”, upload “tilt_comp_compass.ino” to your Arduino, then tumble the compass in all directions until a set of data values appear on your screen. This process will take about one minute.

Copy the screen values into the matching header positions, set “bool Record_data = false;”, upload “tilt_comp_compass.ino” once more and you are ready to go.

Calibrating the gyro

The gyro calibration is automatic and MUST be done each time you power-up the compass. [2]

Place the compass on a level surface and apply power ... a progress bar will appear on the LCD display after which the compass display will appear.

The compass display shows the:

  • Heading
  • Pitch
  • Roll

The pitch and roll headings should both indicate 000.0 +/- 0.1 when the calibration process is complete. If not then edit the following code lines in the main loop() until the residual pitch and roll readings are zero:

    • Accel_pitch -= -0.2f;
    • Accel_roll -= 1.1f;

    True (Geographic) North

    By default the compass indicates Magnetic North.

    The heading can be changed to True North by uncommenting the following code line that appears in the main loop():

    • // Heading += Declination;

    You will also have to replace the “Declination” value in the Arduino “header” with that of your own location.

    References

    [1]

    "Simple and Effective Magnetometer Calibration", Kris Winer, https://github.com/kriswiner/MPU6050/wiki/Simple-...

    [2]

    The gyro calibration assumes that a yaw angle of exactly 360 degrees is obtained when the MPU-9250 is rotated one full revolution.

    Assuming that the crystal (xtal) frequency in your Arduino UNO is exactly 16 MHz, then a main loop() time of 8000 us (microseconds) is required. Unfortunately my Arduino crystal frequency is off by about 5%. For this reason I have set my loop time to 8400 uS.

    Instructions for adjusting your gyro sample (loop) time are found at the end of the main “loop() function.

    The following (temporary) code will display the yaw reading on your Serial Monitor:

    • Serial.println(Mag_z);

    Step 6: Summary

    The tilt compensated compass:

    • Uses a single MPU-9250 chip that contains two integrated circuits within the same package.
    • The tilt and roll is determined by the internal MPU-6050 gyro / accelerometer chip.
    • The internal AK8963 magnetometer chip provides X,Y outputs from which the compass heading may be calculated.
    • The compass heading is only accurate if the magnetometer X,Y outputs are taken when the compass is level.
    • The pitch and roll values from the MPU-6050 gyro / accelerometer are used to cancel the variations in the magnetometer X,Y outputs due to tilt.
    • Once calibrated the resulting compass headings are accurate to within 2 degrees.
    • Tilt compensation can be disabled using a jumper between Arduino pin A0 and GND.
    • The Arduino I2C pull-up resistors should be disabled in the "Wire" library.
    • The Compass wiring should be connected after the "tilt_comp_compass.ino" code has been uploaded.
    • Violent shaking (rapid movement) can affect the gyro accuracy requiring a system reset.
    • Accordingly, do not use this compass in situations involving safety to life, such as navigation at sea.

      Click here   to view my other instructables.

    Arduino Contest 2019

    Participated in the
    Arduino Contest 2019

    1 Person Made This Project!

    Recommendations

    • Puzzles Speed Challenge

      Puzzles Speed Challenge
    • "Can't Touch This" Family Contest

      "Can't Touch This" Family Contest
    • CNC Contest 2020

      CNC Contest 2020

    27 Discussions

    0
    joerg1966
    joerg1966

    Question 13 days ago

    Hi, i get confused Data on all. Heading is verry bad. Roll and pitch are working not 100% For example I roll to 90, thats fine. Roll to -90 get 80 and 0 is than 10. When I screw the heading a little Roll goes back to 0. My Hardware is a uno (smd cpu) Latest Arduino (1.8.13) and the MPU Modul looks 100% like that shown here. Where can I start to search the mistake?

    0
    lingib
    lingib

    Answer 12 days ago

    Thank you for your interest in this project :)

    The symptoms that you are experiencing are likely to arise if your sensor is not properly calibrated.

    Calibrating a sensor is not an easy task .

    If you think of your sensor as a tennis-ball then each surface-point on the tennis ball must be pointed towards a fixed distant object for perfect calibration ... miss a few points and calibration will be out. The action of rotating your compass in all directions means that each of the compass XYZ axes will at some stage be aligned with the Earth's magnetic field at your particular location.

    Since writing this article I have written another compass https://www.instructables.com/id/Quaternion-Compa... that has better long-term stability. The good news is that the code is 100% compatible with you existing hardware ... no hardware changes are required.

    The calibration routines described in https://www.instructables.com/id/Quaternion-Compa... may be used to calibrate your Tilt Stabilised Compass should you wish

    Hopefully this will solve your problem.

    0
    joerg1966
    joerg1966

    Reply 6 days ago

    I tried out the Quaternion Compass and it works good. Problem is that when I move from 0 to 180 the compass shows 170, 90 shows 75 and 270 shows 255. Is that a Problem of calibration? Pitch and Roll is perfect.

    U used the libs from Adafruit. The original sample from Adafruit doesn't works like Your Tilt Compensated Compass. What did You change in code to original Adafruit?

    Thanks a lot for Your help.




    0
    lingib
    lingib

    Reply 5 days ago

    The code changes that I made are explained in detail in Step 3 of my instructable https://www.instructables.com/id/Quaternion-Compa... which says:

    " The problem disappeared when I modified the Sparkfun MahonyQuaternionUpdate() function to read:
    MahonyQuaternionUpdate( myIMU.ax, -myIMU.ay, myIMU.az,
    myIMU.gx * DEG_TO_RAD, -myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD,
    myIMU.my, -myIMU.mx, -myIMU.mz,
    myIMU.deltat); "

    As for your headings, there are a number of things that could be causing the errors:
    - calibration
    - sensor orientation
    - proximity of your sensor to something
    - magnetic radiation from say a TV.

    0
    joerg1966
    joerg1966

    Reply 5 days ago

    so now I made Your Quaternion Code running on VS Code with PlattformIO and a ESP32 Board without Problems. (I'm sorry, but I don't like the Arduino IDE.;-) )
    I printed a Cube to place the sensor and calibrated twice. That s works fine now. Now I have to put Your code in a separate file to integrate it to my application. Thanks a lot for Your help !!!

    IMG_20200809_215053.jpg
    0
    lingib
    lingib

    Reply 4 days ago

    Fantastic ... love your box :)
    And thanks for the feedback ... it's nice to know that my code works on another platform :)

    0
    joerg1966
    joerg1966

    Answer 12 days ago

    I found the "Quaternion Compass" Thats works, but the time compas change to right output is very long.

    0
    lingib
    lingib

    Reply 12 days ago

    The response time can be improved by changing the the K parameter ... but at the expense of overshoot.

    0
    elkomander
    elkomander

    Question 4 weeks ago on Step 6

    First of all, amazing tutorial! I have been able to follow from calibration to receiving compass heading. I am having difficulty getting an accurate compass heading, my assumption is that the calibration is not done correctly, but I am following the calibrate_mag() function and writing in my hard / soft iron values. In this doc, you state that Heading = atan2(Y, X) * RAD_TO_DEG; measuring the angle in ccw from the +x axis but in the code provided Heading = atan2(Y, X) * RAD_TO_DEG; measuring the angle in cw from the +y axis(North Direction?). Is this due to you wanting to measure the angle from the y(north) axis? Then, once you have this heading value calculated as is in the code, what direction do I point my mpu9250 towards for the correct orientation? I think you mention x axis is point up but does that mean up towards the sky in a 3d plane or up in a 2d plane? Once I have the heading value, how do I make this a readable compass direction and degree? Thanks for your help in advance!

    0
    lingib
    lingib

    Answer 4 weeks ago

    The MPU orientation is stated at the top of the main loop():
    /*
    --------------------
    MPU-9250 Orientation
    --------------------
    Component side up
    X-axis facing forward
    */

    The "X" refers to the silk-screen symbol on the breakout board

    Regarding your cw and ccw question ...I can find the Heading = atan2(Y, X) * RAD_TO_DEG; but I can't find the text that you quote.

    Message me with a "cut and paste" of the two code snippets and/or the line numbers and I will try and answer your question.

    0
    doubledutch2
    doubledutch2

    3 months ago

    Hi - Great project. I'm trying to get this to work with a Wemos Pro D1 and whilst everything "works" very quickly the heading changes to something like 214748 and it stays like that. I spotted on your other "calibration" post that there may be an issue with the speed of the Wemos and that I could change the Fusion Function but I'm not sure how to do that. Do you I have any suggestion around the above corrupted numbers? I have 3 MPU9250 sensors and they all do the same thing. Sometimes they work for a while but the heading is never stable - it changes rapidly and then this "overflow" situation occurs. Thanks in advance. I would really like to get this unit to work with my Wemos MPU

    TiltCompass.png
    0
    lingib
    lingib

    Reply 3 months ago

    Another user encountered problems when attempting to convert this project to an Adafruit M4 Feather Express https://www.adafruit.com/product/3857

    While this processor is Arduino compatible the cross-compiler he was using had some quirks. To resolve them he had to:

    (1) change all data-type references from "int" to "short"

    (2) "cast" the data type in some places ... especially when left-shifting data bytes to form a word. The M4 Feather express is a 32-bit processor, whereas the Arduino UNO R3 is a 16-bit processor, and his cross-compiler wasn't extending the Arduino two-byte two's-complement sign-bit correctly into the upper 16-bits of the M4 4-byte registers ... casting and short fixed this.

    I think he also had issues with the loop-timing when changing processor types.

    My code works with an Arduino UNO R3 Arduino. which suggests that you may be encountering a similar problems.

    0
    doubledutch2
    doubledutch2

    Reply 3 months ago

    Thanks for your reply. I managed to get somewhere using your amazing software. On the "tilt compass" I identified that this statement caused the issues using my 8266 Wemos D1 Mini - Pro:

    Accel_total_vector = sqrt((Accel_x * Accel_x) + (Accel_y * Accel_y) + (Accel_z * Accel_z));

    When moving the sensors some of Accelerator values (to be expected) went up a lot and I think that hit an upper limit in the sqrt function which returned invalid results. That meant that the value going into subsequent ASIN functions was not between -1 and 1 which then let to numerical exceptions.

    Having fixed that by checking the validity of the Accel_total_vector it worked but I then realized that this application did not include a lot of sampling or fusion functions and the results were unstable.

    I then moved to your other great project:

    https://www.instructables.com/id/Quaternion-Compass/

    which compiled on both 8266 and Arduino without any major changes. I changed:

    1) The LiquidCrystal driver - I always use the old one so changed to code it would work with that
    2) Removed the while(!Serial) statement (it crashes the 8266)

    I then calibrated the compass and your application now works on both the Arduino and 8266 to reasonable accuracy. Thanks for your help!

    0
    lingib
    lingib

    Reply 3 months ago

    Thank you for your feedback :)

    Well done in finding the square root issue. I used a "long" data type to avoid this happening with the Arduino. Validating the data is a neat solution.

    The while(!Serial) statement is required for the M4 Express to prevent a startup issue. I left it in because it doesn't affect an Arduino Uno R3. Some days you just can't win :)

    Glad that you like the Quaternion Compass :)

    0
    doubledutch2
    doubledutch2

    Reply 3 months ago

    I just uploaded your code to an Arduino Uno and it works MUCH better. Still need to calibrate it but the results are definetly much better. Do you have any idea why that is so I can see if I can try to fix this for the 8266 device?

    0
    lingib
    lingib

    Reply 3 months ago

    Unable to think of anything further to add to my previous comment.

    An alternate method of calibrating your compass is described in my instructable https://www.instructables.com/id/Quaternion-Compass/.

    No hardware changes are needed for the quaternion compass.

    0
    dirkbosmans
    dirkbosmans

    4 months ago

    Hi
    Lingib, I've built the project using an arduino duemilanove. The
    pitch & tilt seem to be working fine, the heading is however
    still uncompensated.
    No bridge was placed & the code is still suppressed for removing
    the tilt compensation calculation. I get the idea it might have to do
    with the calibration of the magnetometer, but I'm not that
    experienced in the matter. So in conclusion, great instructable, well
    documented, but unfortunately no success in function from my side.

    20200330_235521.jpg20200330_235348.jpg
    0
    dirkbosmans
    dirkbosmans

    Reply 4 months ago

    Hi Lingib, thanks for your swift response! I previously did the calibration with the sensor in position, it was glued, so I had not much of a choice :)

    I now did a test with the sensor 100mm further away from the LCD & motors because they can indeed give disturbance in general, resulting in....excellent readings! So thanks for the hint!

    I'm now starting to read your next instructable, having the hardware ready it's just a small step to test out that one as well. I'm curious to the difference between the response & process cycle time of both versions....

    IMG-20200331-WA0009.jpg
    0
    lingib
    lingib

    Reply 4 months ago

    The quaternion compass may appear slower as it includes damping.

    0
    lingib
    lingib

    Reply 4 months ago

    Thank you for your feedback :)

    You may find some clues in my latest instructable https://www.instructables.com/id/Quaternion-Compass/

    This instructable describes a simple method for calibrating your magnetometer :)

    Both compasses use the same circuit so it's just a matter of swapping the code and ensuring that the magnetometer is correctly orientated.

    It is important that you perform the calibration with the magnetometer in place otherwise the magnets in your motors will upset the heading.

    Good luck