Fixed-wing UAV flight controller design

Hello everyone, I am a PhD student in the field of autonomous guidance of small UAVs.

I have done the design of the flight controller for small fixed wing UAV and evaluated it through HIL and comparing the simulation results with the HIL results.

Now I am going to implement the whole closed control loop using Stm32F429. I have used it within the HIL simulation setup.

I am looking for a guide or pseudocode for the flight controller to implement it within my processor. I get confused of the procedure of the control algorithm as we must check the mode, processing the measurements from the utilized sensors, and controlling the motors for changing the UAV attitudes.

Can you help me for building the flowchart of the whole flight control code.

Thank you in advance.