fix(navigator): harden RTL route data handling

src/modules/navigator/mission_route_cache.cpp

@@ -129,8 +129,11 @@ void MissionRouteCache::resetSafePointCacheState(bool clear_source_identity)
 	_dataman_cache_safepoint.invalidate();
 
 	const uint32_t source_id = clear_source_identity ? 0 : _safe_point.source_id;
+	const uint8_t source_dataman_id = clear_source_identity ? static_cast<uint8_t>(DM_KEY_SAFE_POINTS_0) :
+					  _safe_point.source_dataman_id;
 	_safe_point = {};
 	_safe_point.source_id = source_id;
+	_safe_point.source_dataman_id = source_dataman_id;
 }
 
 int MissionRouteCache::safePointCount() const
@@ -175,7 +178,10 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission)
 	MissionLandState &state = _mission_land;
 	const hrt_abstime now = hrt_absolute_time();
 	// Trust the published land_index, no mission rescanning.
-	const int32_t land_index = (mission.land_index >= 0 && mission.land_index < mission.count) ? mission.land_index : -1;
+	const bool valid_dataman_id = mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0
+				      || mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1;
+	const bool valid_land_index = mission.land_index >= 0 && mission.land_index < mission.count;
+	const int32_t land_index = (valid_dataman_id && valid_land_index) ? mission.land_index : -1;
 	const bool mission_land_changed = mission.mission_id != state.mission_id
 					  || mission.count != state.count
 					  || mission.mission_dataman_id != state.dataman_id
@@ -189,6 +195,10 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission)
 		state.count = mission.count;
 		_dataman_cache_land_item.invalidate();
 
