mission_base: Only run mission feasibility if the geofence module is ready

src/modules/navigator/mission_base.cpp

@@ -120,9 +120,12 @@ void MissionBase::onMissionUpdate(bool has_mission_items_changed)
 		_dataman_cache.invalidate();
 		_load_mission_index = -1;
 
-		if (_navigator->home_global_position_valid()) {
+		if (canRunMissionFeasibility()) {
-			_initialized_mission_checked = true;
+			_mission_checked = true;
 			check_mission_valid();
+
+		} else {
+			_mission_checked = false;
 		}
 	}
 
@@ -148,14 +151,15 @@ MissionBase::on_inactive()
 	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 	_global_pos_sub.update();
+	_geofence_status_sub.update();
 
 	parameters_update();
 
 	updateMavlinkMission();
 
-	/* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */
+	/* Check the mission */
-	if (_navigator->home_global_position_valid() && !_initialized_mission_checked) {
+	if (!_mission_checked && canRunMissionFeasibility()) {
-		_initialized_mission_checked = true;
+		_mission_checked = true;
 		check_mission_valid();
 		_is_current_planned_mission_item_valid = isMissionValid();
 	}
@@ -240,12 +244,22 @@ MissionBase::on_active()
 	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 	_global_pos_sub.update();
+	_geofence_status_sub.update();
 
 	parameters_update();
 
 	updateMavlinkMission();
 	updateDatamanCache();
 
+	/* Check the mission */
+	if (!_mission_checked && canRunMissionFeasibility()) {
+		_mission_checked = true;
+		check_mission_valid();
+		_is_current_planned_mission_item_valid = isMissionValid();
+		update_mission();
+		set_mission_items();
+	}
+
 	// check if heading alignment is necessary, and add it to the current mission item if necessary
 	if (_align_heading_necessary && is_mission_item_reached_or_completed()) {
 
@@ -1347,3 +1361,11 @@ bool MissionBase::checkMissionDataChanged(mission_s new_mission)
 		(new_mission.mission_id != _mission.mission_id) ||
 		(new_mission.current_seq != _mission.current_seq));
 }
+
+bool MissionBase::canRunMissionFeasibility()
+{
+	return _navigator->home_global_position_valid() &&
+	       (_geofence_status_sub.get().timestamp > 0) &&
+	       (_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
+	       (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY);
+}

src/modules/navigator/mission_base.h

@@ -43,6 +43,7 @@
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 #include <dataman_client/DatamanClient.hpp>
+#include <uORB/topics/geofence_status.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/parameter_update.h>
@@ -308,7 +309,7 @@ protected:
 
 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
-	bool _initialized_mission_checked{false};		/**< Flag indicating if the initialized mission has been checked by the mission validator*/
+	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/
 	bool _system_disarmed_while_inactive{false};		/**< Flag indicating if the system has been disarmed while mission is inactive*/
 	mission_s _mission;					/**< Currently active mission*/
 	float _mission_init_climb_altitude_amsl{NAN}; 		/**< altitude AMSL the vehicle will climb to when mission starts */
@@ -443,6 +444,8 @@ private:
 	 */
 	bool checkMissionDataChanged(mission_s new_mission);
 
+	bool canRunMissionFeasibility();
+
 	int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/
 	int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/
 
@@ -460,4 +463,5 @@ private:
 	)
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionData<geofence_status_s> _geofence_status_sub{ORB_ID(geofence_status)};
 };