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

FWPosCtrl: Make fast_descend fade out

This commit is contained in:
Konrad 2024-02-13 09:39:36 +01:00
parent 5321da1a3b
commit 8ec0187384
2 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -394,9 +394,9 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
} else if (!flag.airspeed_enabled) {
pitch_speed_weight = 0.0f;
} else if (flag.fast_descend) {
} else if (param.fast_descend > FLT_EPSILON) {
// pitch loop controls the airspeed to max
pitch_speed_weight = 2.0;
pitch_speed_weight = 1.f + param.fast_descend;
}
@ -517,7 +517,7 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
if (flag.fast_descend) {
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;
@ -715,7 +715,8 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
} 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)};
_setFastDescend(hgt_setpoint, altitude);
_control_param.fast_descend = _fast_descend;
// Update airspeedfilter submodule
const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
@ -724,8 +725,8 @@ 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
if (is_fast_descend) {
// Reset the altitude reference model.
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};
@ -741,19 +742,14 @@ 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 = 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,
.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);
}
@ -767,14 +763,17 @@ 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)
void TECS::_setFastDescend(const float alt_setpoint, const float alt)
{
bool ret_val{false};
if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
&& ((alt_setpoint + _fast_descend_alt_err) < alt)) {
ret_val = true;
}
_fast_descend = 1.f;
return ret_val;
} 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;
}
}

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

@ -234,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;
};
/**
@ -279,7 +281,6 @@ 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;
@ -672,6 +673,7 @@ private:
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)
@ -723,21 +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,
.fast_descend = false
};
/**
* @brief Check if fast descend should be used
* @brief Set fast descend value
*
* @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);
void _setFastDescend(float alt_setpoint, float alt);
};