Autopilot PID fixes