Fixed the integral and derivative part in the pid controller of the sample autopilot
The sample autopilot considers that it has reached a waypoint when it is within 200 meters from it