Fixed waypoint reach detection bug that used interpolator rather than target as criteria
removed unused diff_z variable