diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp index 33065c9b4b..3344a36a16 100644 --- a/libraries/AP_Rally/AP_Rally.cpp +++ b/libraries/AP_Rally/AP_Rally.cpp @@ -118,7 +118,6 @@ Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc) co bool AP_Rally::find_nearest_rally_point(const Location ¤t_loc, RallyLocation &return_loc) const { float min_dis = -1; - const struct Location &home_loc = _ahrs.get_home(); for (uint8_t i = 0; i < (uint8_t) _rally_point_total_count; i++) { RallyLocation next_rally; @@ -134,13 +133,8 @@ bool AP_Rally::find_nearest_rally_point(const Location ¤t_loc, RallyLocati } } - // if home is included, return false (meaning use home) if it is closer than all rally points - if (_rally_incl_home && (get_distance(current_loc, home_loc) < min_dis)) { - return false; - } - // if a limit is defined and all rally points are beyond that limit, use home if it is closer - if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f) && (get_distance(current_loc, home_loc) < min_dis)) { + if ((_rally_limit_km > 0) && (min_dis > _rally_limit_km*1000.0f)) { return false; // use home position } @@ -155,14 +149,17 @@ Location AP_Rally::calc_best_rally_or_home_location(const Location ¤t_loc, Location return_loc = {}; const struct Location &home_loc = _ahrs.get_home(); + // no valid rally point, return home position + return_loc = home_loc; + return_loc.alt = rtl_home_alt; + return_loc.flags.relative_alt = false; // read_alt_to_hold returns an absolute altitude + if (find_nearest_rally_point(current_loc, ral_loc)) { - // valid rally point found - return_loc = rally_location_to_location(ral_loc); - } else { - // no valid rally point, return home position - return_loc = home_loc; - return_loc.alt = rtl_home_alt; - return_loc.flags.relative_alt = false; // read_alt_to_hold returns an absolute altitude + Location loc = rally_location_to_location(ral_loc); + // use the rally point if it's closer then home, or we aren't generally considering home as acceptable + if (!_rally_incl_home || (get_distance(current_loc, loc) < get_distance(current_loc, return_loc))) { + return_loc = rally_location_to_location(ral_loc); + } } return return_loc;