Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: BendyRuler returned destination are shortened

Browse Source
This commit is contained in:
Randy Mackay 2021-07-14 17:24:08 +09:00
parent faadaafd53
commit dd4b3295f4
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -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 // 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 // and right. For each direction check if vehicle would avoid all obstacles
float best_bearing = bearing_to_dest; float best_bearing = bearing_to_dest;
float best_bearing_margin = -FLT_MAX;
bool have_best_bearing = false; bool have_best_bearing = false;
float best_margin = -FLT_MAX; float best_margin = -FLT_MAX;
float best_margin_bearing = best_bearing; 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 // now check in there is a clear path in three directions towards the destination
if (!have_best_bearing) { if (!have_best_bearing) {
best_bearing = bearing_test; best_bearing = bearing_test;
best_bearing_margin = margin;
have_best_bearing = true; have_best_bearing = true;
} else if (fabsf(wrap_180(ground_course_deg - bearing_test)) < } else if (fabsf(wrap_180(ground_course_deg - bearing_test)) <
fabsf(wrap_180(ground_course_deg - best_bearing))) { fabsf(wrap_180(ground_course_deg - best_bearing))) {
// replace bearing with one that is closer to our current ground course // replace bearing with one that is closer to our current ground course
best_bearing = bearing_test; best_bearing = bearing_test;
best_bearing_margin = margin;
} }
// perform second stage test in three directions looking for obstacles // 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 // all good, now project in the chosen direction by the full distance
destination_new = current_loc; 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); _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); Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new);
return active; return active;
@ -213,21 +216,24 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
} }
float chosen_bearing; float chosen_bearing;
float chosen_distance;
if (have_best_bearing) { if (have_best_bearing) {
// none of the directions tested were OK for 2-step checks. Choose the direction // none of the directions tested were OK for 2-step checks. Choose the direction
// that was best for the first step // that was best for the first step
chosen_bearing = best_bearing; chosen_bearing = best_bearing;
chosen_distance = MAX(lookahead_step1_dist + MIN(best_bearing_margin, 0), 0);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f); _current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f);
} else { } else {
// none of the possible paths had a positive margin. Choose // none of the possible paths had a positive margin. Choose
// the one with the highest margin // the one with the highest margin
chosen_bearing = best_margin_bearing; 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); _current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f);
} }
// calculate new target based on best effort // calculate new target based on best effort
destination_new = current_loc; destination_new = current_loc;
destination_new.offset_bearing(chosen_bearing, distance_to_dest); destination_new.offset_bearing(chosen_bearing, chosen_distance);
// log results // log results
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new); Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);