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

AC_Avoidance: BendyRuler returns which type was used

Browse Source 
also make serach_xxx_path methods private
This commit is contained in:
Randy Mackay 2021-03-22 13:13:23 +09:00 committed by Andrew Tridgell
parent 7084009454
commit 6b74dd2ab4
2 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -79,11 +79,15 @@ AP_OABendyRuler::AP_OABendyRuler()
// run background task to find best path and update avoidance_results
// returns true and updates origin_new and destination_new if a best path has been found
bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, bool proximity_only)
{
// bendy_type is set to the type of BendyRuler used
bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, OABendyType &bendy_type, bool proximity_only)
{
// bendy ruler always sets origin to current_loc
origin_new = current_loc;
// init bendy_type returned
bendy_type = OABendyType::OA_BENDY_DISABLED;
// calculate bearing and distance to final destination
const float bearing_to_dest = current_loc.get_bearing_to(destination) * 0.01f;
const float distance_to_dest = current_loc.get_distance(destination);
@ -115,12 +119,14 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
case OABendyType::OA_BENDY_VERTICAL:
#if VERTICAL_ENABLED
ret = search_vertical_path(current_loc, destination, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest, proximity_only);
bendy_type = OABendyType::OA_BENDY_VERTICAL;
break;
#endif
case OABendyType::OA_BENDY_HORIZONTAL:
default:
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;
}
return ret;

18
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -19,16 +19,20 @@ public:
// send configuration info stored in front end parameters
void set_config(float margin_max) { _margin_max = MAX(margin_max, 0.0f); }
// run background task to find best path and update avoidance_results
// returns true and populates origin_new and destination_new if OA is required. returns false if OA is not required
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, bool proximity_only);
enum class OABendyType {
OA_BENDY_DISABLED = 0,
OA_BENDY_HORIZONTAL = 1,
OA_BENDY_VERTICAL = 2,
};
// run background task to find best path and update avoidance_results
// returns true and populates origin_new and destination_new if OA is required. returns false if OA is not required
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, OABendyType &bendy_type, bool proximity_only);
static const struct AP_Param::GroupInfo var_info[];
private:
// return type of BendyRuler in use
OABendyType get_type() const;
@ -36,11 +40,7 @@ public:
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
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);
static const struct AP_Param::GroupInfo var_info[];
private:
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);
// calculate minimum distance between a path and any obstacle
float calc_avoidance_margin(const Location &start, const Location &end, bool proximity_only) const;