From 8823f5e1ec77d79806632a25b97849585844121b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 22 Sep 2020 11:49:34 +0300 Subject: [PATCH] TECS: added method to get SKE weighting Signed-off-by: RomanBapst --- src/lib/tecs/TECS.cpp | 27 +++++++++++++++++---------- src/lib/tecs/TECS.hpp | 2 ++ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index a35c92dc2b..404e93cf5b 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -398,18 +398,10 @@ void TECS::_update_pitch_setpoint() * The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements. */ - // Calculate the weighting applied to control of specific kinetic energy error - float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f); - - if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) { - SKE_weighting = 2.0f; - - } else if (!airspeed_sensor_enabled()) { - SKE_weighting = 0.0f; - } + const float SKE_weighting = get_SKE_weighting(); // Calculate the weighting applied to control of specific potential energy error - float SPE_weighting = 2.0f - SKE_weighting; + const float SPE_weighting = 2.0f - SKE_weighting; // Calculate the specific energy balance demand which specifies how the available total // energy should be allocated to speed (kinetic energy) and height (potential energy) @@ -617,3 +609,18 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float } } + +float TECS::get_SKE_weighting() +{ + // Calculate the weighting applied to control of specific kinetic energy error + float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f); + + if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) { + SKE_weighting = 2.0f; + + } else if (!airspeed_sensor_enabled()) { + SKE_weighting = 0.0f; + } + + return SKE_weighting; +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index c025824b14..0110428129 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -331,6 +331,8 @@ private: */ void _update_STE_rate_lim(); + float get_SKE_weighting(); + float _param_filt_speed_deriv{0.95f}; AlphaFilter _STE_rate_error_filter;