Thursday, 13 February 2020

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 ~~


  1. Hi I'm a newbie in this field.
    I'm studying MPU-9250 for making the drone which could be hovering.
    I use Arduino Due for fast processing.
    For now, I follow your modified arduino code. I checked Serial monitor.
    But I really want to check this IMU data on Processing program.
    If you are possible, please provide the related code to me.

    Thank you for your valuable information.

    1. Thanks for your comment. It's nice to know somebody is actually reading LOL. I uploaded my processing code to my github yesterday. The processing code is modified from the tutorial from DIY Hacking by Arvind Sanjeez as mentioned in the post. I only changed the serialEvent() function which gets the data from serial transmission because I found the original method unnecessarily tedious.The only trick lies in the statement split(trim(message)," ") as shown in this post. The second argument indicates the symbol used to separate the data in a single line. In my case, I use blank space to separate data so my second argument is " ", you could use any other symbol if you want. Anyway, my full code is in my github now for your reference. I hope I answered your question

    2. The link for my github is Check the processing folder

  2. We are reading brother : ) nice job you done here. Right now i am trying to figure out control algoritm which is little tricky for me