From 9a7f80789fc5703b57f9dc685ce60082dc554d2f Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sun, 4 Apr 2021 23:49:13 +0100 Subject: [PATCH] AP_Soaring: Update vario tau calculation to use SOAR_THML_BANK. --- libraries/AP_Soaring/Variometer.cpp | 6 +++--- libraries/AP_Soaring/Variometer.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index fc5bd7d8bc..87ca9e23a3 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -32,7 +32,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo _aspd_filt_constrained = _aspd_filt>minV ? _aspd_filt : minV; - tau = calculate_circling_time_constant(); + tau = calculate_circling_time_constant(radians(thermal_bank)); float dt = (float)(AP_HAL::micros64() - _prev_update_time)/1e6; @@ -125,12 +125,12 @@ float Variometer::calculate_aircraft_sinkrate(float phi, return _aspd_filt_constrained * (C1 + C2 / (cosphi * cosphi)); } -float Variometer::calculate_circling_time_constant() +float Variometer::calculate_circling_time_constant(float thermal_bank) { // Calculate a time constant to use to filter quantities over a full thermal orbit. // This is used for rejecting variation in e.g. climb rate, or estimated climb rate // potential, as the aircraft orbits the thermal. // Use the time to circle - variations at the circling frequency then have a gain of 25% // and the response to a step input will reach 64% of final value in three orbits. - return _aparm.loiter_radius*2*M_PI/_aspd_filt_constrained; + return 2*M_PI*_aspd_filt_constrained/(GRAVITY_MSS*tanf(thermal_bank)); } diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h index 8c8dd80d27..96a2820c54 100644 --- a/libraries/AP_Soaring/Variometer.h +++ b/libraries/AP_Soaring/Variometer.h @@ -55,6 +55,6 @@ public: float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;}; - float calculate_circling_time_constant(); + float calculate_circling_time_constant(const float thermal_bank); };