From dd4b3295f49f6404742439f34f549dd6d5b7d942 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Jul 2021 17:24:08 +0900 Subject: [PATCH] AC_Avoidance: BendyRuler returned destination are shortened --- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 5331996fef..e1aaba7e4f 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -141,6 +141,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location // search in OA_BENDYRULER_BEARING_INC degree increments around the vehicle alternating left // and right. For each direction check if vehicle would avoid all obstacles float best_bearing = bearing_to_dest; + float best_bearing_margin = -FLT_MAX; bool have_best_bearing = false; float best_margin = -FLT_MAX; float best_margin_bearing = best_bearing; @@ -173,11 +174,13 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location // now check in there is a clear path in three directions towards the destination if (!have_best_bearing) { best_bearing = bearing_test; + best_bearing_margin = margin; have_best_bearing = true; } else if (fabsf(wrap_180(ground_course_deg - bearing_test)) < fabsf(wrap_180(ground_course_deg - best_bearing))) { // replace bearing with one that is closer to our current ground course best_bearing = bearing_test; + best_bearing_margin = margin; } // perform second stage test in three directions looking for obstacles @@ -202,7 +205,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location // all good, now project in the chosen direction by the full distance destination_new = current_loc; - destination_new.offset_bearing(final_bearing, distance_to_dest); + destination_new.offset_bearing(final_bearing, MIN(distance_to_dest, lookahead_step1_dist)); _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f); Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new); return active; @@ -213,21 +216,24 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location } float chosen_bearing; + float chosen_distance; if (have_best_bearing) { // none of the directions tested were OK for 2-step checks. Choose the direction // that was best for the first step chosen_bearing = best_bearing; + chosen_distance = MAX(lookahead_step1_dist + MIN(best_bearing_margin, 0), 0); _current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f); } else { // none of the possible paths had a positive margin. Choose // the one with the highest margin chosen_bearing = best_margin_bearing; + chosen_distance = MAX(lookahead_step1_dist + MIN(best_margin, 0), 0); _current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f); } // calculate new target based on best effort destination_new = current_loc; - destination_new.offset_bearing(chosen_bearing, distance_to_dest); + destination_new.offset_bearing(chosen_bearing, chosen_distance); // log results Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);