fw pos control: improve mode logic slightly

1 files changed, 36 insertions, 26 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 1bb27168e..1592e70d8 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
@@ -200,9 +200,11 @@ private:
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
-	bool						_was_pos_control_mode;
-	bool						_was_velocity_control_mode;
-	bool						_was_alt_control_mode;
+	enum FW_POSCTRL_MODE {
+		FW_POSCTRL_MODE_AUTO,
+		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_OTHER
+	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
 	struct {
 		float l1_period;
@@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos_valid(false),
 	_l1_control(),
 	_mTecs(),
-	_was_pos_control_mode(false)
+	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -908,20 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
-	/* AUTONOMOUS FLIGHT */
 	if (_control_mode.flag_control_auto_enabled &&
 			pos_sp_triplet.current.valid) {
+		/* AUTONOMOUS FLIGHT */
 
-		if (!_was_pos_control_mode) {
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
-
-		_was_pos_control_mode = true;
-		_was_velocity_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_AUTO;
 
 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
@@ -1248,35 +1249,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	} else if (_control_mode.flag_control_velocity_enabled &&
 			_control_mode.flag_control_altitude_enabled) {
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+
 		const float deadBand = (60.0f/1000.0f);
 		const float factor = 1.0f - deadBand;
-		if (!_was_velocity_control_mode) {
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
+			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = false;
 		}
-		_was_velocity_control_mode = true;
-		_was_pos_control_mode = false;
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_POSITION;
+
 		// Get demanded airspeed
 		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.z;
 
 		// Get demanded vertical velocity from pitch control
-		float pitch = 0.0f;
+		static bool was_in_deadband = false;
 		if (_manual.x > deadBand) {
-			pitch = (_manual.x - deadBand) / factor;
-		} else if (_manual.x < - deadBand) {
-			pitch = (_manual.x + deadBand) / factor;
-		}
-		if (pitch > 0.0f) {
+			float pitch = (_manual.x - deadBand) / factor;
 			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (pitch < 0.0f) {
+			was_in_deadband = false;
+		} else if (_manual.x < - deadBand) {
+			float pitch = (_manual.x + deadBand) / factor;
 			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
-			_was_alt_control_mode = false;
-		} else if (!_was_alt_control_mode) {
+			was_in_deadband = false;
+		} else if (!was_in_deadband) {
+			 /* store altitude at which manual.x was inside deadBand
+			  * The aircraft should immediately try to fly at this altitude
+			  * as this is what the pilot expects when he moves the stick to the center */
 			_hold_alt = _global_pos.alt;
-			_was_alt_control_mode = true;
+			was_in_deadband = true;
 		}
 		tecs_update_pitch_throttle(_hold_alt,
 									altctrl_airspeed,
@@ -1292,8 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 									ground_speed,
 									TECS_MODE_NORMAL);
 	} else {
-		_was_velocity_control_mode = false;
-		_was_pos_control_mode = false;
+		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
 		/** MANUAL FLIGHT **/