mission_base: perform mission_check_valid if mission, geofence ot hom…

…eposition have changed
PX4 · Nov 24, 2023 · 6a34b63 · 6a34b63
1 parent 5e1f0b7
commit 6a34b63
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/msg/MissionResult.msg b/msg/MissionResult.msg
@@ -1,6 +1,8 @@
 uint64 timestamp				# time since system start (microseconds)
 
 uint16 mission_update_counter   # Counter for the mission for which the result was generated
+uint16 geofence_update_counter  # Counter for the corresponding geofence for which the result was generated (used for mission feasibility)
+uint64 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)
 
 int32 seq_reached		# Sequence of the mission item which has been reached, default -1
 uint16 seq_current		# Sequence of the current mission item

diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp
@@ -689,17 +689,20 @@ MissionBase::checkMissionRestart()
 void
 MissionBase::check_mission_valid()
 {
-	if (_navigator->get_mission_result()->mission_update_counter != _mission.mission_update_counter) {
-		MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client);
-
-		bool is_mission_valid =
-			missionFeasibilityChecker.checkMissionFeasible(_mission);
+	if ((_navigator->get_mission_result()->mission_update_counter != _mission.mission_update_counter)
+	    || (_navigator->get_mission_result()->geofence_update_counter != _mission.geofence_update_counter)
+	    || (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) {
 
-		_navigator->get_mission_result()->valid = is_mission_valid;
 		_navigator->get_mission_result()->mission_update_counter = _mission.mission_update_counter;
+		_navigator->get_mission_result()->geofence_update_counter = _mission.geofence_update_counter;
+		_navigator->get_mission_result()->home_position_counter = _navigator->get_home_position()->update_count;
+
+		MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client);
+		_navigator->get_mission_result()->valid = missionFeasibilityChecker.checkMissionFeasible(_mission);
 		_navigator->get_mission_result()->seq_total = _mission.count;
 		_navigator->get_mission_result()->seq_reached = -1;
 		_navigator->get_mission_result()->failure = false;
+
 		set_mission_result();
 	}
 }