From 0f5e800be72906a5c44e9f17f8ef54679bb05a73 Mon Sep 17 00:00:00 2001 From: Cole Perschon Date: Tue, 31 Oct 2023 01:03:12 -0700 Subject: [PATCH] Move _TAScruise declaration to header --- libraries/AP_TECS/AP_TECS.cpp | 4 +++- libraries/AP_TECS/AP_TECS.h | 3 +++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 6da0f560ed96e4..ae843be8b5c1e6 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -470,11 +470,13 @@ void AP_TECS::_update_speed_demand(void) // Constrain speed demand, taking into account the load factor _TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax); + // Determine the true cruising airspeed (m/s) + _TAScruise = 0.01f * (float)aparm.airspeed_cruise_cm * _ahrs.get_EAS2TAS(); + // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists // Use 50% of maximum energy rate on gain, 90% on dissipation to allow margin for total energy controller const float velRateMax = 0.5f * _STEdot_max / _TAS_state; - const float _TAScruise = 0.01f * (float)aparm.airspeed_cruise_cm * _ahrs.get_EAS2TAS(); // Maximum permissible rate of deceleration value at max airspeed const float velRateNegMax = 0.9f * _STEdot_neg_max / _TASmax; // Maximum permissible rate of deceleration value at cruise speed diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index b392a0bfc555be..ecd65d4fa2f9df 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -266,6 +266,9 @@ class AP_TECS { float _TASmax; float _TASmin; + // True cruising airspeed (TRIM_ARSPD) + float _TAScruise; + // Current true airspeed demand float _TAS_dem;