[navigation] precision route fix

When flying to a waypoint in route mode, the uav not accurately get there. This was caused by an inaccurate calculation in the navigation. This fixes that problem.