From ba13bb73c3d42441f7555755f98713f9352018dc Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 3 May 2021 17:41:27 +0300 Subject: [PATCH] tecs: propagate altitude setpoint based on target climb/sink rate - avoids tecs always climbing and sinking and max rates and allows to fine tune these rates - avoid numerical calculation of feedforward velocity using derivative, this was prone to jitter in dt Signed-off-by: RomanBapst --- src/lib/tecs/TECS.cpp | 83 +++++++------------ src/lib/tecs/TECS.hpp | 20 ++--- .../FixedwingPositionControl.cpp | 5 +- .../FixedwingPositionControl.hpp | 2 + 4 files changed, 41 insertions(+), 69 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 0361c32fe4..1840d436d1 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -174,51 +174,31 @@ void TECS::_update_speed_setpoint() } -void TECS::_update_height_setpoint(float desired, float state) +void TECS::updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s, + float alt_amsl) { - // Detect first time through and initialize previous value to demand - if (PX4_ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) { - _hgt_setpoint_in_prev = desired; + target_climbrate_m_s = math::min(target_climbrate_m_s, _max_climb_rate); + target_sinkrate_m_s = math::min(target_sinkrate_m_s, _max_sink_rate); + + float feedforward_height_rate = 0.0f; + + if (fabsf(alt_sp_amsl_m - _hgt_setpoint) < math::max(target_sinkrate_m_s, target_climbrate_m_s) * _dt) { + _hgt_setpoint = alt_sp_amsl_m; + + } else if (alt_sp_amsl_m > _hgt_setpoint) { + _hgt_setpoint += target_climbrate_m_s * _dt; + feedforward_height_rate = target_climbrate_m_s; + + } else if (alt_sp_amsl_m < _hgt_setpoint) { + _hgt_setpoint -= target_sinkrate_m_s * _dt; + feedforward_height_rate = -target_sinkrate_m_s; } - // Apply a 2 point moving average to demanded height to reduce - // intersampling noise effects. - if (PX4_ISFINITE(desired)) { - _hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev); - - } else { - _hgt_setpoint = _hgt_setpoint_in_prev; - } - - _hgt_setpoint_in_prev = _hgt_setpoint; - - // Apply a rate limit to respect vehicle performance limitations - if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) { - _hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt; - - } else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) { - _hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt; - } - - _hgt_setpoint_prev = _hgt_setpoint; - - // Apply a first order noise filter - _hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev; - // Use a first order system to calculate a height rate setpoint from the current height error. // Additionally, allow to add feedforward from heigh setpoint change - _hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff * - (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt; - - _hgt_setpoint_adj_prev = _hgt_setpoint_adj; - - // Limit the rate of change of height demand to respect vehicle performance limits - if (_hgt_rate_setpoint > _max_climb_rate) { - _hgt_rate_setpoint = _max_climb_rate; - - } else if (_hgt_rate_setpoint < -_max_sink_rate) { - _hgt_rate_setpoint = -_max_sink_rate; - } + _hgt_rate_setpoint = (_hgt_setpoint - alt_amsl) * _height_error_gain + _height_setpoint_gain_ff * + feedforward_height_rate; + _hgt_rate_setpoint = math::constrain(_hgt_rate_setpoint, -_max_sink_rate, _max_climb_rate); } void TECS::_detect_underspeed() @@ -229,7 +209,7 @@ void TECS::_detect_underspeed() } if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f)) - || ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) { + || ((_vert_pos_state < _hgt_setpoint) && _underspeed_detected)) { _underspeed_detected = true; @@ -241,7 +221,7 @@ void TECS::_detect_underspeed() void TECS::_update_energy_estimates() { // Calculate specific energy demands in units of (m**2/sec**2) - _SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy + _SPE_setpoint = _hgt_setpoint * CONSTANTS_ONE_G; // potential energy _SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy // Calculate total energy error @@ -477,10 +457,7 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt _last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);; _last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max); _pitch_setpoint_unc = _last_pitch_setpoint; - _hgt_setpoint_adj_prev = baro_altitude; - _hgt_setpoint_adj = _hgt_setpoint_adj_prev; - _hgt_setpoint_prev = _hgt_setpoint_adj_prev; - _hgt_setpoint_in_prev = _hgt_setpoint_adj_prev; + _hgt_setpoint = baro_altitude; _TAS_setpoint_last = _EAS * EAS2TAS; _TAS_setpoint_adj = _TAS_setpoint_last; _underspeed_detected = false; @@ -499,15 +476,12 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt // throttle lower limit is set to a value that prevents throttle reduction _throttle_setpoint_min = _throttle_setpoint_max - 0.01f; - // height demand and associated states are set to track the measured height - _hgt_setpoint_adj_prev = baro_altitude; - _hgt_setpoint_adj = _hgt_setpoint_adj_prev; - _hgt_setpoint_prev = _hgt_setpoint_adj_prev; - // airspeed demand states are set to track the measured airspeed _TAS_setpoint_last = _EAS * EAS2TAS; _TAS_setpoint_adj = _EAS * EAS2TAS; + _hgt_setpoint = baro_altitude; + // disable speed and decent error condition checks _underspeed_detected = false; _uncommanded_descent_recovery = false; @@ -535,7 +509,8 @@ void TECS::_update_STE_rate_lim() void TECS::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_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) + float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max, + float target_climbrate, float target_sinkrate) { // Calculate the time since last update (seconds) uint64_t now = hrt_absolute_time(); @@ -573,8 +548,8 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set // Calculate the demanded true airspeed _update_speed_setpoint(); - // Calculate the demanded height - _update_height_setpoint(hgt_setpoint, baro_altitude); + // calculate heigh rate setpoint based on altitude demand + updateHeightRateSetpoint(hgt_setpoint, target_climbrate, target_sinkrate, baro_altitude); // Calculate the specific energy values required by the control loop _update_energy_estimates(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 207991a973..09f59b97d6 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -84,7 +84,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 pitch_limit_min, float pitch_limit_max); + float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate); float get_throttle_setpoint() { return _last_throttle_setpoint; } float get_pitch_setpoint() { return _last_pitch_setpoint; } @@ -138,7 +138,7 @@ public: uint64_t timestamp() { return _pitch_update_timestamp; } ECL_TECS_MODE tecs_mode() { return _tecs_mode; } - float hgt_setpoint_adj() { return _hgt_setpoint_adj; } + float hgt_setpoint() { return _hgt_setpoint; } float vert_pos_state() { return _vert_pos_state; } float TAS_setpoint_adj() { return _TAS_setpoint_adj; } @@ -185,11 +185,7 @@ public: */ void handle_alt_step(float delta_alt, float altitude) { - // add height reset delta to all variables involved - // in filtering the demanded height - _hgt_setpoint_in_prev += delta_alt; - _hgt_setpoint_prev += delta_alt; - _hgt_setpoint_adj_prev += delta_alt; + _hgt_setpoint += delta_alt; // reset height states _vert_pos_state = altitude; @@ -254,10 +250,6 @@ private: // height demand calculations float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m) - float _hgt_setpoint_in_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering (m) - float _hgt_setpoint_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering and rate limiting (m) - float _hgt_setpoint_adj{0.0f}; ///< demanded height used by the control loops after all filtering has been applied (m) - float _hgt_setpoint_adj_prev{0.0f}; ///< value of _hgt_setpoint_adj from previous frame (m) float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm // vehicle physical limits @@ -313,9 +305,11 @@ private: void _update_speed_setpoint(); /** - * Update the desired height + * Calculate desired height rate from altitude demand */ - void _update_height_setpoint(float desired, float state); + void updateHeightRateSetpoint(float alt_sp_amsl_m, float target_climbrate_m_s, float target_sinkrate_m_s, + float alt_amsl); + /** * Detect if the system is not capable of maintaining airspeed diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 015a49b9e3..1ea42d6853 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -422,7 +422,7 @@ FixedwingPositionControl::tecs_status_publish() break; } - t.altitude_sp = _tecs.hgt_setpoint_adj(); + t.altitude_sp = _tecs.hgt_setpoint(); t.altitude_filtered = _tecs.vert_pos_state(); t.true_airspeed_sp = _tecs.TAS_setpoint_adj(); @@ -2174,7 +2174,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo climbout_pitch_min_rad - radians(_param_fw_psp_off.get()), throttle_min, throttle_max, throttle_cruise, pitch_min_rad - radians(_param_fw_psp_off.get()), - pitch_max_rad - radians(_param_fw_psp_off.get())); + pitch_max_rad - radians(_param_fw_psp_off.get()), + _param_climbrate_target.get(), _param_sinkrate_target.get()); tecs_status_publish(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1ccedbb59a..d537698090 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -423,6 +423,8 @@ private: (ParamFloat) _param_ste_rate_time_const, (ParamFloat) _param_tas_rate_time_const, (ParamFloat) _param_seb_rate_ff, + (ParamFloat) _param_climbrate_target, + (ParamFloat) _param_sinkrate_target, (ParamFloat) _param_fw_thr_alt_scl, (ParamFloat) _param_fw_thr_cruise,