+		if (valid_land_index && !valid_dataman_id) {
+			PX4_ERR("Mission land cache rejected invalid dataman id: %" PRIu8, mission.mission_dataman_id);
+		}
+
 		if (state.index >= 0) {
 			state.validation_pending = queueMissionLandItem();
 
@@ -257,9 +267,10 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
 	SafePointState &state = _safe_point;
 	bool success = false;
 
-	if (mission.safe_points_id != state.source_id) {
-		// A new safe_points_id makes any in-flight read and cached items stale.
+	if (mission.safe_points_id != state.source_id || mission.safepoint_dataman_id != state.source_dataman_id) {
+		// A new safe-point source makes any in-flight read and cached items stale.
 		state.source_id = mission.safe_points_id;
+		state.source_dataman_id = mission.safepoint_dataman_id;
 		resetSafePointCacheState(false);
 	}
 
@@ -305,7 +316,24 @@ void MissionRouteCache::updateSafePointCache(const mission_s &mission)
 			break;
 		}
 
-		if (state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_0 && state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_1) {
+		if (state.read_stats.opaque_id != state.source_id) {
+			PX4_WARN("Safe point state id mismatch, expected %" PRIu32 ", got %" PRIu32,
+				 state.source_id, state.read_stats.opaque_id);
+			state.error_state = SafePointDatamanState::kReadWait;
+			state.dataman_state = SafePointDatamanState::kError;
+			break;
+		}
+
+		if (state.read_stats.num_items > 0 && state.read_stats.dataman_id != state.source_dataman_id) {
+			PX4_WARN("Safe point dataman id mismatch, expected %" PRIu8 ", got %" PRIu8,
+				 state.source_dataman_id, state.read_stats.dataman_id);
+			state.error_state = SafePointDatamanState::kReadWait;
+			state.dataman_state = SafePointDatamanState::kError;
+			break;
+		}
+
+		if (state.read_stats.num_items > 0 && state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_0
+		    && state.read_stats.dataman_id != DM_KEY_SAFE_POINTS_1) {
 			PX4_ERR("Safe points update failed! invalid dataman id: %" PRIu8, state.read_stats.dataman_id);
 			state.error_state = SafePointDatamanState::kReadWait;
 			state.dataman_state = SafePointDatamanState::kError;

src/modules/navigator/mission_route_cache.h

@@ -91,6 +91,8 @@ public:
 		       && !_mission_land.ready
 		       && (_mission_land.validation_pending || _mission_land.retry.retry_at != 0);
 	}
+	/** True after queuing, reading, or validating the current mission land item failed. */
+	bool missionLandItemAttemptFailed() const { return _mission_land.retry.retry_count > 0; }
 
 	int safePointCount() const override;
 	bool loadSafePointItem(int index, mission_item_s &safe_point_item) const override;
@@ -148,6 +150,7 @@ private:
 		mission_stats_entry_s read_stats{};   ///< Async DM_KEY_SAFE_POINTS_STATE read buffer.
 		mission_stats_entry_s cached_stats{}; ///< Identity of the safe-point set currently in the RAM cache.
 		uint32_t source_id{0};                ///< Active mission.safe_points_id being tracked.
+		uint8_t source_dataman_id{DM_KEY_SAFE_POINTS_0}; ///< Active mission.safepoint_dataman_id being tracked.
 		uint32_t cached_source_id{0};         ///< mission.safe_points_id the RAM cache was built for.
 		bool ready{false};
 		bool update_requested{true};

src/modules/navigator/mission_route_types.cpp

@@ -115,10 +115,17 @@ bool copyPositionToYawSetpoint(const Position &position, PositionYawSetpoint &se
 
 loiter_point_s makeVtolLandApproachPoint(const mission_item_s &mission_item, float home_altitude_amsl)
 {
+	const Position position{mission_item.lat, mission_item.lon,
+				getAbsoluteAltitudeForMissionItem(mission_item, home_altitude_amsl)};
 	loiter_point_s approach{};
-	approach.lat = mission_item.lat;
-	approach.lon = mission_item.lon;
-	approach.height_m = getAbsoluteAltitudeForMissionItem(mission_item, home_altitude_amsl);
+
+	if (!position.valid() || !PX4_ISFINITE(mission_item.loiter_radius)) {
+		return approach;
+	}
+
+	approach.lat = position.lat;
+	approach.lon = position.lon;
+	approach.height_m = position.alt;
 	approach.loiter_radius_m = mission_item.loiter_radius;
 	return approach;
 }

src/modules/navigator/rtl.cpp

@@ -280,7 +280,7 @@ void RTL::setRtlTypeAndDestination()
 		if (hasMissionLandStart()) {
 			new_rtl_type = RtlType::RTL_MISSION_FAST;
 
-		} else if (_navigator->get_mission_result()->valid) {
+		} else if (hasValidMission()) {
 			new_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
 
 		} else {
@@ -292,7 +292,7 @@ void RTL::setRtlTypeAndDestination()
 		if (hasMissionLandStart() && reverseIsFurther()) {
 			new_rtl_type = RtlType::RTL_MISSION_FAST;
 
-		} else if (_navigator->get_mission_result()->valid) {
+		} else if (hasValidMission()) {
 			new_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
 
 		} else {
@@ -480,13 +480,31 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 			const bool success = mission_route_cache.getMissionLandItem(land_index, land_mission_item);
 
 			if (!success) {
+				const mission_s &mission = _mission_sub.get();
+				const bool same_reported_source = _mission_land_failure_reported
+								  && _mission_land_failure_mission_id == mission.mission_id
+								  && _mission_land_failure_count == mission.count
+								  && _mission_land_failure_index == mission.land_index
+								  && _mission_land_failure_dataman_id == mission.mission_dataman_id;
 
-				/* Not supposed to happen unless the datamanager can't access the SD card, etc. */
-				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t");
-				events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error,
-					     "Mission land item could not be read");
+				if (!same_reported_source) {
+					_mission_land_failure_reported = false;
+				}
+
+				if ((!mission_route_cache.missionLandItemUpdatePending()
+				     || mission_route_cache.missionLandItemAttemptFailed()) && !_mission_land_failure_reported) {
+					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t");
+					events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error,
+						     "Mission land item could not be read");
+					_mission_land_failure_mission_id = mission.mission_id;
+					_mission_land_failure_count = mission.count;
+					_mission_land_failure_index = mission.land_index;
+					_mission_land_failure_dataman_id = mission.mission_dataman_id;
+					_mission_land_failure_reported = true;
+				}
 
 			} else {
+				_mission_land_failure_reported = false;
 				const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)};
 
 				if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
@@ -582,7 +600,7 @@ void RTL::initRtlMissionType(RtlType new_rtl_type, float rtl_alt)
 
 	switch (new_rtl_type) {
 	case RtlType::RTL_DIRECT_MISSION_LAND:
-		_rtl_mission_type_handle = new RtlDirectMissionLand(_navigator);
+		_rtl_mission_type_handle = new RtlDirectMissionLand(_navigator, new_mission);
 
 		if (_rtl_mission_type_handle) {
 			_rtl_mission_type_handle->setRtlAlt(rtl_alt);
@@ -634,7 +652,16 @@ void RTL::parameters_update()
 bool RTL::hasMissionLandStart() const
 {
 	return _mission_sub.get().land_start_index >= 0 && _mission_sub.get().land_index >= 0
-	       && _navigator->get_mission_result()->valid;
+	       && hasValidMission();
+}
+
+bool RTL::hasValidMission() const
+{
+	const mission_result_s &mission_result = *_navigator->get_mission_result();
+	return mission_result.valid
+	       && mission_result.mission_id == _mission_sub.get().mission_id
+	       && mission_result.geofence_id == _mission_sub.get().geofence_id
+	       && mission_result.home_position_counter == _home_pos_sub.get().update_count;
 }
 
 bool RTL::reverseIsFurther() const

src/modules/navigator/rtl.h

@@ -103,6 +103,7 @@ private:
 	 * @return true if mission has a land start, a land and is valid
 	 */
 	bool hasMissionLandStart() const;
+	bool hasValidMission() const;
 
 	/**
 	 * @brief Check whether there are more waypoints between current waypoint
@@ -179,6 +180,11 @@ private:
 
 	RtlBase *_rtl_mission_type_handle{nullptr};
 	RtlType _rtl_type{RtlType::RTL_DIRECT};
+	uint32_t _mission_land_failure_mission_id{0};
+	uint16_t _mission_land_failure_count{0};
+	int32_t _mission_land_failure_index{-1};
+	uint8_t _mission_land_failure_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0};
+	bool _mission_land_failure_reported{false};
 
 	bool _home_has_land_approach{false};           ///< Flag if the home position has a land approach defined
 	bool _one_rally_point_has_land_approach{false}; ///< Flag if a rally point has a land approach defined

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -50,10 +50,10 @@
 
 static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5;
 
-RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) :
+RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator, const mission_s &mission) :
 	RtlBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE)
 {
-
+	_mission = mission;
 }
 
 void

src/modules/navigator/rtl_direct_mission_land.h

@@ -55,7 +55,7 @@ class Navigator;
 class RtlDirectMissionLand : public RtlBase
 {
 public:
-	RtlDirectMissionLand(Navigator *navigator);
+	RtlDirectMissionLand(Navigator *navigator, const mission_s &mission);
 	~RtlDirectMissionLand() = default;
 
 	void on_activation() override;

src/modules/navigator/test/test_RTL.cpp

@@ -48,12 +48,14 @@
 #include <lib/geo/geo.h>
 #include <parameters/param.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/wind.h>
 
 #include "navigator.h"
 #include "rtl.h"
+#include "rtl_direct_mission_land.h"
 #include "mission_route_types.h"
 #include "support/mission_route_cache_test_peer.h"
 #include "support/mission_route_test_helpers.h"
@@ -199,6 +201,22 @@ public:
 		_wind_sub.update();
 		return selectLandingApproach(destination);
 	}
+
+	bool hasValidMissionForTest()
+	{
+		_mission_sub.update();
+		_home_pos_sub.update();
+		return hasValidMission();
+	}
+};
+
+class RtlDirectMissionLandTestPeer : public RtlDirectMissionLand
+{
+public:
+	RtlDirectMissionLandTestPeer(Navigator *navigator, const mission_s &mission) :
+		RtlDirectMissionLand(navigator, mission) {}
+
+	const mission_s &mission() const { return _mission; }
 };
 
 class RTLTest : public NavigatorDatamanTestBase
@@ -244,6 +262,11 @@ protected:
 			_wind_pub = nullptr;
 		}
 
+		if (_mission_pub != nullptr) {
+			orb_unadvertise(_mission_pub);
+			_mission_pub = nullptr;
+		}
+
 		param_control_autosave(true);
 	}
 
@@ -274,7 +297,7 @@ protected:
 				<< "test safe points did not load";
 	}
 
-	void publishHomePosition(const PositionYawSetpoint &position)
+	void publishHomePosition(const PositionYawSetpoint &position, uint32_t update_count = 0)
 	{
 		home_position_s home{};
 		home.timestamp = hrt_absolute_time();
@@ -283,6 +306,7 @@ protected:
 		home.alt = position.alt;
 		home.valid_hpos = true;
 		home.valid_alt = true;
+		home.update_count = update_count;
 
 		if (_home_pub == nullptr) {
 			_home_pub = orb_advertise(ORB_ID(home_position), &home);
@@ -292,6 +316,16 @@ protected:
 		}
 	}
 
+	void publishMission(const mission_s &mission)
+	{
+		if (_mission_pub == nullptr) {
+			_mission_pub = orb_advertise(ORB_ID(mission), &mission);
+
+		} else {
+			orb_publish(ORB_ID(mission), _mission_pub, &mission);
+		}
+	}
+
 	void publishVehicleStatus(bool is_vtol, uint8_t vehicle_type)
 	{
 		vehicle_status_s status{};
@@ -334,11 +368,56 @@ protected:
 	orb_advert_t _home_pub{nullptr};
 	orb_advert_t _vehicle_status_pub{nullptr};
 	orb_advert_t _wind_pub{nullptr};
+	orb_advert_t _mission_pub{nullptr};
 	DatamanClient _dataman_client{};
 	uint32_t _safe_points_id{0};
 	uint32_t _safe_points_opaque_id{0};
 };
 
+TEST_F(RTLTest, MissionValidityMatchesMissionAndFeasibilityInputs)
+{
+	mission_s mission{};
+	mission.timestamp = hrt_absolute_time();
+	mission.mission_id = 42;
+	mission.geofence_id = 7;
+	const uint32_t home_update_count = 3;
+	publishHomePosition(makePositionYawSetpointFromOffset(kBaseLat, kBaseLon, 0.f, 0.f, kAlt), home_update_count);
+	publishMission(mission);
+
+	mission_result_s *mission_result = _navigator.get_mission_result();
+	mission_result->valid = true;
+	mission_result->mission_id = mission.mission_id - 1;
+	EXPECT_FALSE(_rtl.hasValidMissionForTest());
+
+	mission_result->mission_id = mission.mission_id;
+	mission_result->geofence_id = mission.geofence_id - 1;
+	EXPECT_FALSE(_rtl.hasValidMissionForTest());
+
+	mission_result->geofence_id = mission.geofence_id;
+	mission_result->home_position_counter = home_update_count - 1;
+	EXPECT_FALSE(_rtl.hasValidMissionForTest());
+
+	mission_result->home_position_counter = home_update_count;
+	EXPECT_TRUE(_rtl.hasValidMissionForTest());
+}
+
+TEST_F(RTLTest, DirectMissionLandStartsWithCurrentMission)
+{
+	mission_s mission{};
+	mission.mission_id = 43;
+	mission.count = 4;
+	mission.land_start_index = 2;
+	mission.land_index = 3;
+	mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_1;
+
+	RtlDirectMissionLandTestPeer direct_mission_land{&_navigator, mission};
+	EXPECT_EQ(direct_mission_land.mission().mission_id, mission.mission_id);
+	EXPECT_EQ(direct_mission_land.mission().count, mission.count);
+	EXPECT_EQ(direct_mission_land.mission().land_start_index, mission.land_start_index);
+	EXPECT_EQ(direct_mission_land.mission().land_index, mission.land_index);
+	EXPECT_EQ(direct_mission_land.mission().mission_dataman_id, mission.mission_dataman_id);
+}
+
 // WHY: No land point means no usable approach bearing.
 // WHAT: The chooser should return an invalid loiter.
 TEST_F(RTLTest, ChooseBestLandingApproachRequiresLandLocation)
@@ -863,3 +942,16 @@ TEST_F(RTLTest, MakeVtolLandApproachPointConvertsRelativeAndAbsoluteAltitude)
 	EXPECT_NEAR(relative_point.loiter_radius_m, kApproachRadius, 0.01f);
 	EXPECT_NEAR(relative_int_point.loiter_radius_m, kApproachRadius, 0.01f);
 }
+
+TEST_F(RTLTest, MakeVtolLandApproachPointRejectsInvalidInput)
+{
+	const mission_item_s invalid_latitude = makeLandApproachItem(91.0, kBaseLon, kAlt, kApproachRadius);
+	const mission_item_s invalid_longitude = makeLandApproachItem(kBaseLat, 181.0, kAlt, kApproachRadius);
+	const mission_item_s invalid_altitude = makeLandApproachItem(kBaseLat, kBaseLon, NAN, kApproachRadius);
+	const mission_item_s invalid_radius = makeLandApproachItem(kBaseLat, kBaseLon, kAlt, NAN);
+
+	EXPECT_FALSE(mission_route::makeVtolLandApproachPoint(invalid_latitude, kAlt).isValid());
+	EXPECT_FALSE(mission_route::makeVtolLandApproachPoint(invalid_longitude, kAlt).isValid());
+	EXPECT_FALSE(mission_route::makeVtolLandApproachPoint(invalid_altitude, kAlt).isValid());
+	EXPECT_FALSE(mission_route::makeVtolLandApproachPoint(invalid_radius, kAlt).isValid());
+}

src/modules/navigator/test/test_mission_route_cache.cpp

@@ -75,7 +75,8 @@ protected:
 	}
 
 	mission_s makeMission(uint32_t mission_id, uint16_t count, uint32_t safe_points_id = 0,
-			      int32_t land_index = -1, dm_item_t mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0) const
+			      int32_t land_index = -1, dm_item_t mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0,
+			      dm_item_t safepoint_dataman_id = DM_KEY_SAFE_POINTS_0) const
 	{
 		mission_s mission{};
 		mission.timestamp = hrt_absolute_time();
@@ -84,7 +85,7 @@ protected:
 		mission.land_index = land_index;
 		mission.mission_dataman_id = static_cast<uint8_t>(mission_dataman_id);
 		mission.safe_points_id = safe_points_id;
-		mission.safepoint_dataman_id = DM_KEY_SAFE_POINTS_0;
+		mission.safepoint_dataman_id = static_cast<uint8_t>(safepoint_dataman_id);
 		return mission;
 	}
 
@@ -242,6 +243,7 @@ TEST_F(MissionRouteCacheTest, MissionLandItemRejectsNonLandPublishedIndex)
 	EXPECT_FALSE(_cache.missionLandItemReady());
 	EXPECT_TRUE(_cache.missionLandItemUpdatePending());
 	EXPECT_GT(MissionRouteCacheTestPeer::missionLandRetryCount(_cache), 0U);
+	EXPECT_TRUE(_cache.missionLandItemAttemptFailed());
 
 	// Failed reads leave output parameters untouched.
 	int32_t land_index = 123;
@@ -250,12 +252,21 @@ TEST_F(MissionRouteCacheTest, MissionLandItemRejectsNonLandPublishedIndex)
 	EXPECT_EQ(land_index, 123);
 }
 
+TEST_F(MissionRouteCacheTest, MissionLandItemRejectsInvalidDatamanId)
+{
+	const mission_s mission = makeMission(24, 1, 0, 0, static_cast<dm_item_t>(DM_KEY_NUM_KEYS));
+	_cache.update(mission);
+
+	EXPECT_FALSE(_cache.missionLandItemReady());
+	EXPECT_FALSE(_cache.missionLandItemUpdatePending());
+}
+
 // Transient safe-point state errors retry without changing safe_points_id.
 TEST_F(MissionRouteCacheTest, SafePointCacheRetriesAfterInvalidStateWithoutIdChange)
 {
 	// Start with an invalid state entry for the current safe_points_id.
 	const mission_s mission = makeMission(0, 0, 41);
-	writeSafePointState(DM_KEY_SAFE_POINTS_MAX + 1, 0);
+	writeSafePointState(DM_KEY_SAFE_POINTS_MAX + 1, 41);
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission,
 			[&] { return MissionRouteCacheTestPeer::safePointRetryScheduled(_cache); }))
 			<< "safe-point retry was not scheduled";
@@ -264,7 +275,7 @@ TEST_F(MissionRouteCacheTest, SafePointCacheRetriesAfterInvalidStateWithoutIdCha
 	const std::vector<mission_item_s> safe_points{
 		makeSafePointFromOffset(kBaseLat, kBaseLon, 20.f, 5.f, kAlt),
 	};
-	writeSafePointItems(safe_points, static_cast<uint16_t>(safe_points.size()), 0);
+	writeSafePointItems(safe_points, static_cast<uint16_t>(safe_points.size()), 41);
 
 	// Skip the retry backoff wait and keep driving the cache on the now-valid state.
 	MissionRouteCacheTestPeer::expireSafePointRetryBackoff(_cache);
@@ -278,10 +289,19 @@ TEST_F(MissionRouteCacheTest, SafePointCacheRetriesAfterInvalidStateWithoutIdCha
 	expectMissionItemMatches(safe_point, safe_points[0]);
 }
 
-// safe_points_id participates in cache identity even when opaque_id is reused.
-TEST_F(MissionRouteCacheTest, SafePointIdChangeReloadsWhenOpaqueIdStaysTheSame)
+TEST_F(MissionRouteCacheTest, SafePointCacheRejectsMismatchedSourceId)
+{
+	const mission_s mission = makeMission(0, 0, 90);
+	writeSafePointState(1, 91);
+
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission,
+			[&] { return MissionRouteCacheTestPeer::safePointRetryScheduled(_cache); }));
+	EXPECT_FALSE(_cache.safePointsReady());
+}
+
+// safe_points_id participates in cache identity.
+TEST_F(MissionRouteCacheTest, SafePointIdChangeReloadsReplacementSet)
 {
-	// Two safe-point sets reuse the same opaque_id.
 	const std::vector<mission_item_s> safe_points_a{
 		makeSafePointFromOffset(kBaseLat, kBaseLon, 10.f, 0.f, kAlt),
 	};
@@ -291,7 +311,7 @@ TEST_F(MissionRouteCacheTest, SafePointIdChangeReloadsWhenOpaqueIdStaysTheSame)
 	};
 
 	mission_s mission = makeMission(0, 0, 100);
-	writeSafePointItems(safe_points_a, static_cast<uint16_t>(safe_points_a.size()), 77);
+	writeSafePointItems(safe_points_a, static_cast<uint16_t>(safe_points_a.size()), 100);
 
 	// Load the first set before changing safe_points_id.
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointsReady(); }))
@@ -301,8 +321,7 @@ TEST_F(MissionRouteCacheTest, SafePointIdChangeReloadsWhenOpaqueIdStaysTheSame)
 	ASSERT_TRUE(_cache.loadSafePointItem(0, safe_point));
 	expectMissionItemMatches(safe_point, safe_points_a[0]);
 
-	// Reuse opaque_id to verify safe_points_id identity.
-	writeSafePointItems(safe_points_b, static_cast<uint16_t>(safe_points_b.size()), 77);
+	writeSafePointItems(safe_points_b, static_cast<uint16_t>(safe_points_b.size()), 101);
 	mission.safe_points_id = 101;
 	mission.timestamp = hrt_absolute_time();
 
@@ -332,7 +351,7 @@ TEST_F(MissionRouteCacheTest, SafePointSourceChangeDuringLoadDoesNotExposeStaleD
 	};
 
 	const mission_s mission_a = makeMission(0, 0, 200);
-	writeSafePointItems(safe_points_a, static_cast<uint16_t>(safe_points_a.size()), 11);
+	writeSafePointItems(safe_points_a, static_cast<uint16_t>(safe_points_a.size()), 200);
 
 	// Stop once the first set is in flight.
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission_a,
@@ -342,7 +361,7 @@ TEST_F(MissionRouteCacheTest, SafePointSourceChangeDuringLoadDoesNotExposeStaleD
 
 	// Change the source while the first set is still loading.
 	mission_s mission_b = makeMission(0, 0, 201);
-	writeSafePointItems(safe_points_b, static_cast<uint16_t>(safe_points_b.size()), 12);
+	writeSafePointItems(safe_points_b, static_cast<uint16_t>(safe_points_b.size()), 201);
 
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission_b, [&] { return _cache.safePointsReady(); }))
 			<< "safe-point cache did not become ready";
@@ -358,7 +377,7 @@ TEST_F(MissionRouteCacheTest, SafePointSourceChangeDuringLoadDoesNotExposeStaleD
 TEST_F(MissionRouteCacheTest, SafePointZeroCountIsReadyAndEmpty)
 {
 	const mission_s mission = makeMission(0, 0, 50);
-	writeSafePointState(0, 7);
+	writeSafePointState(0, 50);
 
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointsReady(); }))
 			<< "zero-count safe-point set did not become ready";
@@ -369,6 +388,15 @@ TEST_F(MissionRouteCacheTest, SafePointZeroCountIsReadyAndEmpty)
 	EXPECT_FALSE(_cache.loadSafePointItem(0, safe_point));
 }
 
+TEST_F(MissionRouteCacheTest, SafePointZeroCountIgnoresUnusedDatamanId)
+{
+	const mission_s mission = makeMission(0, 0, 51);
+	writeSafePointState(0, 51, DM_KEY_WAYPOINTS_OFFBOARD_0);
+
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointsReady(); }));
+	EXPECT_EQ(_cache.safePointCount(), 0);
+}
+
 // Changed safe-point stats with a reused opaque_id and unchanged safe_points_id still reload.
 TEST_F(MissionRouteCacheTest, SafePointStatsChangeWithSameOpaqueIdReloads)
 {
@@ -381,13 +409,13 @@ TEST_F(MissionRouteCacheTest, SafePointStatsChangeWithSameOpaqueIdReloads)
 	};
 
 	const mission_s mission = makeMission(0, 0, 60);
-	writeSafePointItems(safe_points_a, static_cast<uint16_t>(safe_points_a.size()), 88);
+	writeSafePointItems(safe_points_a, static_cast<uint16_t>(safe_points_a.size()), 60);
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointsReady(); }))
 			<< "safe-point cache did not become ready";
 	ASSERT_EQ(_cache.safePointCount(), 1);
 
 	// Grow the stored set while reusing both the opaque id and safe_points_id.
-	writeSafePointItems(safe_points_b, static_cast<uint16_t>(safe_points_b.size()), 88);
+	writeSafePointItems(safe_points_b, static_cast<uint16_t>(safe_points_b.size()), 60);
 	MissionRouteCacheTestPeer::requestSafePointRecheck(_cache);
 
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointCount() == 2; }))
@@ -398,8 +426,8 @@ TEST_F(MissionRouteCacheTest, SafePointStatsChangeWithSameOpaqueIdReloads)
 	expectMissionItemMatches(safe_point, safe_points_b[1]);
 }
 
-// A safe-point dataman_id change with a reused opaque_id reloads from the new storage.
-TEST_F(MissionRouteCacheTest, SafePointDatamanIdChangeWithSameOpaqueIdReloads)
+// A safe-point source-bank change reloads from the new storage.
+TEST_F(MissionRouteCacheTest, SafePointDatamanIdChangeReloadsFromNewStorage)
 {
 	const std::vector<mission_item_s> safe_points_0{
 		makeSafePointFromOffset(kBaseLat, kBaseLon, 15.f, 0.f, kAlt),
@@ -408,8 +436,8 @@ TEST_F(MissionRouteCacheTest, SafePointDatamanIdChangeWithSameOpaqueIdReloads)
 		makeSafePointFromOffset(kBaseLat, kBaseLon, 45.f, 0.f, kAlt),
 	};
 
-	const mission_s mission = makeMission(0, 0, 70);
-	writeSafePointItems(safe_points_0, static_cast<uint16_t>(safe_points_0.size()), 99, DM_KEY_SAFE_POINTS_0);
+	mission_s mission = makeMission(0, 0, 70);
+	writeSafePointItems(safe_points_0, static_cast<uint16_t>(safe_points_0.size()), 70, DM_KEY_SAFE_POINTS_0);
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointsReady(); }))
 			<< "safe-point cache did not become ready";
 
@@ -417,9 +445,9 @@ TEST_F(MissionRouteCacheTest, SafePointDatamanIdChangeWithSameOpaqueIdReloads)
 	ASSERT_TRUE(_cache.loadSafePointItem(0, safe_point));
 	expectMissionItemMatches(safe_point, safe_points_0[0]);
 
-	// Same opaque id and safe_points_id, but the set now lives in safe-point storage 1.
-	writeSafePointItems(safe_points_1, static_cast<uint16_t>(safe_points_1.size()), 99, DM_KEY_SAFE_POINTS_1);
-	MissionRouteCacheTestPeer::requestSafePointRecheck(_cache);
+	writeSafePointItems(safe_points_1, static_cast<uint16_t>(safe_points_1.size()), 70, DM_KEY_SAFE_POINTS_1);
+	mission.safepoint_dataman_id = DM_KEY_SAFE_POINTS_1;
+	mission.timestamp = hrt_absolute_time();
 
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.safePointsReady(); }))
 			<< "safe-point cache did not reload after dataman id change";
@@ -431,10 +459,11 @@ TEST_F(MissionRouteCacheTest, SafePointDatamanIdChangeWithSameOpaqueIdReloads)
 // Safe-point states pointing at a non safe-point dataman key are rejected and retried.
 TEST_F(MissionRouteCacheTest, SafePointCacheRejectsInvalidDatamanId)
 {
-	const mission_s mission = makeMission(0, 0, 80);
+	const mission_s mission = makeMission(0, 0, 80, -1, DM_KEY_WAYPOINTS_OFFBOARD_0,
+					      DM_KEY_WAYPOINTS_OFFBOARD_0);
 
 	// A plausible count but an unsupported storage key must be rejected before any load.
-	writeSafePointState(1, 5, DM_KEY_WAYPOINTS_OFFBOARD_0);
+	writeSafePointState(1, 80, DM_KEY_WAYPOINTS_OFFBOARD_0);
 
 	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission,
 			[&] { return MissionRouteCacheTestPeer::safePointRetryScheduled(_cache); }))