diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 718ed8c225..a35c92dc2b 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -91,10 +91,11 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { // Assuming the vehicle is flying X axis forward, use the X axis measured acceleration // compensated for gravity to estimate the rate of change of speed - float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); + const float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); // Apply some noise filtering - _speed_derivative = 0.95f * _speed_derivative + 0.05f * speed_deriv_raw; + _TAS_rate_filter.update(speed_deriv_raw); + _speed_derivative = _TAS_rate_filter.getState(); } else { _speed_derivative = 0.0f; @@ -271,7 +272,8 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix:: // Calculate the total energy rate error, applying a first order IIR filter // to reduce the effect of accelerometer noise - _STE_rate_error = 0.2f * (STE_rate_setpoint - _SPE_rate - _SKE_rate) + 0.8f * _STE_rate_error; + _STE_rate_error_filter.update(-_SPE_rate - _SKE_rate); + _STE_rate_error = _STE_rate_error_filter.getState(); // Calculate the throttle demand if (_underspeed_detected) { @@ -526,6 +528,14 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt _uncommanded_descent_recovery = false; } + // filter specific energy rate error using first order filter with 0.5 second time constant + _STE_rate_error_filter.setParameters(DT_DEFAULT, _STE_rate_time_const); + _STE_rate_error_filter.reset(0.0f); + + // filter true airspeed rate using first order filter with 0.5 second time constant + _TAS_rate_filter.setParameters(DT_DEFAULT, _speed_derivative_time_const); + _TAS_rate_filter.reset(0.0f); + _states_initialized = true; } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index ef491ecbe7..c025824b14 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -41,6 +41,7 @@ #include #include +#include class TECS { @@ -127,6 +128,10 @@ public: void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; } + 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; } + + // TECS status uint64_t timestamp() { return _pitch_update_timestamp; } ECL_TECS_MODE tecs_mode() { return _tecs_mode; } @@ -199,6 +204,8 @@ private: float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) 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) // controller outputs float _throttle_setpoint{0.0f}; ///< normalized throttle demand (0..1) @@ -324,4 +331,10 @@ private: */ void _update_STE_rate_lim(); + float _param_filt_speed_deriv{0.95f}; + + AlphaFilter _STE_rate_error_filter; + + AlphaFilter _TAS_rate_filter; + }; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index a875056773..699dbd1413 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -111,6 +111,9 @@ FixedwingPositionControl::parameters_update() _tecs.set_heightrate_p(_param_fw_t_hrate_p.get()); _tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get()); _tecs.set_speedrate_p(_param_fw_t_srate_p.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()); + // Landing slope /* check if negative value for 2/3 of flare altitude is set for throttle cut */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 66403a435d..f5bea730f4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -408,6 +408,8 @@ private: (ParamFloat) _param_fw_t_thro_const, (ParamFloat) _param_fw_t_time_const, (ParamFloat) _param_fw_t_vert_acc, + (ParamFloat) _param_ste_rate_time_const, + (ParamFloat) _param_tas_rate_time_const, (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 5df640bf44..ab1e01e10a 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 @@ -737,3 +737,30 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); * @group FW L1 Control */ PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0); + +/** + * Specific total energy rate first order filter time constant. + * + * This filter is applied to the specific total energy rate used for throttle damping. + * + * @min 0.0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f); + + +/** + * True airspeed rate first order filter time constant. + * + * This filter is applied to the true airspeed rate. + * + * @min 0.0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.4f);