diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 6306618573..3579fe42b8 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -436,7 +436,8 @@ void TECS::_update_pitch_setpoint() } // Calculate a specific energy correction that doesn't include the integrator contribution - float SEB_rate_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint; + float SEB_rate_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + _SEB_rate_ff * + SEB_rate_setpoint; // During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle // demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 2c39d931ce..f3ced9bfa3 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -131,6 +131,8 @@ public: void set_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; } void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; } + void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } + // TECS status uint64_t timestamp() { return _pitch_update_timestamp; } @@ -224,6 +226,7 @@ private: float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec) float _STE_rate_time_const{0.1f}; ///< filter time constant for specific total energy rate (damping path) (s) float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) + float _SEB_rate_ff{1.0f}; // complimentary filter states float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 54a4c0f718..7efea241ec 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -125,6 +125,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); + _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); // Landing slope diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index e376015dde..d6080394cd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -394,6 +394,7 @@ private: (ParamFloat) _param_fw_t_vert_acc, (ParamFloat) _param_ste_rate_time_const, (ParamFloat) _param_tas_rate_time_const, + (ParamFloat) _param_seb_rate_ff, (ParamFloat) _param_fw_thr_alt_scl, (ParamFloat) _param_fw_thr_cruise, 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 75475a76f6..26856cb788 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 @@ -761,3 +761,16 @@ PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f); * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f); + + +/** + * Specific total energy balance rate feedforward gain. + * + * + * @min 0.5 + * @max 3 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f);