AP_Rally: Move handling of home out of find_nearest_rally_point

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 &current_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 &current_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 &current_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;