Add controls, refactor autopilot part