Custom Flight Controller Part 2.7: Getting the Orientation --- Mag Calibration and Overall Algorithm Structure.

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.

The overall main.c structure can be summarized as follows:
Step 1. Initialize STM32 clock and peripherals
Step 2. Check MPU9250 is accessible and calibrate Magnetometer
Step 3. Start infinite loop
Step 4. (Inside loop) check whether initial data has been collected, if not, collect data. 
Step 5. (Inside loop)continue collecting data for initial data calculation until 100 sets of data has been collected.
Step 6. (Inside loop) Calculate the initial data and set initial data ready flag.
Step 7. (Inside loop) Get data from MPU9250 and calculate device orientation using QKF.

The resultant quaternion, indicating the current attitude is published via serial terminal to my laptop. The next post will explain how to use processing to read the published data and visualize the attitude of MPU9250. 

Comments

Popular posts from this blog

Custom Flight Controller Part 2.2: Getting the Orientation --- The Quaternion Kalman Filter

Custom Flight Controller Part 2.1: Getting the Orientation --- Get Data from MPU9250 Sensor Module

Custom Flight Controller Part 2.4: Getting the Orientation --- Migrate to STM32