diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 3f9b67aa30..ea509bd3bd 100644 --- a/src/lib/tecs/TECS.cpp +++ b/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 diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 1f7e6694f4..4fe4a312ac 100644 --- a/src/lib/tecs/TECS.hpp +++ b/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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b714e97d1e..77897189e1 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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()); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1329d46c9f..b6804ed79c 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -396,7 +396,8 @@ private: (ParamFloat) _param_fw_t_clmb_max, (ParamFloat) _param_fw_t_hrate_ff, (ParamFloat) _param_fw_t_h_error_tc, - (ParamFloat) _param_fw_t_integ_gain, + (ParamFloat) _param_fw_t_I_gain_thr, + (ParamFloat) _param_fw_t_I_gain_pit, (ParamFloat) _param_fw_t_ptch_damp, (ParamFloat) _param_fw_t_rll2thr, (ParamFloat) _param_fw_t_sink_max, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 89ceea71ff..004aa54c31 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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