fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: ramp up fast descend over 2_s to ramp down the throttle command

Browse Source
This commit is contained in:
Konrad 2024-09-05 17:08:53 +02:00 committed by KonradRudin
parent 585c92796d
commit 4713a6737e
4 changed files with 50 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/TecsStatus.msg
View File

@ -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]

36
src/lib/tecs/TECS.cpp
View File

@ -44,11 +44,11 @@
#include "matrix/Matrix.hpp"
#include "matrix/Vector2.hpp"
#include <mathlib/math/Functions.hpp>
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<float>(now - _enabled_fast_descend_timestamp) /
static_cast<float>(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;
}
}

27
src/lib/tecs/TECS.hpp
View File

@ -50,6 +50,8 @@
#include <motion_planning/VelocitySmoothing.hpp>
#include <motion_planning/ManualVelocitySmoothingZ.hpp>
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 &param,
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{};

3
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -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();