From 357842b8b95594a9ff84a5e36e1780cc3e21be24 Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 12 Jan 2024 12:52:39 +0100 Subject: [PATCH] TECS: fast descend cleanup --- src/lib/tecs/TECS.cpp | 8 ++++---- src/lib/tecs/TECS.hpp | 3 +-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 30f7822885..9229350063 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -239,7 +239,7 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param _detectUnderspeed(input, param, flag); - const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)}; + const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)}; _pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag); @@ -381,8 +381,7 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, cons math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f); } -TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Input &input, const Param ¶m, - const Flag &flag) +TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag) { SpecificEnergyWeighting weight; @@ -412,7 +411,7 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates, const Param ¶m, const Flag &flag) { - const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)}; + const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; _calcPitchControlUpdate(dt, input, seb_rate, param); @@ -521,6 +520,7 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)}; if (flag.fast_descend) { + // During fast descend, we control airspeed over the pitch control loop and give minimal thrust. _throttle_setpoint = param.throttle_min; } else { diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 88baed65ba..e5ef9c9cd0 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -430,12 +430,11 @@ private: /** * @brief Update specific energy balance weights. * - * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @param flag is the control flags. * @return Weights used for the specific energy balance. */ - SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Input &input, const Param ¶m, const Flag &flag); + SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag); /** * @brief Calculate pitch control. *