From 4713a6737ee660af113bd412671a62a2fb8a755a Mon Sep 17 00:00:00 2001 From: Konrad Date: Thu, 5 Sep 2024 17:08:53 +0200 Subject: [PATCH] TECS: ramp up fast descend over 2_s to ramp down the throttle command --- msg/TecsStatus.msg | 1 + src/lib/tecs/TECS.cpp | 36 +++++++++++++------ src/lib/tecs/TECS.hpp | 27 +++++++++++--- .../FixedwingPositionControl.cpp | 3 +- 4 files changed, 50 insertions(+), 17 deletions(-) diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index ae6835dc52..af3826b155 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -27,3 +27,4 @@ float32 pitch_sp_rad # Current pitch setpoint [rad] float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions float32 underspeed_ratio # 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0. +float32 fast_descend_ratio # value indicating if fast descend mode is enabled with ramp up and ramp down [0-1] diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 4a6d3593e8..6a09b87759 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -44,11 +44,11 @@ #include "matrix/Matrix.hpp" #include "matrix/Vector2.hpp" +#include using math::constrain; using math::max; using math::min; -using namespace time_literals; static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);} @@ -525,16 +525,12 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec if (1.f - param.fast_descend < FLT_EPSILON) { // During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending - if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing - throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet - - } else { - throttle_setpoint = param.throttle_min; - } + throttle_setpoint = param.throttle_min; } else { _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); - throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag); + throttle_setpoint = (1.f - param.fast_descend) * _calcThrottleControlOutput(limit, ste_rate, param, + flag) + param.fast_descend * param.throttle_min; } // Rate limit the throttle demand @@ -677,6 +673,11 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa _control_param.throttle_min = throttle_min; } +float TECS::calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint) +{ + return lerp(eas_to_tas * eas_setpoint, _control_param.tas_max, _fast_descend); +} + void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed, float eas_to_tas) { @@ -699,6 +700,9 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo .tas_rate = 0.0f}; _control.initialize(control_setpoint, control_input, _control_param, _control_flag); + + _fast_descend = 0.f; + _enabled_fast_descend_timestamp = 0U; } void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, @@ -753,8 +757,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect(); - control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas * - EAS_setpoint; + control_setpoint.tas_setpoint = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint); const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, @@ -765,11 +768,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set } _debug_status.control = _control.getDebugOutput(); + _debug_status.true_airspeed_sp = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint); _debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed; _debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate; _debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt; _debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate; _debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect(); + _debug_status.fast_descend = _fast_descend; _update_timestamp = now; } @@ -778,13 +783,22 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt) { if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON) && ((alt_setpoint + _fast_descend_alt_err) < alt)) { - _fast_descend = 1.f; + auto now = hrt_absolute_time(); + + if (_enabled_fast_descend_timestamp == 0U) { + _enabled_fast_descend_timestamp = now; + } + + _fast_descend = constrain(max(_fast_descend, static_cast(now - _enabled_fast_descend_timestamp) / + static_cast(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f); } else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { // Were in fast descend, scale it down. up until 5m above target altitude _fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f); + _enabled_fast_descend_timestamp = 0U; } else { _fast_descend = 0.f; + _enabled_fast_descend_timestamp = 0U; } } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index f033bf72da..57cc12690b 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -50,6 +50,8 @@ #include #include +using namespace time_literals; + class TECSAirspeedFilter { public: @@ -203,8 +205,8 @@ public: float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s]. float tas_min; ///< True airspeed demand lower limit [m/s]. float tas_max; ///< True airspeed demand upper limit [m/s]. - float pitch_max; ///< Maximum pitch angle allowed in [rad]. - float pitch_min; ///< Minimal pitch angle allowed in [rad]. + float pitch_max; ///< Maximum pitch angle above trim allowed in [rad]. + float pitch_min; ///< Minimal pitch angle below trim allowed in [rad]. float throttle_trim; ///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1] float throttle_max; ///< Normalized throttle upper limit. float throttle_min; ///< Normalized throttle lower limit. @@ -320,7 +322,7 @@ public: /** * @brief Get the pitch setpoint. * - * @return THe commanded pitch angle in [rad]. + * @return The commanded pitch angle above trim in [rad]. */ float getPitchSetpoint() const {return _pitch_setpoint;}; /** @@ -478,7 +480,7 @@ private: * @param seb_rate is the specific energy balance rate in [m²/s³]. * @param param is the control parameters. * @param flag is the control flags. - * @return pitch setpoint angle [rad]. + * @return pitch setpoint angle above trim [rad]. */ float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param ¶m, const Flag &flag) const; @@ -537,7 +539,7 @@ private: // Output DebugOutput _debug_output; ///< Debug output. - float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint [rad]. + float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint above trim [rad]. float _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint [0,1]. float _ratio_undersped{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1] }; @@ -547,11 +549,13 @@ class TECS public: struct DebugOutput { TECSControl::DebugOutput control; + float true_airspeed_sp; float true_airspeed_filtered; float true_airspeed_derivative; float altitude_reference; float height_rate_reference; float height_rate_direct; + float fast_descend; }; public: TECS() = default; @@ -658,6 +662,17 @@ private: void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max, float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim); + /** + * @brief calculate true airspeed setpoint + * + * Calculate true airspeed setpoint based on input and fast descend ratio + * + * @param eas_to_tas is the eas to tas conversion factor + * @param eas_setpoint is the desired equivalent airspeed setpoint [m/s] + * @return true airspeed setpoint[m/s] + */ + float calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint); + /** * @brief Initialize the control loop * @@ -675,9 +690,11 @@ private: float _equivalent_airspeed_max{20.0f}; ///< equivalent airspeed demand upper limit (m/sec) float _fast_descend_alt_err{-1.f}; ///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m]. float _fast_descend{0.f}; ///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude. + hrt_abstime _enabled_fast_descend_timestamp{0U}; ///< timestamp at activation of fast descend mode static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) + static constexpr hrt_abstime FAST_DESCEND_RAMP_UP_TIME = 2_s; ///< Ramp up time until fast descend is fully engaged DebugOutput _debug_status{}; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 00e7869c69..fcd5ce5539 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -424,7 +424,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control; tecs_status.height_rate = -_local_pos.vz; tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp; - tecs_status.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp; + tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp; tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered; tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control; tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative; @@ -439,6 +439,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint(); tecs_status.throttle_trim = throttle_trim; tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio(); + tecs_status.fast_descend_ratio = debug_output.fast_descend; tecs_status.timestamp = hrt_absolute_time();