Fix map bug adding points in the wrong direction
Add waypoint heading and distance to nav map