From 5e32e9be5a81bdd6be017c80633673e283f744ab Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 19 Jan 2021 08:57:15 +0100 Subject: [PATCH] TECS: rename variables and methods to make clear which are EAS and which TAS Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 35 +++++++++---------- src/lib/tecs/TECS.hpp | 19 +++++----- .../FixedwingPositionControl.cpp | 4 +-- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 8f79b874d7..d470e8d675 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -55,7 +55,8 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil * inertial nav data is not available. It also calculates a true airspeed derivative * which is used by the airspeed complimentary filter. */ -void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock, +void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, + bool altitude_lock, bool in_air, float altitude, float vz) { @@ -79,7 +80,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri } _state_update_timestamp = now; - _EAS = airspeed; + _EAS = equivalent_airspeed; _in_air = in_air; @@ -88,7 +89,7 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri _vert_pos_state = altitude; // Update and average speed rate of change if airspeed is being measured - if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) { + if (PX4_ISFINITE(equivalent_airspeed) && airspeed_sensor_enabled()) { // Apply some noise filtering _TAS_rate_filter.update(speed_deriv_forward); _speed_derivative = _TAS_rate_filter.getState(); @@ -103,25 +104,24 @@ void TECS::update_vehicle_state_estimates(float airspeed, const float speed_deri } -void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS) +void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS) { // Calculate the time in seconds since the last update and use the default time step value if out of bounds uint64_t now = hrt_absolute_time(); const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX); // Convert equivalent airspeed quantities to true airspeed - _EAS_setpoint = airspeed_setpoint; + _EAS_setpoint = equivalent_airspeed_setpoint; _TAS_setpoint = _EAS_setpoint * EAS2TAS; - _TAS_max = _indicated_airspeed_max * EAS2TAS; - _TAS_min = _indicated_airspeed_min * EAS2TAS; + _TAS_max = _equivalent_airspeed_max * EAS2TAS; + _TAS_min = _equivalent_airspeed_min * EAS2TAS; - // If airspeed measurements are not being used, fix the airspeed estimate to halfway between - // min and max limits - if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { - _EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max); + // If airspeed measurements are not being used, fix the EAS estimate to halfway between min and max limits + if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) { + _EAS = 0.5f * (_equivalent_airspeed_min + _equivalent_airspeed_max); } else { - _EAS = indicated_airspeed; + _EAS = equivalent_airspeed; } // If first time through or not flying, reset airspeed states @@ -130,7 +130,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee _tas_state = (_EAS * EAS2TAS); } - // Obtain a smoothed airspeed estimate using a second order complementary filter + // Obtain a smoothed TAS estimate using a second order complementary filter // Update TAS rate state float tas_error = (_EAS * EAS2TAS) - _tas_state; @@ -139,7 +139,6 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee // limit integrator input to prevent windup if (_tas_state < 3.1f) { tas_rate_state_input = max(tas_rate_state_input, 0.0f); - } // Update TAS state @@ -147,7 +146,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f; _tas_state = _tas_state + tas_state_input * dt; - // Limit the airspeed state to a minimum of 3 m/s + // Limit the TAS state to a minimum of 3 m/s _tas_state = max(_tas_state, 3.0f); _speed_update_timestamp = now; @@ -155,7 +154,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee void TECS::_update_speed_setpoint() { - // Set the airspeed demand to the minimum value if an underspeed or + // 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) || (_underspeed_detected)) { _TAS_setpoint = _TAS_min; @@ -535,7 +534,7 @@ void TECS::_update_STE_rate_lim() } void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, - float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, + float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { // Calculate the time since last update (seconds) @@ -558,7 +557,7 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float } // Update the true airspeed state estimate - _update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas); + _update_speed_states(EAS_setpoint, equivalent_airspeed, eas_to_tas); // Calculate rate limits for specific total energy _update_STE_rate_lim(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 64de3b4f16..619a83cffa 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -74,14 +74,15 @@ public: * Must be called prior to udating tecs control loops * Must be called at 50Hz or greater */ - void update_vehicle_state_estimates(float airspeed, const float speed_deriv_forward, bool altitude_lock, bool in_air, + void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock, + bool in_air, float altitude, float vz); /** * Update the control loop calculations */ void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, - float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, + 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 pitch_limit_min, float pitch_limit_max); @@ -111,8 +112,8 @@ public: void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; } void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); } - void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; } - void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; } + void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; } + void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; } void set_pitch_damping(float damping) { _pitch_damping_gain = damping; } void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } @@ -214,12 +215,12 @@ private: 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] float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate - float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s] - float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) - float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) + 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 _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) + float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) // complimentary filter states float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) @@ -297,7 +298,7 @@ private: /** * Update the airspeed internal state using a second order complementary filter */ - void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas); + void _update_speed_states(float airspeed_setpoint, float equivalent_airspeed, float cas_to_tas); /** * Update the desired airspeed diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index bddbaa776b..84c1a10725 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -96,8 +96,8 @@ 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_indicated_airspeed_min(_param_fw_airspd_min.get()); - _tecs.set_indicated_airspeed_max(_param_fw_airspd_max.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()); _tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());