feat(navigator): confirm that the land item is fully loaded before making it available

src/modules/navigator/mission_route_cache.cpp

@@ -118,6 +118,25 @@ bool MissionRouteCache::missionCacheFullyLoaded(const mission_s &mission) const
 	return true;
 }
 
+bool MissionRouteCache::missionLandItemCacheFullyLoaded() const
+{
+	if (_mission_land.index < 0 || _mission_land.index >= _mission.count) {
+		return false;
+	}
+
+	mission_item_s land_item{};
+
+	if (!_dataman_cache_land_item.loadWait(static_cast<dm_item_t>(_mission_land.dataman_id),
+					       _mission_land.index,
+					       reinterpret_cast<uint8_t *>(&land_item),
+					       sizeof(land_item),
+					       kCacheOnlyLoadWait)) {
+		return false;
+	}
+
+	return isMissionLandCommand(land_item.nav_cmd);
+}
+
 bool MissionRouteCache::safePointCacheFullyLoaded() const
 {
 	// Validate against the set currently being loaded.
@@ -211,14 +230,21 @@ bool MissionRouteCache::loadSafePointItem(int index, mission_item_s &safe_point_
 
 bool MissionRouteCache::getMissionLandItem(int32_t &index, mission_item_s &land_item) const
 {
-	if (_mission_land.index < 0 || _mission_land.index >= _mission.count) {
+	if (!missionLandItemReady()) {
+		return false;
+	}
+
+	mission_item_s cached_land_item{};
+
+	if (!_dataman_cache_land_item.loadWait(static_cast<dm_item_t>(_mission_land.dataman_id), _mission_land.index,
+					       reinterpret_cast<uint8_t *>(&cached_land_item), sizeof(cached_land_item),
+					       kCacheOnlyLoadWait)) {
 		return false;
 	}
 
 	index = _mission_land.index;
-	return _dataman_cache_land_item.loadWait(static_cast<dm_item_t>(_mission_land.dataman_id), index,
-			reinterpret_cast<uint8_t *>(&land_item), sizeof(land_item),
-			kCacheOnlyLoadWait);
+	land_item = cached_land_item;
+	return true;
 }
 
 bool MissionRouteCache::loadMissionItem(const mission_s &mission, int32_t index, mission_item_s &mission_item) const
@@ -246,11 +272,40 @@ bool MissionRouteCache::syncMissionItem(const mission_s &mission, int32_t index,
 			return false;
 		}
 
-		if (!_dataman_cache_land_item.updateCachedItem(static_cast<dm_item_t>(_mission_land.dataman_id), index,
-				reinterpret_cast<const uint8_t *>(&mission_item), sizeof(mission_item))) {
+		const hrt_abstime now = hrt_absolute_time();
+
+		if (_mission_land.ready) {
+			if (!_dataman_cache_land_item.updateCachedItem(static_cast<dm_item_t>(_mission_land.dataman_id), index,
+					reinterpret_cast<const uint8_t *>(&mission_item), sizeof(mission_item))) {
+				_dataman_cache_land_item.invalidate();
+				_mission_land.ready = false;
+				_mission_land.validation_pending = false;
+				_mission_land.retry.scheduleRetry(now);
+				return false;
+			}
+
+			if (!isMissionLandCommand(mission_item.nav_cmd)) {
+				_dataman_cache_land_item.invalidate();
+				_mission_land.ready = false;
+				_mission_land.validation_pending = false;
+				_mission_land.retry.scheduleRetry(now);
+			}
+
+		} else {
 			_dataman_cache_land_item.invalidate();
-			_mission_land.index = -1;
-			return false;
+			_mission_land.validation_pending = false;
+			_mission_land.retry.clear();
+
+			if (isMissionLandCommand(mission_item.nav_cmd)) {
+				_mission_land.validation_pending = queueMissionLandItem();
+
+				if (!_mission_land.validation_pending) {
+					_mission_land.retry.scheduleRetry(now);
+				}
+
+			} else {
+				_mission_land.retry.scheduleRetry(now);
+			}
 		}
 	}
 
@@ -324,6 +379,7 @@ void MissionRouteCache::updateMissionCache(const mission_s &mission)
 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 mission_land_changed = mission.mission_id != state.mission_id
@@ -338,29 +394,66 @@ void MissionRouteCache::updateMissionLandItemCache(const mission_s &mission)
 		_dataman_cache_land_item.invalidate();
 
 		if (state.index >= 0) {
-			queueMissionLandItem();
-		}
+			state.validation_pending = queueMissionLandItem();
 
-	} else if (state.index >= 0 && state.retry.due(hrt_absolute_time())) {
-		// A transient queue failure must not permanently disable the mission-land fallback.
-		_dataman_cache_land_item.invalidate();
-		queueMissionLandItem();
+			if (!state.validation_pending) {
+				state.retry.scheduleRetry(now);
+			}
+		}
 	}
 
 	_dataman_cache_land_item.update();
-}
 
