fix navigator bug: "Takeoff to x meters above home." after mission ended

msg/vehicle_control_mode.msg

@@ -1,5 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
-bool flag_armed			# is set whenever the writing thread stores new data
+bool flag_armed			# synonym for actuator_armed.armed
 
 bool flag_external_manual_override_ok		# external override non-fatal for system. Only true for fixed wing
 

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp

@@ -137,10 +137,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 	// Check if triplet is valid. There must be at least a valid altitude.
 
 	if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
-		// Best we can do is to just set all waypoints to current state and return false.
+		// Best we can do is to just set all waypoints to current state
 		_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
-		_type = WaypointType::position;
-		return false;
+		_type = WaypointType::loiter;
+		_yaw_setpoint = _yaw;
+		_yawspeed_setpoint = NAN;
+		_updateInternalWaypoints();
+		return true;
 	}
 
 	_type = (WaypointType)_sub_triplet_setpoint.get().current.type;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -254,7 +254,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 	 * to true, the failsafe will be initiated immediately.
 	 */
 	void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force,
-		      const bool warn);
+		      bool warn);
 
 	/**
 	 * Reset setpoints to NAN
@@ -921,8 +921,13 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoin
 
 void
 MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
-				     const bool force, const bool warn)
+				     const bool force, bool warn)
 {
+	// do not warn while we are disarmed, as we might not have valid setpoints yet
+	if (!_control_mode.flag_armed) {
+		warn = false;
+	}
+
 	_failsafe_land_hysteresis.set_state_and_update(true, hrt_absolute_time());
 
 	if (!_failsafe_land_hysteresis.get_state() && !force) {

src/modules/navigator/mission.cpp

@@ -88,7 +88,7 @@ Mission::on_inactive()
 		}
 
 		/* reset the current mission if needed */
-		if (need_to_reset_mission(false)) {
+		if (need_to_reset_mission()) {
 			reset_mission(_mission);
 			update_mission();
 			_navigator->reset_cruising_speed();
@@ -195,14 +195,8 @@ Mission::on_active()
 		update_mission();
 	}
 
-	/* reset the current mission if needed */
-	if (need_to_reset_mission(true)) {
-		reset_mission(_mission);
-		_navigator->reset_triplets();
-		update_mission();
-		_navigator->reset_cruising_speed();
-		mission_sub_updated = true;
-	}
+	/* mission is running (and we are armed), need reset after disarm */
+	_need_mission_reset = true;
 
 	_mission_changed = false;
 
@@ -1723,16 +1717,12 @@ Mission::reset_mission(struct mission_s &mission)
 }
 
 bool
-Mission::need_to_reset_mission(bool active)
+Mission::need_to_reset_mission()
 {
 	/* reset mission state when disarmed */
 	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) {
 		_need_mission_reset = false;
 		return true;
-
-	} else if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED && active) {
-		/* mission is running, need reset after disarm */
-		_need_mission_reset = true;
 	}
 
 	return false;

src/modules/navigator/mission.h

@@ -218,9 +218,9 @@ class Mission : public MissionBlock, public ModuleParams
 	void reset_mission(struct mission_s &mission);
 
 	/**
-	 * Returns true if we need to reset the mission
+	 * Returns true if we need to reset the mission (call this only when inactive)
 	 */
-	bool need_to_reset_mission(bool active);
+	bool need_to_reset_mission();
 
 	/**
 	 * Project current location with heading to far away location and fill setpoint.

src/modules/navigator/navigator_main.cpp

@@ -647,6 +647,11 @@ Navigator::run()
 			break;
 		}
 
+		// Do not execute any state machine while we are disarmed
+		if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+			navigation_mode_new = nullptr;
+		}
+
 		// update the vehicle status
 		_previous_nav_state = _vstatus.nav_state;