dtu_smr_wptnav: Waypoint navigation tested and updated.