autopilot almost works, searching for sonar bug