UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: allow for fast descend up to maximum airspeed. Use pitch control loop to control max airspeed while giving minimal throttle.

Browse Source
This commit is contained in:
Konrad
2023-11-20 20:42:30 +01:00
committed by KonradRudin
parent e17faece3d
commit 6a789b54c6
5 changed files with 85 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -394,6 +394,10 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
} else if (!flag.airspeed_enabled) {
pitch_speed_weight = 0.0f;
} else if (param.fast_descend > FLT_EPSILON) {
// pitch loop controls the airspeed to max
pitch_speed_weight = 1.f + param.fast_descend;
}
// don't allow any weight to be larger than one, as it has the same effect as reducing the control
@@ -405,8 +409,7 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
}
void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates,
const Param &param,
const Flag &flag)
const Param &param, const Flag &flag)
{
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
@@ -514,10 +517,17 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
float throttle_setpoint{param.throttle_min};
if (1.f - param.fast_descend < FLT_EPSILON) {
// During fast descend, we control airspeed over the pitch control loop and give minimal thrust.
throttle_setpoint = param.throttle_min;
} else {
_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
}
// Rate limit the throttle demand
if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
@@ -651,6 +661,7 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
_reference_param.target_sinkrate = target_sinkrate;
// Control
_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
_control_param.tas_max = eas_to_tas * _equivalent_airspeed_max;
_control_param.pitch_max = pitch_limit_max;
_control_param.pitch_min = pitch_limit_min;
_control_param.throttle_trim = throttle_trim;
@@ -705,6 +716,11 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);
} else {
/* Check if we want to fast descend. On fast descend, we set the throttle to min, and use the altitude control
loop to control the speed to the maximum airspeed. */
_setFastDescend(hgt_setpoint, altitude);
_control_param.fast_descend = _fast_descend;
// Update airspeedfilter submodule
const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
@@ -712,15 +728,25 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);
// Update Reference model submodule
const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
.alt_rate = hgt_rate_sp};
if (1.f - _fast_descend < FLT_EPSILON) {
// Reset the altitude reference model, while we are in fast descend.
const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{
.alt = altitude,
.alt_rate = hgt_rate};
_altitude_reference_model.initialize(init_state);
_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
} else {
const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
.alt_rate = hgt_rate_sp};
_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
}
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 = eas_to_tas * EAS_setpoint;
control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
EAS_setpoint;
const TECSControl::Input control_input{ .altitude = altitude,
.altitude_rate = hgt_rate,
@@ -740,3 +766,17 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
_update_timestamp = now;
}
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;
} 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);
} else {
_fast_descend = 0.f;
}
}

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

@@ -202,6 +202,7 @@ public:
float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²].
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 throttle_trim; ///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
@@ -233,6 +234,8 @@ public:
float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³].
float load_factor; ///< Additional normal load factor.
float fast_descend;
};
/**
@@ -393,7 +396,7 @@ private:
* @brief calculate airspeed control proportional output.
*
* @param setpoint is the control setpoints.
* @param input is the current input measurment of the UAS.
* @param input is the current input measurement of the UAS.
* @param param is the control parameters.
* @param flag is the control flags.
* @return controlled airspeed rate setpoint in [m/s²].
@@ -404,7 +407,7 @@ private:
* @brief calculate altitude control proportional output.
*
* @param setpoint is the control setpoints.
* @param input is the current input measurment of the UAS.
* @param input is the current input measurement of the UAS.
* @param param is the control parameters.
* @return controlled altitude rate setpoint in [m/s].
*/
@@ -413,14 +416,14 @@ private:
* @brief Calculate specific energy rates.
*
* @param control_setpoint is the controlles altitude and airspeed rate setpoints.
* @param input is the current input measurment of the UAS.
* @param input is the current input measurement of the UAS.
* @return Specific energy rates in [m²/s³].
*/
SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
/**
* @brief Detect underspeed.
*
* @param input is the current input measurment of the UAS.
* @param input is the current input measurement of the UAS.
* @param param is the control parameters.
* @param flag is the control flags.
*/
@@ -602,9 +605,11 @@ public:
void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };
void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };
void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f); };
void set_fast_descend_altitude_error(float altitude_error) { _fast_descend_alt_err = altitude_error; };
void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }
void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
@@ -665,7 +670,10 @@ private:
hrt_abstime _update_timestamp{0}; ///< last timestamp of the update function call.
float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
float _equivalent_airspeed_min{10.0f}; ///< equivalent airspeed demand lower limit (m/sec)
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.
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)
@@ -697,6 +705,7 @@ private:
.vert_accel_limit = 0.0f,
.equivalent_airspeed_trim = 15.0f,
.tas_min = 10.0f,
.tas_max = 20.0f,
.pitch_max = 0.5f,
.pitch_min = -0.5f,
.throttle_trim = 0.0f,
@@ -716,11 +725,20 @@ private:
.throttle_slewrate = 0.0f,
.load_factor_correction = 0.0f,
.load_factor = 1.0f,
.fast_descend = 0.f
};
TECSControl::Flag _control_flag{
.airspeed_enabled = false,
.detect_underspeed_enabled = false,
};
/**
* @brief Set fast descend value
*
* @param alt_setpoint is the altitude setpoint
* @param alt is the current altitude
*/
void _setFastDescend(float alt_setpoint, float alt);
};

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

@@ -129,6 +129,7 @@ FixedwingPositionControl::parameters_update()
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
@@ -137,6 +138,7 @@ FixedwingPositionControl::parameters_update()
_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
_tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get());
_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());

1
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@@ -964,6 +964,7 @@ private:
(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,

9
src/modules/fw_pos_control/fw_path_navigation_params.c
View File

@@ -633,6 +633,15 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f);
*/
PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);
/**
* Minimum altitude error needed to descend with max airspeed. A negative value disables fast descend.
*
* @min -1.0
* @decimal 0
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f);
/**
* Height rate feed forward
*