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: remove umcommanded_descent flag

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-02-27 18:08:01 +01:00
parent 527225357b
commit 76116d79f9
4 changed files with 3 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/TecsStatus.msg
View File

@ -28,7 +28,3 @@ float32 throttle_trim # estimated throttle value [0,1] required to fly level a
uint8 mode
uint8 TECS_MODE_NORMAL = 0
uint8 TECS_MODE_UNDERSPEED = 1
uint8 TECS_MODE_TAKEOFF = 2
uint8 TECS_MODE_LAND = 3
uint8 TECS_MODE_LAND_THROTTLELIM = 4
uint8 TECS_MODE_BAD_DESCENT = 5

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

@ -619,12 +619,8 @@ float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, con
float new_setpoint{tas_setpoint};
const float percent_undersped = _control.getRatioUndersped();
// Set the TAS demand to the minimum value if an underspeed or
// or a uncontrolled descent condition exists to maximise climb rate
if (_uncommanded_descent_recovery) {
new_setpoint = tas_min;
} else if (percent_undersped > FLT_EPSILON) {
// Set the TAS demand to the minimum value if an underspeed condition exists to maximise climb rate
if (percent_undersped > FLT_EPSILON) {
// TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still
// between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint
// from this line back into this method each time).
@ -638,47 +634,6 @@ float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, con
return new_setpoint;
}
void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
float tas_setpoint)
{
/*
* This function detects a condition that can occur when the demanded airspeed is greater than the
* aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce altitude
* while attempting to maintain speed.
*/
// Calculate specific energy demands in units of (m**2/sec**2)
const float SPE_setpoint = altitude_setpoint * CONSTANTS_ONE_G; // potential energy
const float SKE_setpoint = 0.5f * altitude_setpoint * altitude_setpoint; // kinetic energy
// Calculate specific energies in units of (m**2/sec**2)
const float SPE_estimate = altitude * CONSTANTS_ONE_G; // potential energy
const float SKE_estimate = 0.5f * tas * tas; // kinetic energy
// Calculate total energy error
const float SPE_error = SPE_setpoint - SPE_estimate;
const float SKE_error = SKE_setpoint - SKE_estimate;
const float STE_error = SPE_error + SKE_error;
const bool underspeed_detected = _control.getRatioUndersped() > FLT_EPSILON;
// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f)
&& (_control.getSteRate() < 0.0f)
&& (_control.getThrottleSetpoint() >= throttle_setpoint_max * 0.9f);
// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (STE_error < 0.0f));
if (enter_mode) {
_uncommanded_descent_recovery = true;
} else if (exit_mode) {
_uncommanded_descent_recovery = false;
}
}
void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
const float eas_to_tas)
{
@ -774,10 +729,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);
// Detect an uncommanded descent caused by an unachievable airspeed demand
_detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed * eas_to_tas,
control_setpoint.tas_setpoint);
// Update time stamps
_update_timestamp = now;
@ -786,9 +737,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
if (_control.getRatioUndersped() > FLT_EPSILON) {
_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_uncommanded_descent_recovery) {
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
} else {
// This is the default operation mode
_tecs_mode = ECL_TECS_MODE_NORMAL;

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

@ -543,8 +543,7 @@ class TECS
public:
enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT
ECL_TECS_MODE_UNDERSPEED
};
struct DebugOutput {
@ -670,9 +669,6 @@ private:
float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec)
float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec)
// controller mode logic
bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected
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)
@ -733,11 +729,5 @@ private:
* Update the desired airspeed
*/
float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas);
/**
* Detect an uncommanded descent
*/
void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas,
float tas_setpoint);
};

4
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -473,10 +473,6 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
case TECS::ECL_TECS_MODE_UNDERSPEED:
t.mode = tecs_status_s::TECS_MODE_UNDERSPEED;
break;
case TECS::ECL_TECS_MODE_BAD_DESCENT:
t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT;
break;
}
t.altitude_sp = alt_sp;