Merge branch 'master' into beta_mavlink2

1 files changed, 44 insertions, 8 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index ed6d8792c..774758ef4 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -176,6 +176,8 @@ private:
 	bool launch_detected;
 	bool usePreTakeoffThrust;
 
+	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
 	/* Landingslope object */
 	Landingslope landingslope;
 
@@ -344,6 +346,16 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+	/*
+	 * Reset takeoff state
+	 */
+	int reset_takeoff_state();
+
+	/*
+	 * Reset landing state
+	 */
+	int reset_landing_state();
 };
 
 namespace l1_control
@@ -389,6 +401,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	launch_detected(false),
+	last_manual(false),
 	usePreTakeoffThrust(false),
 	flare_curve_alt_last(0.0f),
 	launchDetector(),
@@ -1022,19 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		// mission is active
 		_loiter_hold = false;
 
-		/* reset land state */
+		/* reset landing state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
-			land_noreturn_horizontal = false;
-			land_noreturn_vertical = false;
-			land_stayonground = false;
-			land_motor_lim = false;
-			land_onslope = false;
+			reset_landing_state();
 		}
 
 		/* reset takeoff/launch state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
-			launch_detected = false;
-			usePreTakeoffThrust = false;
+			reset_takeoff_state();
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1131,6 +1139,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
+
+		/* reset landing and takeoff state */
+		if (!last_manual) {
+			reset_landing_state();
+			reset_takeoff_state();
+		}
 	}
 
 	if (usePreTakeoffThrust) {
@@ -1141,6 +1155,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
 
+	if (_control_mode.flag_control_position_enabled) {
+		last_manual = false;
+	} else {
+		last_manual = true;
+	}
+
 
 	return setpoint;
 }
@@ -1291,6 +1311,22 @@ FixedwingPositionControl::task_main()
 	_exit(0);
 }
 
+int FixedwingPositionControl::reset_takeoff_state()
+{
+	launch_detected = false;
+	usePreTakeoffThrust = false;
+	launchDetector.reset();
+}
+
+int FixedwingPositionControl::reset_landing_state()
+{
+	land_noreturn_horizontal = false;
+	land_noreturn_vertical = false;
+	land_stayonground = false;
+	land_motor_lim = false;
+	land_onslope = false;
+}
+
 int
 FixedwingPositionControl::start()
 {