navigator: mission check refactor

The mission feasability checker was called with the same arguments
twice which made it hard to understand when a mission is marked valid.

The mission check should run in these two cases:
- When initializing (if home comes up) if there is already a mission saved.
- When the mission gets updated.

src/modules/navigator/mission.cpp

@@ -81,7 +81,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_need_takeoff(true),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
-	_home_inited(false),
 	_need_mission_reset(false),
 	_missionFeasibilityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
@@ -100,6 +99,11 @@ Mission::~Mission()
 void
 Mission::on_inactive()
 {
+	/* Without home a mission can't be valid yet anyway, let's wait. */
+	if (!_navigator->home_position_valid()) {
+		return;
+	}
+
 	if (_inited) {
 		/* check if missions have changed so that feedback to ground station is given */
 		bool onboard_updated = false;
@@ -138,11 +142,12 @@ Mission::on_inactive()
 			_current_offboard_mission_index = mission_state.current_seq;
 		}
 
+		/* On init let's check the mission, maybe there is already one available. */
+		check_mission_valid();
+
 		_inited = true;
 	}
 
-	check_mission_valid();
-
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
@@ -274,24 +279,14 @@ Mission::update_offboard_mission()
 			/* otherwise, just leave it */
 		}
 
-		/* Check mission feasibility, for now do not handle the return value,
+		check_mission_valid();
-		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
-		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+		failed = !_navigator->get_mission_result()->valid;
-				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
-				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
-				_navigator->get_land_detected()->landed);
 
-		_navigator->get_mission_result()->valid = !failed;
 		if (!failed) {
 			/* reset mission failure if we have an updated valid mission */
 			_navigator->get_mission_result()->mission_failure = false;
 		}
-		_navigator->increment_mission_instance_count();
-		_navigator->set_mission_result_updated();
 
 	} else {
 		PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub());
@@ -1131,28 +1126,28 @@ Mission::set_mission_finished()
 	_navigator->set_mission_result_updated();
 }
 
-bool
+void
 Mission::check_mission_valid()
 {
-	/* check if the home position became valid in the meantime */
-	if (!_home_inited && _navigator->home_position_valid()) {
 
-		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
+	_navigator->get_mission_result()->valid =
-				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+		_missionFeasibilityChecker.checkMissionFeasible(
-				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
+			_navigator->get_mavlink_log_pub(),
-				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+			(_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(),
+			dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_land_detected()->landed);
+			_navigator->get_home_position()->alt,
+			_navigator->home_position_valid(),
+			_navigator->get_global_position()->lat,
+			_navigator->get_global_position()->lon,
+			_param_dist_1wp.get(),
+			_navigator->get_mission_result()->warning,
+			_navigator->get_default_acceptance_radius(),
+			_navigator->get_land_detected()->landed);
 
-		_navigator->increment_mission_instance_count();
+	_navigator->increment_mission_instance_count();
-		_navigator->set_mission_result_updated();
+	_navigator->set_mission_result_updated();
-
-		_home_inited = true;
-	}
-
-	return _navigator->get_mission_result()->valid;
 }
 
 void

src/modules/navigator/mission.h

@@ -211,7 +211,7 @@ private:
 	/**
 	 * Check wether a mission is ready to go
 	 */
-	bool check_mission_valid();
+	void check_mission_valid();
 
 	/**
 	 * Reset offboard mission
@@ -244,7 +244,6 @@ private:
 	} _mission_type;
 
 	bool _inited;
-	bool _home_inited;
 	bool _need_mission_reset;
 
 	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */