Fixed several AutoPilot control issues and improved flight stability. Optimized heading control and path tracking for smoother and more reliable automatic flight.