-void MissionRouteCache::queueMissionLandItem()
-{
-	MissionLandState &state = _mission_land;
-
-	if (_dataman_cache_land_item.load(static_cast<dm_item_t>(state.dataman_id), state.index)) {
-		state.retry.clear();
+	if (state.index < 0 || state.ready) {
 		return;
 	}
 
+	if (state.validation_pending) {
+		if (_dataman_cache_land_item.isLoading()) {
+			return;
+		}
+
+		state.validation_pending = false;
+
+		if (missionLandItemCacheFullyLoaded()) {
+			state.ready = true;
+			state.retry.clear();
+
+		} else {
+			PX4_WARN("Mission land cache invalid or incomplete, retrying mission_id=%" PRIu32 ", index=%" PRIi32,
+				 state.mission_id, state.index);
+			_dataman_cache_land_item.invalidate();
+			state.retry.scheduleRetry(now);
+		}
+
+	} else if (state.retry.due(now)) {
+		_dataman_cache_land_item.invalidate();
+
+		state.validation_pending = queueMissionLandItem();
+
+		if (state.validation_pending) {
+			state.retry.retry_at = 0;
+
+		} else {
+			state.retry.scheduleRetry(now);
+		}
+	}
+}
+
+bool MissionRouteCache::queueMissionLandItem()
+{
+	const MissionLandState &state = _mission_land;
+
+	if (state.index < 0) {
+		return false;
+	}
+
+	if (_dataman_cache_land_item.load(static_cast<dm_item_t>(state.dataman_id), state.index)) {
+		return true;
+	}
+
 	PX4_WARN("Mission land cache queue failed, retrying! item=%" PRIu8 ", index=%" PRIi32, state.dataman_id, state.index);
-	state.retry.scheduleRetry(hrt_absolute_time());
+	return false;
 }
 
 void MissionRouteCache::updateSafePointCache(const mission_s &mission)

src/modules/navigator/mission_route_cache.h

@@ -80,7 +80,21 @@ public:
 		       || _safe_point.dataman_state != SafePointDatamanState::kUpdateRequestWait;
 	}
 	uint32_t safePointsId() const { return _safe_point.source_id; }
+	bool missionLandItemReady() const
+	{
+		return _mission_land.ready
+		       && _mission_land.index >= 0
+		       && _mission_land.index < _mission.count;
+	}
+	bool missionLandItemUpdatePending() const
+	{
+		return _mission_land.index >= 0
+		       && _mission_land.index < _mission.count
+		       && !_mission_land.ready
+		       && (_mission_land.validation_pending || _mission_land.retry.retry_at != 0);
+	}
 
+	inline bool isMissionLandCommand(uint16_t nav_cmd) const { return nav_cmd == NAV_CMD_LAND || nav_cmd == NAV_CMD_VTOL_LAND;}
 	int missionCount() const override;
 	bool loadMissionItem(int index, mission_item_s &mission_item) const override;
 	int safePointCount() const override;
@@ -137,6 +151,8 @@ private:
 		uint32_t mission_id{0};
 		uint8_t dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0};
 		int32_t index{-1};
