Reverted navigation.c worke, added current waypoint status report to qgroundcontrol