dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Konrad 2023-11-20 20:42:30 +01:00
parent 0f3925b21d
commit 29145ed941
5 changed files with 93 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -239,7 +239,7 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param
_detectUnderspeed(input, param, flag);
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)};
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)};
_pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag);
@ -381,7 +381,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param &param, cons
math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
}
TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param &param, const Flag &flag)
TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Input &input, const Param &param,
const Flag &flag)
{
SpecificEnergyWeighting weight;
@ -394,6 +395,10 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
} else if (!flag.airspeed_enabled) {
pitch_speed_weight = 0.0f;
} else if (flag.fast_descend) {
// pitch loop controls the airspeed to max
pitch_speed_weight = 2.0;
}
// don't allow any weight to be larger than one, as it has the same effect as reducing the control
@ -405,10 +410,9 @@ 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)};
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)};
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
_calcPitchControlUpdate(dt, input, seb_rate, param);
@ -514,20 +518,25 @@ 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)};
// Rate limit the throttle demand
if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
_throttle_setpoint + throttle_increment_limit);
if (flag.fast_descend) {
_throttle_setpoint = param.throttle_min;
} else {
_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
// Rate limit the throttle demand
if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
_throttle_setpoint + throttle_increment_limit);
}
_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
}
_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
// Debug output
_debug_output.total_energy_rate_estimate = ste_rate.estimate;
_debug_output.total_energy_rate_sp = ste_rate.setpoint;
@ -651,6 +660,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 +715,10 @@ 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. */
const bool is_fast_descend{_checkFastDescend(hgt_setpoint, altitude)};
// Update airspeedfilter submodule
const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
@ -712,10 +726,19 @@ 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 (is_fast_descend) {
// Reset the altitude reference model.
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();
@ -727,6 +750,12 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
.tas = eas_to_tas * _airspeed_filter.getState().speed,
.tas_rate = eas_to_tas * _airspeed_filter.getState().speed_rate};
if (is_fast_descend) {
control_setpoint.tas_setpoint = _control_param.tas_max;
}
_control_flag.fast_descend = is_fast_descend;
_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);
}
@ -740,3 +769,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
_update_timestamp = now;
}
bool TECS::_checkFastDescend(const float alt_setpoint, const float alt)
{
bool ret_val{false};
if ((_fast_descend_alt_err > FLT_EPSILON) && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
ret_val = true;
}
return ret_val;
}

31
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]
@ -278,6 +279,7 @@ public:
struct Flag {
bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled.
bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled.
bool fast_descend; ///< Flag if descending with higher airspeed is allowed.
};
public:
TECSControl() = default;
@ -393,7 +395,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 +406,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 +415,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.
*/
@ -428,11 +430,12 @@ private:
/**
* @brief Update specific energy balance weights.
*
* @param input is the current input measurement of the UAS.
* @param param is the control parameters.
* @param flag is the control flags.
* @return Weights used for the specific energy balance.
*/
SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param &param, const Flag &flag);
SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Input &input, const Param &param, const Flag &flag);
/**
* @brief Calculate pitch control.
*
@ -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,9 @@ 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].
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 +704,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,
@ -722,5 +730,14 @@ private:
.airspeed_enabled = false,
.detect_underspeed_enabled = false,
};
/**
* @brief Check if fast descend should be used
*
* @param alt_setpoint is the altitude setpoint
* @param alt is the
* @return true if fast descend should be enabled
*/
bool _checkFastDescend(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

@ -957,6 +957,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
*