AC_WPNav: bug fix for track covered being 2D

Replaced abs and fabs with fabsf
Added get_destination method