Fix auto pilot
Improve auto pilot test