moved waypoint navigation functions to navdata.c, revised functions use inertial data