From 4b42a1639b164b66f5766274671e92ce25aad9c5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sun, 6 Dec 2020 13:29:13 +0100 Subject: [PATCH] TECS: in airspeed-less mode, set airspeed estimate to cruise airspeed In airspeed-less mode, instead of the average of min/max airspeed, take the cruise airspeed (from param) for current airspeed estimate. A diff of the airspeed setpoint from this value results in increased enegery demand (incerase of throttle) even in airspeed-less mode. Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 4 ++-- src/lib/tecs/TECS.hpp | 2 ++ src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 0361c32fe4..26695120fb 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -116,9 +116,9 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva _TAS_max = _equivalent_airspeed_max * EAS2TAS; _TAS_min = _equivalent_airspeed_min * EAS2TAS; - // If airspeed measurements are not being used, fix the EAS estimate to halfway between min and max limits + // If airspeed measurements are not being used, fix the airspeed estimate to the nominal cruise airspeed if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) { - _EAS = 0.5f * (_equivalent_airspeed_min + _equivalent_airspeed_max); + _EAS = _equivalent_airspeed_cruise; } else { _EAS = equivalent_airspeed; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 207991a973..67ffd93a7d 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -114,6 +114,7 @@ public: void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; } void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; } + void set_equivalent_airspeed_cruise(float airspeed) { _equivalent_airspeed_cruise = airspeed; } void set_pitch_damping(float damping) { _pitch_damping_gain = damping; } void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } @@ -223,6 +224,7 @@ private: float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s] float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) + float _equivalent_airspeed_cruise{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (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) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 53dcba255d..c438351d92 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -109,6 +109,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_max_climb_rate(_param_fw_t_clmb_max.get()); _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); _tecs.set_speed_weight(_param_fw_t_spdweight.get()); + _tecs.set_equivalent_airspeed_cruise(_param_fw_airspd_trim.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); _tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get()); _tecs.set_min_sink_rate(_param_fw_t_sink_min.get());