Fix bug where the AUTOPILOT state were never entered, add ability to plan new path during AUTOPILOT state