TECS: separate integral gains for throttle and pitch loops

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/tecs/TECS.cpp

@@ -328,7 +328,7 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
 		throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
 		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
 
-		if (_integrator_gain > 0.0f) {
+		if (_integrator_gain_throttle > 0.0f) {
 			// Calculate throttle integrator state upper and lower limits with allowance for
 			// 10% throttle saturation to accommodate noise on the demand.
 			float integ_state_max = _throttle_setpoint_max - throttle_setpoint + 0.1f;
@@ -336,7 +336,8 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::
 
 			// Calculate a throttle demand from the integrated total energy error
 			// This will be added to the total throttle demand to compensate for steady state errors
-			_throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain) * _dt * STE_rate_to_throttle;
+			_throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain_throttle) * _dt *
+						STE_rate_to_throttle;
 
 			if (_climbout_mode_active) {
 				// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
@@ -427,9 +428,9 @@ void TECS::_update_pitch_setpoint()
 	// Calculate derivative from change in climb angle to rate of change of specific energy balance
 	const float climb_angle_to_SEB_rate = _tas_state * CONSTANTS_ONE_G;
 
-	if (_integrator_gain > 0.0f) {
+	if (_integrator_gain_pitch > 0.0f) {
 		// Calculate pitch integrator input term
-		float pitch_integ_input = _SEB_rate_error * _integrator_gain;
+		float pitch_integ_input = _SEB_rate_error * _integrator_gain_pitch;
 
 		// Prevent the integrator changing in a direction that will increase pitch demand saturation
 		// Decay the integrator using a 1 second time constant if the pitch demand from the previous time step is saturated

src/lib/tecs/TECS.hpp

@@ -102,7 +102,8 @@ public:
 	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
 
 	// setters for controller parameters
-	void set_integrator_gain(float gain) { _integrator_gain = gain; }
+	void set_integrator_gain_throttle(float gain) { _integrator_gain_throttle = gain; }
+	void set_integrator_gain_pitch(float gain) { _integrator_gain_pitch = gain; }
 
 	void set_min_sink_rate(float rate) { _min_sink_rate = rate; }
 	void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; }
@@ -206,7 +207,8 @@ private:
 	float _max_sink_rate{2.0f};					///< maximum safe sink rate (m/sec)
 	float _pitch_damping_gain{0.0f};				///< damping gain of the pitch demand calculation (sec)
 	float _throttle_damping_gain{0.0f};				///< damping gain of the throttle demand calculation (sec)
-	float _integrator_gain{0.0f};					///< integrator gain used by the throttle and pitch demand calculation
+	float _integrator_gain_throttle{0.0f};				///< integrator gain used by the throttle demand calculation
+	float _integrator_gain_pitch{0.0f};				///< integrator gain used by the pitch demand calculation
 	float _vert_accel_limit{0.0f};					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
 	float _load_factor_correction{0.0f};				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
 	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -100,7 +100,8 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
-	_tecs.set_integrator_gain(_param_fw_t_integ_gain.get());
+	_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
+	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
 	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
 	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
 	_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -396,7 +396,8 @@ private:
 		(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max,
 		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
 		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
-		(ParamFloat<px4::params::FW_T_INTEG_GAIN>) _param_fw_t_integ_gain,
+		(ParamFloat<px4::params::FW_T_I_GAIN_THR>) _param_fw_t_I_gain_thr,
+		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
 		(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_pos_control_l1/fw_pos_control_l1_params.c

@@ -535,9 +535,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.1f);
 
 /**
- * Integrator gain
+ * Integrator gain throttle
  *
- * This is the integrator gain on the control loop.
+ * This is the integrator gain on the throttle part of the control loop.
  * Increasing this gain increases the speed at which speed
  * and height offsets are trimmed out, but reduces damping and
  * increases overshoot. Set this value to zero to completely
@@ -549,7 +549,24 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.1f);
  * @increment 0.05
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
+PARAM_DEFINE_FLOAT(FW_T_I_GAIN_THR, 0.1f);
+
+/**
+ * Integrator gain pitch
+ *
+ * This is the integrator gain on the pitch part of the control loop.
+ * Increasing this gain increases the speed at which speed
+ * and height offsets are trimmed out, but reduces damping and
+ * increases overshoot. Set this value to zero to completely
+ * disable all integrator action.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.05
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
 
 /**
  * Maximum vertical acceleration