Do not stop at the first rally point close enough if invalid

src/modules/navigator/rtl.cpp

@@ -441,20 +441,21 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
 				continue;
 			}
 
+			PositionYawSetpoint safepoint_position{};
+
+			if (!extractValidSafePointPosition(mission_safe_point, _home_pos_sub.get().alt, safepoint_position)) {
+				continue;
+			}
+
 			// Ignore safepoints which are too close to the homepoint (only if home is an option to return to)
 			const bool far_from_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
-						   mission_safe_point.lat, mission_safe_point.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES;
+						   safepoint_position.lat, safepoint_position.lon) > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES;
 
 			if (far_from_home || (_param_rtl_type.get() == 5)) {
-				const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
+				const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
+						 safepoint_position.lat, safepoint_position.lon)};
 
-				PositionYawSetpoint safepoint_position;
+				const bool current_safe_point_has_approaches{hasVtolLandApproachesAtSafePointIndex(current_seq, _home_pos_sub.get().alt)};
-
-				if (!extractValidSafePointPosition(mission_safe_point, _home_pos_sub.get().alt, safepoint_position)) {
-					continue;
-				}
-
-				const bool current_safe_point_has_approaches{hasVtolLandApproach(current_seq, _home_pos_sub.get().alt)};
 
 				_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
 
@@ -482,7 +483,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 	float min_dist = FLT_MAX;
 
 	if (_param_rtl_type.get() != 5) {
-		_home_has_land_approach = hasVtolLandApproach(destination, _home_pos_sub.get().alt);
+		_home_has_land_approach = hasVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
 
 		const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode);
 		const bool required_approach_missing_for_home = (vtol_in_fw_mode && (_param_rtl_appr_force.get() == 1) && !_home_has_land_approach);
@@ -703,14 +704,14 @@ bool RTL::reverseIsFurther() const
 }
 
 
-bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index,
+bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
-				       mission_item_s &safe_point_item) const
+				       int &safe_point_index, mission_item_s &safe_point_item) const
 {
 	if (!_safe_points_updated) {
 		return false;
 	}
 
-	// Find the first rally point within the home-association distance
+	// The first nearby valid rally point defines the block that belongs to this destination.
 	for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
 		mission_item_s mission_item{};
 
@@ -726,7 +727,14 @@ bool RTL::findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position,
 			continue;
 		}
 
-		const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon,
+		PositionYawSetpoint safe_point_position{};
+
+		// Skip invalid rally points so later valid rally points are considered
+		if (!extractValidSafePointPosition(mission_item, home_altitude_amsl, safe_point_position)) {
+			continue;;
+		}
+
+		const float dist_to_safepoint = get_distance_to_next_waypoint(safe_point_position.lat, safe_point_position.lon,
 						rtl_position.lat, rtl_position.lon);
 
 		if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
@@ -756,6 +764,7 @@ bool RTL::scanVtolLandApproachBlock(int safe_point_index, float home_altitude_am
 		}
 
 		if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
+			// A rally point starts the next block, break
 			break;
 		}
 
@@ -790,37 +799,32 @@ loiter_point_s RTL::makeVtolLandApproachPoint(const mission_item_s &mission_item
 	return approach;
 }
 
-land_approaches_s RTL::readVtolLandApproaches(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
+land_approaches_s RTL::getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
+		float home_altitude_amsl) const
 {
 	int safe_point_index = -1;
 	mission_item_s safe_point_item{};
 
-	if (!findAssociatedSafePointIndex(rtl_position, safe_point_index, safe_point_item)) {
+	if (!findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item)) {
-		return {};
-	}
-
-	PositionYawSetpoint safe_point_position{};
-
-	if (!extractValidSafePointPosition(safe_point_item, home_altitude_amsl, safe_point_position)) {
 		return {};
 	}
 
 	land_approaches_s vtol_land_approaches{};
-	vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_position.lat, safe_point_position.lon);
+	vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(safe_point_item.lat, safe_point_item.lon);
 	scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, &vtol_land_approaches);
 	return vtol_land_approaches;
 }
 
-bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
+bool RTL::hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const
 {
 	int safe_point_index = -1;
 	mission_item_s safe_point_item{};
 
-	return findAssociatedSafePointIndex(rtl_position, safe_point_index, safe_point_item)
+	return findAssociatedSafePointIndex(rtl_position, home_altitude_amsl, safe_point_index, safe_point_item)
-	       && hasVtolLandApproach(safe_point_index, home_altitude_amsl);
+	       && hasVtolLandApproachesAtSafePointIndex(safe_point_index, home_altitude_amsl);
 }
 
