From 7af888633dd93bd45dbf2ffdfeb7fe596f2cb55f Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 14 Mar 2016 11:43:13 -0700 Subject: [PATCH] Plane: 3of3 add loiter_xtrack option flag for post-loiter navigation 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location --- ArduPlane/commands_logic.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a7eb9dcb94..401b741d10 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1027,7 +1027,11 @@ bool Plane::verify_loiter_heading(bool init) if (labs(heading_err_cd) <= 1000 || loiter.sum_cd > loiter.total_cd) { // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp - next_WP_loc = current_loc; + + // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + if (next_WP_loc.flags.loiter_xtrack) { + next_WP_loc = current_loc; + } return true; } return false;