Custom Flight Controller Part 2.3.5: Sensor Fusion --- (Optional) Processing Orientation Visualization

A test app is implemented to test the sensor fusion. Note that test app resides in the STM32 Processing Simulation folder in my github repository, separate from the main flight controller code. To use the test app, just download it into STM32 and run it. 

The overall structure of the test app 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. 

The last step is to visualize the orientation to check whether the orientation calculated correspond to the real orientation. Previously, Matlab was used which proved to be slow. Therefore, processing is used this time. It has been a popular choice for orientation visualization and there are a lot of ready-to-use packages to aid the development. I have chosen to follow the steps indicated in tutorial from DIY Hacking by Arvind Sanjeez.

The serial function inside the processing code provided by the tutorial has to be modified however to interface with serial communication sent from STM32. So I have configured STM32 to send quaternion in the following format. 

For example, if the quaternion is [1 0 0 0], it would be printed as "Q: 1 0 0 0". 

Character "Q:" can be used to check whether the data received in processing is correct. To extract the quaternion from the string received. The following code is implemented.

functions like split() and trim() are inbuilt processing functions. Their documentation can be readily found on processing's website. 

I have to admit that I did not study deep into processing due to time constraint. Therefore, I am unable to explain every line of the code that are provided by the aforementioned tutorial. There might be some modification and optimization that needs to be done. Anyway, the code works. 

To run the overall simulation, 

1. Run the program on STM32 from IAR embedded workbench

2. Use a serial terminal such as Putty and follow the instructions to calibrate magnetometer and collect initial values.

3. Close Putty (Otherwise a port conflict will happen) and run the processing script to visualize orientation.

The result is shown in the video below.

The orientation visualization is much better than that previously. Yaw, pitch, roll as well as arbitrary rotation all yield pretty good results.

But it still remains to be proven that this could be implemented on to a real quadcopter. But for that, control systems have to be designed and programmed first. So this finally concludes Part 2 --- Getting the orientation. Future posts will be about Part 3 --- Control System.

A big thank you to everyone who has supported and helped me so far ~~