-bool RTL::hasVtolLandApproach(int safe_point_index, float home_altitude_amsl) const
+bool RTL::hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const
 {
 	return scanVtolLandApproachBlock(safe_point_index, home_altitude_amsl, nullptr);
 }
@@ -835,7 +839,7 @@ loiter_point_s RTL::selectLandingApproach(const PositionYawSetpoint &destination
 		return landing_approach;
 	}
 
-	const land_approaches_s vtol_land_approaches = readVtolLandApproaches(destination, _home_pos_sub.get().alt);
+	const land_approaches_s vtol_land_approaches = getVtolLandApproachesNearLocation(destination, _home_pos_sub.get().alt);
 
 	if (vtol_land_approaches.isAnyApproachValid()) {
 		landing_approach = chooseBestLandingApproach(vtol_land_approaches);

src/modules/navigator/rtl.h

@@ -177,44 +177,51 @@ private:
 	void parameters_update();
 
 	/**
-	 * @brief Read all VTOL landing approaches associated with rtl_position.
+	 * @brief Read the landing-approach block associated with the first valid rally point near rtl_position.
-	 */
-	land_approaches_s readVtolLandApproaches(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
-
-	/**
-	 * @brief Return whether rtl_position has at least one valid VTOL landing approach.
-	 */
-	bool hasVtolLandApproach(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
-
-	/**
-	 * @brief Return whether the indexed safe point has at least one valid VTOL landing approach.
-	 */
-	bool hasVtolLandApproach(int safe_point_index, float home_altitude_amsl) const;
-
-	/**
-	 * @brief Choose best landing approach
 	 *
-	 * Choose the wind-aligned landing approach for the associated land location.
+	 * A block starts at the associated rally point and contains the consecutive NAV_CMD_LOITER_TO_ALT
+	 * items that follow it. The next rally point starts a new block.
+	 * Invalid rally points are skipped so a later nearby valid rally point can still be considered.
+	 */
+	land_approaches_s getVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position,
+			float home_altitude_amsl) const;
+
+	/**
+	 * @brief Return whether a valid associated block near rtl_position has at least one valid approach.
+	 */
+	bool hasVtolLandApproachesNearLocation(const PositionYawSetpoint &rtl_position, float home_altitude_amsl) const;
+
+	/**
+	 * @brief Return whether the block after safe_point_index contains at least one valid approach.
+	 */
+	bool hasVtolLandApproachesAtSafePointIndex(int safe_point_index, float home_altitude_amsl) const;
+
+	/**
+	 * @brief Choose the most wind-aligned approach in a landing-approach block.
 	 *
-	 * @return loiter_point_s best landing approach
+	 * Bearings are evaluated from the block's land location.
 	 */
 	loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches) const;
 
 	/**
-	 * @brief Return the wind-selected VTOL landing approach for a destination, or an invalid loiter if none exists.
+	 * @brief Return the wind-selected VTOL approach for destination, or an invalid loiter if none exists.
 	 */
 	loiter_point_s selectLandingApproach(const PositionYawSetpoint &destination) const;
 
 	/**
-	 * @brief Find the dataman index of the first rally point within association distance of rtl_position.
+	 * @brief Find the first rally point whose block should be associated with rtl_position.
 	 *
-	 * On success, safe_point_index and safe_point_item are populated with the found item.
+	 * Invalid rally points are skipped so nearby valid fallbacks can still be associated.
+	 * On success, safe_point_index and safe_point_item are populated with the rally point that starts the block.
 	 */
-	bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, int &safe_point_index,
+	bool findAssociatedSafePointIndex(const PositionYawSetpoint &rtl_position, float home_altitude_amsl,
-					  mission_item_s &safe_point_item) const;
+					  int &safe_point_index, mission_item_s &safe_point_item) const;
 
 	/**
-	 * @brief Scan the loiter-to-alt block following a safe point.
+	 * @brief Scan one landing-approach block after a rally point.
+	 *
+	 * A block is the consecutive NAV_CMD_LOITER_TO_ALT items after safe_point_index.
+	 * Scanning stops at the next rally point because it starts a different safe-point block.
 	 *
 	 * If result is non-null, all valid approaches are collected into it.
 	 * If result is null, returns true on the first valid approach (early exit).
@@ -224,6 +231,9 @@ private:
 	bool scanVtolLandApproachBlock(int safe_point_index, float home_altitude_amsl,
 				       land_approaches_s *result) const;
 
+	/**
+	 * @brief Convert one loiter mission item into a landing-approach entry.
+	 */
 	loiter_point_s makeVtolLandApproachPoint(const mission_item_s &mission_item, float home_altitude_amsl) const;
 
 	enum class DatamanState {