From 92cdff6798426dc1fa2bd3274e5e4cb76efecb8c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 7 Jul 2022 13:02:19 +0300 Subject: [PATCH] TECS: rename airspeed/throttle "cruise" to "trim" Signed-off-by: RomanBapst --- src/lib/tecs/TECS.cpp | 40 ++++++++++++++++++++++------------------ src/lib/tecs/TECS.hpp | 2 +- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 019f667d31..173ddd9ac8 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 airspeed estimate to the nominal cruise airspeed + // If airspeed measurements are not being used, fix the airspeed estimate to the nominal trim airspeed if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) { - _EAS = _equivalent_airspeed_cruise; + _EAS = _equivalent_airspeed_trim; } else { _EAS = equivalent_airspeed; @@ -248,11 +248,11 @@ void TECS::_update_energy_estimates() _SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change } -void TECS::_update_throttle_setpoint(const float throttle_cruise) +void TECS::_update_throttle_setpoint() { // Calculate demanded rate of change of total energy, respecting vehicle limits. // We will constrain the value below. - float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint; + _STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint; // Calculate the total energy rate error, applying a first order IIR filter // to reduce the effect of accelerometer noise @@ -270,24 +270,26 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise) // Adjust the demanded total energy rate to compensate for induced drag rise in turns. // Assume induced drag scales linearly with normal load factor. // The additional normal load factor is given by (1/cos(bank angle) - 1) - STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (_load_factor - 1.f); + _STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f); - STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max); + _STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max); - // Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle + // Calculate a predicted throttle from the demanded rate of change of energy, using the trim throttle // as the starting point. Assume: // Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max - // Specific total energy rate = 0 at cruise throttle + // Specific total energy rate = 0 at trim throttle // Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min float throttle_predicted = 0.0f; - if (STE_rate_setpoint >= 0) { - // throttle is between cruise and maximum - throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise); + if (_STE_rate_setpoint >= 0) { + // throttle is between trim and maximum + throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - + _throttle_trim); } else { - // throttle is between cruise and minimum - throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise); + // throttle is between trim and minimum + throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - + _throttle_trim); } @@ -488,7 +490,7 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat } } -void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout, +void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout, float EAS2TAS) { if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) { @@ -500,7 +502,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt _tas_state = _EAS * EAS2TAS; _throttle_integ_state = 0.0f; _pitch_integ_state = 0.0f; - _last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);; + _last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);; _last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max); _pitch_setpoint_unc = _last_pitch_setpoint; _TAS_setpoint_last = _EAS * EAS2TAS; @@ -559,7 +561,8 @@ void TECS::_update_STE_rate_lim() void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, - float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max, + float throttle_min, float throttle_max, float throttle_trim, + float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp) { // Calculate the time since last update (seconds) @@ -572,9 +575,10 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set _pitch_setpoint_max = pitch_limit_max; _pitch_setpoint_min = pitch_limit_min; _climbout_mode_active = climb_out_setpoint; + _throttle_trim = throttle_trim; // Initialize selected states and variables as required - _initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas); + _initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas); // Don't run TECS control algorithms when not in flight if (!_in_air) { @@ -606,7 +610,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set _update_energy_estimates(); // Calculate the throttle demand - _update_throttle_setpoint(throttle_cruise); + _update_throttle_setpoint(); // Calculate the pitch demand _update_pitch_setpoint(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index ea24dc8923..c3288b4856 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -95,6 +95,7 @@ public: float get_throttle_setpoint() { return _last_throttle_setpoint; } float get_pitch_setpoint() { return _last_pitch_setpoint; } float get_speed_weight() { return _pitch_speed_weight; } + float get_throttle_trim() { return _throttle_trim; } void reset_state() { _states_initialized = false; } @@ -140,7 +141,6 @@ public: void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; } - // TECS status uint64_t timestamp() { return _pitch_update_timestamp; } ECL_TECS_MODE tecs_mode() { return _tecs_mode; }