Autopilot code

fix bug