diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index 0f1261c5b9..d6207a1511 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -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 diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index de5f4cd73d..1a6f7348e5 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -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; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index bf8b92b144..eac8d07492 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -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); }; diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index e3da655ba7..f8b639a516 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -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;