+		bool ready{false};
+		bool validation_pending{false};
 		RetryBackoff retry{};
 	};
 
@@ -154,11 +170,12 @@ private:
 
 	void updateMissionCache(const mission_s &mission);
 	void updateMissionLandItemCache(const mission_s &mission);
-	void queueMissionLandItem();
+	bool queueMissionLandItem();
 	void updateSafePointCache(const mission_s &mission);
 	bool missionMatchesCache(const mission_s &mission) const;
 	bool queueMissionCacheLoads(const mission_s &mission);
 	bool missionCacheFullyLoaded(const mission_s &mission) const;
+	bool missionLandItemCacheFullyLoaded() const;
 	bool safePointCacheFullyLoaded() const;
 	bool safePointCacheMatchesReadStats() const;
 	void publishSafePointCache();

src/modules/navigator/rtl.cpp

@@ -151,6 +151,8 @@ void RTL::on_activation()
 	_home_pos_sub.update();
 	_wind_sub.update();
 
+	updateDatamanCache();
+
 	setRtlTypeAndDestination();
 
 	switch (_rtl_type) {
@@ -371,7 +373,9 @@ void RTL::setRtlTypeAndDestination()
 	// Publish rtl status
 	rtl_status_s rtl_status{};
 	rtl_status.safe_points_id = mission_route_cache != nullptr ? mission_route_cache->safePointsId() : 0;
-	rtl_status.is_evaluation_pending = mission_route_cache != nullptr && mission_route_cache->safePointUpdatePending();
+	rtl_status.is_evaluation_pending = mission_route_cache != nullptr
+					   && (mission_route_cache->safePointUpdatePending()
+					       || mission_route_cache->missionLandItemUpdatePending());
 	rtl_status.has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
 	rtl_status.rtl_type = static_cast<uint8_t>(_rtl_type);
 	rtl_status.safe_point_index = safe_point_index;
@@ -482,10 +486,15 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 			const bool success = mission_route_cache != nullptr && mission_route_cache->getMissionLandItem(land_index, land_mission_item);
 
 			if (!success) {
-				/* 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");
+				const bool land_item_pending = mission_route_cache != nullptr
+							       && mission_route_cache->missionLandItemUpdatePending();
+
+				if (!land_item_pending) {
+					/* 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");
+				}
 
 			} else {
 				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)};

src/modules/navigator/test/support/mission_route_cache_test_peer.h

@@ -61,6 +61,16 @@ public:
 		return cache._mission.retry.retry_count;
 	}
 
+	static bool missionLandRetryScheduled(const MissionRouteCache &cache)
+	{
+		return cache._mission_land.retry.retry_at != 0;
+	}
+
+	static uint8_t missionLandRetryCount(const MissionRouteCache &cache)
+	{
+		return cache._mission_land.retry.retry_count;
+	}
+
 	static bool safePointRetryScheduled(const MissionRouteCache &cache)
 	{
 		return cache._safe_point.retry.retry_at != 0;

src/modules/navigator/test/test_mission_route_cache.cpp

@@ -214,6 +214,33 @@ TEST_F(MissionRouteCacheTest, MissionCacheRejectsTooLargeMission)
 	EXPECT_FALSE(_cache.loadMissionItem(mission, 0, cached_item));
 }
 
+// The dedicated land-item cache is not exposed until the async load has been validated.
+TEST_F(MissionRouteCacheTest, MissionLandItemIsHiddenUntilValidated)
+{
+	const std::vector<mission_item_s> mission_items{
+		makeTakeoffItemFromOffset(kBaseLat, kBaseLon,   0.f, 0.f, kAlt + 10.f),
+		makeLandItemFromOffset(kBaseLat, kBaseLon, 100.f, 0.f, kAlt),
+	};
+	const int32_t land_index_expected = 1;
+	const mission_s mission = makeMission(22, static_cast<uint16_t>(mission_items.size()), 0, land_index_expected);
+	writeMissionItems(mission_items);
+
+	_cache.update(mission);
+
+	EXPECT_TRUE(_cache.missionLandItemUpdatePending());
+	EXPECT_FALSE(_cache.missionLandItemReady());
+
+	// Failed reads leave output parameters untouched.
+	int32_t land_index = 123;
+	mission_item_s land_item{};
+	EXPECT_FALSE(_cache.getMissionLandItem(land_index, land_item));
+	EXPECT_EQ(land_index, 123);
+
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] { return _cache.missionLandItemReady(); }))
+			<< "mission land item did not become ready";
+	EXPECT_FALSE(_cache.missionLandItemUpdatePending());
+}
+
 // Published land_index loads the dedicated land-item cache.
 TEST_F(MissionRouteCacheTest, MissionLandItemLoadsReferencedWaypoint)
 {
@@ -236,6 +263,8 @@ TEST_F(MissionRouteCacheTest, MissionLandItemLoadsReferencedWaypoint)
 			<< "mission land item did not become ready";
 
 	// The cached land item follows the published index.
+	EXPECT_TRUE(_cache.missionLandItemReady());
+	EXPECT_FALSE(_cache.missionLandItemUpdatePending());
 	int32_t land_index = -1;
 	mission_item_s land_item{};
 	ASSERT_TRUE(_cache.getMissionLandItem(land_index, land_item));
@@ -261,11 +290,40 @@ TEST_F(MissionRouteCacheTest, MissionLandItemRejectsOutOfBoundsPublishedIndex)
 			<< "mission cache did not become ready";
 
 	// No land item is exposed from the invalid index.
+	EXPECT_FALSE(_cache.missionLandItemReady());
+	EXPECT_FALSE(_cache.missionLandItemUpdatePending());
 	int32_t land_index = -1;
 	mission_item_s land_item{};
 	EXPECT_FALSE(_cache.getMissionLandItem(land_index, land_item));
 }
 
+// A published land_index must contain a land command.
+TEST_F(MissionRouteCacheTest, MissionLandItemRejectsNonLandPublishedIndex)
+{
+	// The mission contains a land item, but the published land_index points at a normal waypoint.
+	const std::vector<mission_item_s> mission_items{
+		makeTakeoffItemFromOffset(kBaseLat, kBaseLon,   0.f,  0.f, kAlt + 15.f),
+		makePositionItemFromOffset(kBaseLat, kBaseLon, 120.f, 0.f, kAlt + 30.f),
+		makeLandItemFromOffset(kBaseLat, kBaseLon, 240.f, 0.f, kAlt),
+	};
+	const mission_s mission = makeMission(23, static_cast<uint16_t>(mission_items.size()), 0, 1);
+	writeMissionItems(mission_items);
+
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission,
+			[&] { return MissionRouteCacheTestPeer::missionLandRetryScheduled(_cache); }))
+			<< "mission land cache retry was not scheduled";
+
+	EXPECT_FALSE(_cache.missionLandItemReady());
+	EXPECT_TRUE(_cache.missionLandItemUpdatePending());
+	EXPECT_GT(MissionRouteCacheTestPeer::missionLandRetryCount(_cache), 0U);
+
+	// Failed reads leave output parameters untouched.
+	int32_t land_index = 123;
+	mission_item_s land_item{};
+	EXPECT_FALSE(_cache.getMissionLandItem(land_index, land_item));
+	EXPECT_EQ(land_index, 123);
+}
+
 // Transient safe-point state errors retry without changing safe_points_id.
 TEST_F(MissionRouteCacheTest, SafePointCacheRetriesAfterInvalidStateWithoutIdChange)
 {
@@ -565,6 +623,79 @@ TEST_F(MissionRouteCacheTest, SyncMissionItemUpdatesMissionLandCache)
 	expectMissionItemMatches(cached_item, updated_land);
 }
 
+// Changing the published land item to a non-land command makes the dedicated land cache unavailable.
+TEST_F(MissionRouteCacheTest, SyncMissionItemInvalidatesMissionLandCacheForNonLandCommand)
+{
+	const std::vector<mission_item_s> mission_items{
+		makeTakeoffItemFromOffset(kBaseLat, kBaseLon,   0.f, 0.f, kAlt + 10.f),
+		makePositionItemFromOffset(kBaseLat, kBaseLon, 100.f, 0.f, kAlt + 20.f),
+		makeLandItemFromOffset(kBaseLat, kBaseLon, 200.f, 0.f, kAlt),
+	};
+	const int32_t land_index = 2;
+	const mission_s mission = makeMission(43, static_cast<uint16_t>(mission_items.size()), 0, land_index);
+	writeMissionItems(mission_items);
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] {
+		int32_t ready_index = -1;
+		mission_item_s ready_land_item{};
+		return _cache.isReady(mission) && _cache.getMissionLandItem(ready_index, ready_land_item);
+	}))
+			<< "mission land item did not become ready";
+
+	const mission_item_s updated_item = makePositionItemFromOffset(kBaseLat, kBaseLon, 222.f, 11.f, kAlt + 1.f);
+	ASSERT_TRUE(_cache.syncMissionItem(mission, land_index, updated_item));
+
+	EXPECT_FALSE(_cache.missionLandItemReady());
+	EXPECT_TRUE(_cache.missionLandItemUpdatePending());
+
+	// Failed reads leave output parameters untouched.
+	int32_t out_index = 123;
+	mission_item_s land_item{};
+	EXPECT_FALSE(_cache.getMissionLandItem(out_index, land_item));
+	EXPECT_EQ(out_index, 123);
+
+	mission_item_s cached_item{};
+	ASSERT_TRUE(_cache.loadMissionItem(mission, land_index, cached_item));
+	expectMissionItemMatches(cached_item, updated_item);
+}
+
+// Syncing the land index while its dedicated cache is pending must discard the stale pending read.
+TEST_F(MissionRouteCacheTest, SyncMissionItemInvalidatesPendingMissionLandCacheForNonLandCommand)
+{
+	const std::vector<mission_item_s> mission_items{
+		makeTakeoffItemFromOffset(kBaseLat, kBaseLon,   0.f, 0.f, kAlt + 10.f),
+		makePositionItemFromOffset(kBaseLat, kBaseLon, 100.f, 0.f, kAlt + 20.f),
+		makeLandItemFromOffset(kBaseLat, kBaseLon, 200.f, 0.f, kAlt),
+	};
+	const int32_t land_index = 2;
+	const mission_s mission = makeMission(44, static_cast<uint16_t>(mission_items.size()), 0, land_index);
+	writeMissionItems(mission_items);
+
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission, [&] {
+		return _cache.isReady(mission) && _cache.missionLandItemUpdatePending() && !_cache.missionLandItemReady();
+	}))
+			<< "mission cache did not become ready before the land-item cache";
+
+	const mission_item_s updated_item = makePositionItemFromOffset(kBaseLat, kBaseLon, 222.f, 11.f, kAlt + 1.f);
+	ASSERT_TRUE(_cache.syncMissionItem(mission, land_index, updated_item));
+
+	ASSERT_TRUE(MissionRouteCacheTestPeer::runCacheUntil(_cache, mission,
+			[&] { return MissionRouteCacheTestPeer::missionLandRetryScheduled(_cache); }))
+			<< "mission land cache retry was not scheduled";
+
+	EXPECT_FALSE(_cache.missionLandItemReady());
+	EXPECT_TRUE(_cache.missionLandItemUpdatePending());
+
+	// Failed reads leave output parameters untouched.
+	int32_t out_index = 123;
+	mission_item_s land_item{};
+	EXPECT_FALSE(_cache.getMissionLandItem(out_index, land_item));
+	EXPECT_EQ(out_index, 123);
+
+	mission_item_s cached_item{};
+	ASSERT_TRUE(_cache.loadMissionItem(mission, land_index, cached_item));
+	expectMissionItemMatches(cached_item, updated_item);
+}
+
 // syncMissionItem only patches the active mission it was loaded for.
 TEST_F(MissionRouteCacheTest, SyncMissionItemRejectsInactiveMission)
 {