Changed the distance calculation to stock

I've created a new function called getDistanceToWaypoint() to calculate
the current distance with the stock functionality
Merge pull request #2 from Peppie23/distance_calculation

Changed the distance calculation to stock