Add the flight control task and start using FreeRTOS

*The flight control task should be separated into different parts to let the structure more clearly