From 6984e6da7f770971fcb16779014a5452416df811 Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 13 Feb 2024 14:57:08 +0100 Subject: [PATCH] TECS:use tas_setpoint instead of measured tas for specific kinetic energy calculation --- src/lib/tecs/TECS.cpp | 5 ++++- src/lib/tecs/TECS.hpp | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b6ac0d8fcad5..0c264706831d 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -231,6 +231,8 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param AltitudePitchControl control_setpoint; + control_setpoint.tas_setpoint = setpoint.tas_setpoint; + control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param); @@ -274,6 +276,7 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input & AltitudePitchControl control_setpoint; + control_setpoint.tas_setpoint = setpoint.tas_setpoint; control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) { @@ -350,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt // Calculate specific energy rate demands in units of (m**2/sec**3) specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change - specific_energy_rates.ske_rate.setpoint = input.tas * + specific_energy_rates.ske_rate.setpoint = control_setpoint.tas_setpoint * control_setpoint.tas_rate_setpoint; // kinetic energy rate of change // Calculate specific energy rates in units of (m**2/sec**3) diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 64881fb17f31..3c16b97066e8 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -366,6 +366,7 @@ class TECSControl struct AltitudePitchControl { float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s]. float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²]. + float tas_setpoint; ///< Controller true airspeed setpoint [m/s] }; /**