From 250d5b8acc4de26269f9805794bb36e6d5b6bee9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Nov 2019 10:19:48 -0500 Subject: [PATCH] TECS: remove height rate complementary filter --- tecs/tecs.cpp | 50 ++++---------------------------------------------- tecs/tecs.h | 6 +----- 2 files changed, 5 insertions(+), 51 deletions(-) diff --git a/tecs/tecs.cpp b/tecs/tecs.cpp index 690dcade4d..9255ce0cd2 100644 --- a/tecs/tecs.cpp +++ b/tecs/tecs.cpp @@ -56,7 +56,7 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil */ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, - float altitude, bool vz_valid, float vz, float az) + float altitude, float vz) { // calculate the time lapsed since the last update uint64_t now = ecl_absolute_time(); @@ -74,16 +74,6 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro } if (reset_altitude) { - _vert_pos_state = altitude; - - if (vz_valid) { - _vert_vel_state = -vz; - - } else { - _vert_vel_state = 0.0f; - } - - _vert_accel_state = 0.0f; _states_initalized = false; } @@ -92,40 +82,9 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro _in_air = in_air; - // Generate the height and climb rate state estimates - if (vz_valid) { - // Set the velocity and position state to the the INS data - _vert_vel_state = -vz; - _vert_pos_state = altitude; - - } else { - // Get height acceleration - float hgt_ddot_mea = -az; - - // If we have no vertical INS data, estimate the vertical velocity using a complementary filter - // Perform filter calculation using backwards Euler integration - // Coefficients selected to place all three filter poles at omega - // Reference Paper: Optimising the Gains of the Baro-Inertial Vertical Channel - // Widnall W.S, Sinha P.K, AIAA Journal of Guidance and Control, 78-1307R - float omega2 = _hgt_estimate_freq * _hgt_estimate_freq; - float hgt_err = altitude - _vert_pos_state; - float vert_accel_input = hgt_err * omega2 * _hgt_estimate_freq; - _vert_accel_state = _vert_accel_state + vert_accel_input * dt; - float vert_vel_input = _vert_accel_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f; - _vert_vel_state = _vert_vel_state + vert_vel_input * dt; - float vert_pos_input = _vert_vel_state + hgt_err * _hgt_estimate_freq * 3.0f; - - // If more than 1 second has elapsed since last update then reset the position state - // to the measured height - if (reset_altitude) { - _vert_pos_state = altitude; - - } else { - _vert_pos_state = _vert_pos_state + vert_pos_input * dt; - - } - - } + // Set the velocity and position state to the the INS data + _vert_vel_state = -vz; + _vert_pos_state = altitude; // Update and average speed rate of change if airspeed is being measured if (ISFINITE(airspeed) && airspeed_sensor_enabled()) { @@ -521,7 +480,6 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initalized) { // On first time through or when not using TECS of if there has been a large time slip, // states must be reset to allow filters to a clean start - _vert_accel_state = 0.0f; _vert_vel_state = 0.0f; _vert_pos_state = baro_altitude; _tas_rate_state = 0.0f; diff --git a/tecs/tecs.h b/tecs/tecs.h index 31cc8b23e4..7466f46496 100644 --- a/tecs/tecs.h +++ b/tecs/tecs.h @@ -75,7 +75,7 @@ public: */ void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, - float altitude, bool vz_valid, float vz, float az); + float altitude, float vz); /** * Update the control loop calculations @@ -108,7 +108,6 @@ public: void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; } void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; } - void set_height_comp_filter_omega(float omega) { _hgt_estimate_freq = omega; } void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; } void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; } @@ -169,7 +168,6 @@ public: // reset height states _vert_pos_state = altitude; - _vert_accel_state = 0.0f; _vert_vel_state = 0.0f; } @@ -183,7 +181,6 @@ private: uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call // controller parameters - float _hgt_estimate_freq{0.0f}; ///< cross-over frequency of the height rate complementary filter (rad/sec) float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec) float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec) float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec) @@ -208,7 +205,6 @@ private: float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians) // complimentary filter states - float _vert_accel_state{0.0f}; ///< complimentary filter state - height second derivative (m/sec**2) float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m) float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2)