From 25127bf1a66d697b71b39b57a8d1738c1e2ae57b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Jul 2022 15:52:16 +1000 Subject: [PATCH] Plane: fixed combination of passby and acceptance dist WP when a user sets a passby distance we should calculate the turn point based on the extrapolated distance, not the original waypoint also simplify the passby logic using offset_bearing() --- ArduPlane/commands_logic.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 244ab38ed0..a4631886c4 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -617,13 +617,11 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp if (cmd_passby > 0) { - float dist = prev_WP_loc.get_distance(flex_next_WP_loc); + const float dist = prev_WP_loc.get_distance(flex_next_WP_loc); + const float bearing_deg = degrees(prev_WP_loc.get_bearing(flex_next_WP_loc)); - if (!is_zero(dist)) { - float factor = (dist + cmd_passby) / dist; - - flex_next_WP_loc.lat = flex_next_WP_loc.lat + (flex_next_WP_loc.lat - prev_WP_loc.lat) * (factor - 1.0f); - flex_next_WP_loc.lng = flex_next_WP_loc.lng + Location::diff_longitude(flex_next_WP_loc.lng,prev_WP_loc.lng) * (factor - 1.0f); + if (is_positive(dist)) { + flex_next_WP_loc.offset_bearing(bearing_deg, cmd_passby); } } @@ -653,8 +651,8 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } else if (cmd_passby == 0) { acceptance_distance_m = nav_controller->turn_distance(get_wp_radius(), auto_state.next_turn_angle); } - - if (auto_state.wp_distance <= acceptance_distance_m) { + const float wp_dist = current_loc.get_distance(flex_next_WP_loc); + if (wp_dist <= acceptance_distance_m) { gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um", (unsigned)mission.get_current_nav_cmd().index, (unsigned)current_loc.get_distance(flex_next_WP_loc));