Changed the distance calculation to stock

I've created a new function called getDistanceToWaypoint() to calculate
the current distance with the stock functionality