Limit TECS integrators

src/lib/tecs/TECS.cpp

@@ -485,6 +485,11 @@ void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const Co
 		// Update the pitch integrator state.
 		_pitch_integ_state = _pitch_integ_state + pitch_integ_input * dt;
 
+		// Limit the integrator state
+		if (param.pitch_integrator_max > FLT_EPSILON) {
+			_pitch_integ_state = constrain(_pitch_integ_state, -param.pitch_integrator_max, param.pitch_integrator_max);
+		}
+
 	} else {
 		_pitch_integ_state = 0.0f;
 	}
@@ -598,6 +603,12 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit
 			// This will be added to the total throttle demand to compensate for steady state errors
 			_throttle_integ_state = PX4_ISFINITE(throttle_integ_input) ? _throttle_integ_state + throttle_integ_input : 0.f;
 
+			// Limit the integrator state
+			if (param.throttle_integrator_max > FLT_EPSILON) {
+				_throttle_integ_state = constrain(_throttle_integ_state, -param.throttle_integrator_max,
+								  param.throttle_integrator_max);
+			}
+
 		} else {
 			_throttle_integ_state = 0.0f;
 		}

src/lib/tecs/TECS.hpp

@@ -228,11 +228,13 @@ public:
 		float pitch_speed_weight;				///< Speed control weighting used by pitch demand calculation.
 		float integrator_gain_pitch;				///< Integrator gain used by the pitch demand calculation.
 		float pitch_damping_gain;				///< Damping gain of the pitch demand calculation [s].
+		float pitch_integrator_max;				///< Maximum pitch integrator state [rad].
 
 		// Throttle control param
 		float integrator_gain_throttle;				///< Integrator gain used by the throttle demand calculation.
 		float throttle_damping_gain;				///< Damping gain of the throttle demand calculation [s].
 		float throttle_slewrate;				///< Throttle demand slew rate limit [1/s].
+		float throttle_integrator_max;				///< Maximum throttle integrator state [-].
 
 		float load_factor_correction;				///< Gain from normal load factor increase to total energy rate demand [m²/s³].
 		float load_factor;					///< Additional normal load factor.
@@ -604,6 +606,8 @@ public:
 
 	void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
 	void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };
+	void set_throttle_integrator_max(float max) { _control_param.throttle_integrator_max = max; };
+	void set_pitch_integrator_max(float max) { _control_param.pitch_integrator_max = max; };
 
 	void set_max_sink_rate(float max_sink_rate) { _control_param.max_sink_rate = max_sink_rate; _reference_param.max_sink_rate = max_sink_rate; };
 	void set_min_sink_rate(float min_sink_rate) { _control_param.min_sink_rate = min_sink_rate; };
@@ -746,9 +750,11 @@ private:
 		.pitch_speed_weight = 1.0f,
 		.integrator_gain_pitch = 0.0f,
 		.pitch_damping_gain = 0.0f,
+		.pitch_integrator_max = 0.5f,
 		.integrator_gain_throttle = 0.0f,
 		.throttle_damping_gain = 0.0f,
 		.throttle_slewrate = 0.0f,
+		.throttle_integrator_max = 0.3f,
 		.load_factor_correction = 0.0f,
 		.load_factor = 1.0f,
 		.fast_descend = 0.f

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -97,7 +97,9 @@ FwLateralLongitudinalControl::parameters_update()
 	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
+	_tecs.set_throttle_integrator_max(_param_fw_t_thr_i_max.get());
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
+	_tecs.set_pitch_integrator_max(_param_fw_t_ptch_i_max.get());
 	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
 	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
 	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp

@@ -148,7 +148,9 @@ private:
 		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
 		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
 		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
+		(ParamFloat<px4::params::FW_T_THR_I_MAX>) _param_fw_t_thr_i_max,
 		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
+		(ParamFloat<px4::params::FW_T_PTCH_I_MAX>) _param_fw_t_ptch_i_max,
 		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
 		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
 		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,

src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.c

@@ -90,6 +90,21 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f);
  */
 PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f);
 
+/**
+ * Throttle integrator limit
+ *
+ * Maximum integrator state contribution to throttle output (in the range
+ * between minimum and maximum throttle). This prevents integrator windup,
+ * especially important in soaring mode where throttle authority may be limited.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.05
+ * @group FW Longitudinal Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_I_MAX, 0.3f);
+
 /**
  * Integrator gain pitch
  *
@@ -104,6 +119,21 @@ PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f);
  */
 PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
 
+/**
+ * Pitch integrator limit
+ *
+ * Maximum integrator state contribution to pitch output (in radians).
+ * This prevents integrator windup.
+ *
+ * @unit rad
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.05
+ * @group FW Longitudinal Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_PTCH_I_MAX, 0.5f);
+
 /**
  * Maximum vertical acceleration
  *