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

TECS: fast descend cleanup

This commit is contained in:
Konrad 2024-01-12 12:52:39 +01:00
parent 042257c33f
commit 357842b8b9
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
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); _detectUnderspeed(input, param, flag);
const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)}; const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)};
_pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag); _pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag);
@ -381,8 +381,7 @@ 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); math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
} }
TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Input &input, const Param &param, TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param &param, const Flag &flag)
const Flag &flag)
{ {
SpecificEnergyWeighting weight; SpecificEnergyWeighting weight;
@ -412,7 +411,7 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates, 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(input, param, flag)}; const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
_calcPitchControlUpdate(dt, input, seb_rate, param); _calcPitchControlUpdate(dt, input, seb_rate, param);
@ -521,6 +520,7 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)}; ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
if (flag.fast_descend) { if (flag.fast_descend) {
// During fast descend, we control airspeed over the pitch control loop and give minimal thrust.
_throttle_setpoint = param.throttle_min; _throttle_setpoint = param.throttle_min;
} else { } else {

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

@ -430,12 +430,11 @@ private:
/** /**
* @brief Update specific energy balance weights. * @brief Update specific energy balance weights.
* *
* @param input is the current input measurement of the UAS.
* @param param is the control parameters. * @param param is the control parameters.
* @param flag is the control flags. * @param flag is the control flags.
* @return Weights used for the specific energy balance. * @return Weights used for the specific energy balance.
*/ */
SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Input &input, const Param &param, const Flag &flag); SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param &param, const Flag &flag);
/** /**
* @brief Calculate pitch control. * @brief Calculate pitch control.
* *