From 6dbea21ef5629c0114a6c507fcb0a72de5c74c61 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 7 Jul 2022 12:30:17 +0300 Subject: [PATCH] TECS: renamed airspeed cruse to airspeed trim & general cleanup Signed-off-by: RomanBapst --- src/lib/tecs/TECS.hpp | 14 ++++++++------ .../fw_pos_control_l1/FixedwingPositionControl.cpp | 8 ++++---- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index f95f483423..ea24dc8923 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -89,7 +89,7 @@ public: */ void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, - float throttle_min, float throttle_setpoint_max, float throttle_cruise, + float throttle_min, float throttle_setpoint_max, float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN); float get_throttle_setpoint() { return _last_throttle_setpoint; } @@ -120,7 +120,7 @@ public: void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; } void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; } - void set_equivalent_airspeed_cruise(float airspeed) { _equivalent_airspeed_cruise = airspeed; } + void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; } void set_pitch_damping(float damping) { _pitch_damping_gain = damping; } void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } @@ -175,7 +175,7 @@ public: float STE_rate() { return _SPE_rate + _SKE_rate; } - float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; } + float STE_rate_setpoint() { return _STE_rate_setpoint; } float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; } @@ -223,7 +223,7 @@ private: float _integrator_gain_throttle{0.0f}; ///< integrator gain used by the throttle demand calculation float _integrator_gain_pitch{0.0f}; ///< integrator gain used by the pitch demand calculation float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2) - float _load_factor{0.0f}; ///< additional normal load factor + float _load_factor{1.0f}; ///< additional normal load factor float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3) float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation float _height_error_gain{0.2f}; ///< height error inverse time constant [1/s] @@ -231,7 +231,7 @@ private: float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s] 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) - float _equivalent_airspeed_cruise{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec) + float _equivalent_airspeed_trim{15.0f}; ///< equivalent cruise airspeed for airspeed less mode (m/sec) float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec) float _STE_rate_time_const{0.1f}; ///< filter time constant for specific total energy rate (damping path) (s) float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) @@ -272,6 +272,7 @@ private: float _STE_rate_min{0.0f}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3) float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit + float _throttle_trim{0.0f}; ///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight float _pitch_setpoint_max{0.5f}; ///< pitch demand upper limit (rad) float _pitch_setpoint_min{-0.5f}; ///< pitch demand lower limit (rad) @@ -280,6 +281,7 @@ private: float _SKE_setpoint{0.0f}; ///< specific kinetic energy demand (m**2/sec**2) float _SPE_rate_setpoint{0.0f}; ///< specific potential energy rate demand (m**2/sec**3) float _SKE_rate_setpoint{0.0f}; ///< specific kinetic energy rate demand (m**2/sec**3) + float _STE_rate_setpoint{0.0f}; ///< specific total energy rate demand (m**s/sec**3) float _SPE_estimate{0.0f}; ///< specific potential energy estimate (m**2/sec**2) float _SKE_estimate{0.0f}; ///< specific kinetic energy estimate (m**2/sec**2) float _SPE_rate{0.0f}; ///< specific potential energy rate estimate (m**2/sec**3) @@ -337,7 +339,7 @@ private: /** * Update throttle setpoint */ - void _update_throttle_setpoint(float throttle_cruise); + void _update_throttle_setpoint(); /** * Detect an uncommanded descent diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index a7fddf10b3..80261537d4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -125,7 +125,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_max_climb_rate(_param_fw_t_clmb_max.get()); _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); _tecs.set_speed_weight(_param_fw_t_spdweight.get()); - _tecs.set_equivalent_airspeed_cruise(_param_fw_airspd_trim.get()); + _tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); _tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get()); _tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); @@ -189,16 +189,16 @@ FixedwingPositionControl::parameters_update() if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() || _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) { - mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds\t"); + mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed trim out of min or max bounds\t"); /* EVENT * @description * - FW_AIRSPD_MAX: {1:.1} * - FW_AIRSPD_MIN: {2:.1} * - FW_AIRSPD_TRIM: {3:.1} */ - events::send(events::ID("fixedwing_position_control_conf_invalid_cruise_bounds"), + events::send(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"), events::Log::Error, - "Invalid configuration: Airspeed cruise out of min or max bounds", + "Invalid configuration: Airspeed trim out of min or max bounds", _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get()); check_ret = PX4_ERROR; }