Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: bendy ruler format fixes

This commit is contained in:
Randy Mackay 2021-07-14 12:59:27 +09:00
parent 0b26bace7f
commit 075c05baea
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -113,7 +113,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
} else { } else {
ground_course_deg = degrees(ground_speed_vec.angle()); ground_course_deg = degrees(ground_speed_vec.angle());
} }
bool ret; bool ret;
switch (get_type()) { switch (get_type()) {
case OABendyType::OA_BENDY_VERTICAL: case OABendyType::OA_BENDY_VERTICAL:
@ -128,7 +128,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only); ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only);
bendy_type = OABendyType::OA_BENDY_HORIZONTAL; bendy_type = OABendyType::OA_BENDY_HORIZONTAL;
} }
return ret; return ret;
} }
@ -236,7 +236,7 @@ bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location
} }
// Search for path in the vertical directions // Search for path in the vertical directions
bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest, bool proximity_only) bool AP_OABendyRuler::search_vertical_path(const Location &current_loc, const Location &destination, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only)
{ {
// check OA_BEARING_INC_VERTICAL definition allows checking in all directions // check OA_BEARING_INC_VERTICAL definition allows checking in all directions
static_assert(360 % OA_BENDYRULER_BEARING_INC_VERTICAL == 0, "check 360 is a multiple of OA_BEARING_INC_VERTICAL"); static_assert(360 % OA_BENDYRULER_BEARING_INC_VERTICAL == 0, "check 360 is a multiple of OA_BEARING_INC_VERTICAL");
@ -285,7 +285,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Lo
for (uint8_t j = 0; j < ARRAY_SIZE(test_pitch_step2); j++) { for (uint8_t j = 0; j < ARRAY_SIZE(test_pitch_step2); j++) {
float bearing_test2 = wrap_180(test_pitch_step2[j]); float bearing_test2 = wrap_180(test_pitch_step2[j]);
Location test_loc2 = test_loc; Location test_loc2 = test_loc;
test_loc2.offset_bearing_and_pitch(bearing_to_dest2, bearing_test2 ,distance2); test_loc2.offset_bearing_and_pitch(bearing_to_dest2, bearing_test2, distance2);
// calculate minimum margin to fence and obstacles for this scenario // calculate minimum margin to fence and obstacles for this scenario
float margin2 = calc_avoidance_margin(test_loc, test_loc2, proximity_only); float margin2 = calc_avoidance_margin(test_loc, test_loc2, proximity_only);
@ -309,7 +309,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Lo
} }
// project in the chosen direction by the full distance // project in the chosen direction by the full distance
destination_new = current_loc; destination_new = current_loc;
destination_new.offset_bearing_and_pitch(bearing_to_dest,pitch_delta, distance_to_dest); destination_new.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f); _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new); Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);

2
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -40,7 +40,7 @@ private:
bool search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step_1_dist, float lookahead_step_2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only); bool search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step_1_dist, float lookahead_step_2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only);
// search for path in the Vertical directions // search for path in the Vertical directions
bool search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest, bool proximity_only); bool search_vertical_path(const Location &current_loc, const Location &destination, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest, bool proximity_only);
// calculate minimum distance between a path and any obstacle // calculate minimum distance between a path and any obstacle
float calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const; float calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const;