PID dev, refactoring flight managment