Custom Flight Controller Part 2.4: (Optional) Mag Calibration

This post explains how to perform mag calibration. The detailed code can be found in my Github. Note that in my final implementation, magnetometer is not used as it makes the attitude state estimation a lot noisier. So this post merely serves the purpose of documentation.

Magnetometer measures the direction and magnitude of magnetic field present. It is a great tool to correct yaw measurement. However, it needs calibration before it can be used to update orientation. For example, if a uncalibrated magnetometer measures 100 milliGause in x direction currently, rotating the magnetometer around z axis for 180 degrees should theoretically yield -100 milliGauss. But since the magnetometer is uncalibrated, it might give you a value like -50 millGauss. In this case, a -25 milliGauss bias should be added to the magnetometer x axis measurement so that the two measurements become +75 and -75 and are just opposite to each other.  This procedure of determining the magnetometer bias is done via magnetometer calibration. 

The method proposed in this post by Kris Winer is adopted to calibrate magnetometer in this project. Essentially, user needs to rotate the magnetometer non-stop during the calibration until the magnetomer has collected enough sample data. Then the bias of an axis is calculated as the average value of the minimum value and the maximum value measured on that axis.

The code is shown below. It is very similar to the code provided by the Kris Winer. The code is grouped under a function called calibrateMag();

After getting the magnetometer biases, these biases need to be subtracted away from each magnetometer measurement as shown below.

Other than mag calibration, there are one more task that need to be done before running orientation calculation, namely, getting the initial measurement value of magnetometer and accelerometer.

In order to calculate orientation, one need to know the sensor measurements when the copter is lying on the ground. These initial values can not be known theoretically because of the ever-changing environment so they have to be obtained experimentally. Therefore, the program will ask user to put the MPU9250 to rest after magnetometer calibration. During this period of time the program will collect 100 sets of data from MPU9250 and the average of these data is stored as the initial measurements. The code is reproduced below.

As seen above, after the initial values of the three sensors are collected and calculated, the initDataRdy flag is  set, allowing the program to run Quaternion Kalman Filter to calculate orientation.