From 8741a9784d51c7a130c8267d84e3e3371c5f6a4e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 13 Jul 2023 11:18:54 +0200 Subject: [PATCH] TECS: remove TECS_MODE enum and instead add descriptive boolean flag to tecs_status New flag: underspeed_mode_enabled. Signed-off-by: Silvan Fuhrer --- msg/TecsStatus.msg | 5 +---- src/lib/tecs/TECS.cpp | 12 ------------ src/lib/tecs/TECS.hpp | 10 +--------- .../fw_pos_control/FixedwingPositionControl.cpp | 11 +---------- 4 files changed, 3 insertions(+), 35 deletions(-) diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index e823af4c64..8b220703cf 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -26,7 +26,4 @@ float32 throttle_sp # Current throttle setpoint [-] float32 pitch_sp_rad # Current pitch setpoint [rad] float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions -# TECS mode -uint8 mode -uint8 TECS_MODE_NORMAL = 0 -uint8 TECS_MODE_UNDERSPEED = 1 +bool underspeed_mode_enabled # System has detected a low airspeed and takes measures to avoid stalling the plane diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index bbf377db41..71bdbbe3ce 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -649,7 +649,6 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo _control.initialize(control_setpoint, control_input, _control_param, _control_flag); - _debug_status.tecs_mode = _tecs_mode; _debug_status.control = _control.getDebugOutput(); _debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed; _debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate; @@ -721,17 +720,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set // Update time stamps _update_timestamp = now; - - // Set TECS mode for next frame - if (_control.getRatioUndersped() > FLT_EPSILON) { - _tecs_mode = ECL_TECS_MODE_UNDERSPEED; - - } else { - // This is the default operation mode - _tecs_mode = ECL_TECS_MODE_NORMAL; - } - - _debug_status.tecs_mode = _tecs_mode; _debug_status.control = _control.getDebugOutput(); _debug_status.true_airspeed_filtered = eas_to_tas * eas.speed; _debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index ded1511a5e..43b1cc3889 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -539,11 +539,6 @@ private: class TECS { public: - enum ECL_TECS_MODE { - ECL_TECS_MODE_NORMAL = 0, - ECL_TECS_MODE_UNDERSPEED - }; - struct DebugOutput { TECSControl::DebugOutput control; float true_airspeed_filtered; @@ -551,7 +546,6 @@ public: float altitude_reference; float height_rate_reference; float height_rate_direct; - enum ECL_TECS_MODE tecs_mode; }; public: TECS() = default; @@ -652,15 +646,13 @@ public: float get_throttle_setpoint() {return _control.getThrottleSetpoint();} uint64_t timestamp() { return _update_timestamp; } - ECL_TECS_MODE tecs_mode() { return _tecs_mode; } + bool underspeed_detected() { return _control.getRatioUndersped() > 0.f; } private: TECSControl _control; ///< Control submodule. TECSAirspeedFilter _airspeed_filter; ///< Airspeed filter submodule. TECSAltitudeReferenceModel _altitude_reference_model; ///< Setpoint reference model submodule. - enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode. - 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) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 2eb9907513..93d7e9c912 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -485,16 +485,6 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air const TECS::DebugOutput &debug_output{_tecs.getStatus()}; - switch (_tecs.tecs_mode()) { - case TECS::ECL_TECS_MODE_NORMAL: - tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL; - break; - - case TECS::ECL_TECS_MODE_UNDERSPEED: - tecs_status.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - } - tecs_status.altitude_sp = alt_sp; tecs_status.altitude_reference = debug_output.altitude_reference; tecs_status.height_rate_reference = debug_output.height_rate_reference; @@ -516,6 +506,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air tecs_status.throttle_sp = _tecs.get_throttle_setpoint(); tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint(); tecs_status.throttle_trim = throttle_trim; + tecs_status.underspeed_mode_enabled = _tecs.underspeed_detected(); tecs_status.timestamp = hrt_absolute_time();