From a6fcf7b48cf37a2bef8d6e2e1a50ac1c311b1c11 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 12 Jul 2023 20:46:22 +0200 Subject: [PATCH] TECS: remove airspeed adaption in case of underspeed Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 26 +------------------ src/lib/tecs/TECS.hpp | 7 ----- .../FixedwingPositionControl.cpp | 3 +-- 3 files changed, 2 insertions(+), 34 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 38ed565a05..bbf377db41 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -627,26 +627,6 @@ void TECSControl::resetIntegrals() _throttle_integ_state = 0.0f; } -float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas) -{ - float new_setpoint{tas_setpoint}; - const float percent_undersped = _control.getRatioUndersped(); - - // 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). - // TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to - // avoid the fear of side effects in simple operations like this. - new_setpoint = tas_min * percent_undersped + (1.0f - percent_undersped) * tas_setpoint; - } - - new_setpoint = constrain(new_setpoint, tas_min, tas_max); - - return new_setpoint; -} - void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed, const float eas_to_tas) { @@ -729,11 +709,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect(); - - // Calculate the demanded true airspeed - // TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved. - control_setpoint.tas_setpoint = _update_speed_setpoint(eas_to_tas * _equivalent_airspeed_min, - eas_to_tas * _equivalent_airspeed_max, EAS_setpoint * eas_to_tas, eas_to_tas * eas.speed); + control_setpoint.tas_setpoint = eas_to_tas * EAS_setpoint; const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 5673342772..ded1511a5e 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -614,7 +614,6 @@ public: void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; }; void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; }; - 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_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; } @@ -665,7 +664,6 @@ private: 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) - float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) 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) @@ -723,10 +721,5 @@ private: .airspeed_enabled = false, .detect_underspeed_enabled = false, }; - - /** - * Update the desired airspeed - */ - float _update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas); }; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index ed233e1e0f..2eb9907513 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -132,7 +132,6 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_param_fw_t_spdweight.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_throttle_damp(_param_fw_t_thr_damp.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); @@ -1210,7 +1209,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - tecs_status_s::TECS_MODE_NORMAL, + false, pos_sp_curr.vz); }