Refactoring and small fixes to flight control