AP_Soaring: Update vario tau calculation to use SOAR_THML_BANK.

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));
 }

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);
 };