AP_Soaring: Vario filter cleanup and convert in-line filters to LowPassFilter instances

libraries/AP_Soaring/AP_Soaring.cpp

@@ -172,9 +172,10 @@ bool SoaringController::suppress_throttle()
         _spdHgt.reset_pitch_I();
 
         _cruise_start_time_us = AP_HAL::micros64();
+
         // Reset the filtered vario rate - it is currently elevated due to the climb rate and would otherwise take a while to fall again,
         // leading to false positives.
-        _vario.filtered_reading = 0;
+        _vario.reset_trigger_filter(0.0f);
     }
 
     return _throttle_suppressed;
@@ -186,7 +187,7 @@ bool SoaringController::check_thermal_criteria()
 
     return (status == ActiveStatus::AUTO_MODE_CHANGE
             && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6))
-            && (_vario.filtered_reading - _vario.get_exp_thermalling_sink()) > thermal_vspeed
+            && (_vario.get_trigger_value() - _vario.get_exp_thermalling_sink()) > thermal_vspeed
             && _vario.alt < alt_max
             && _vario.alt > alt_min);
 }
@@ -217,7 +218,7 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2
         const float mcCreadyAlt = McCready(alt);
         if (_thermalability < mcCreadyAlt) {
             result = LoiterStatus::THERMAL_WEAK;
-        } else if (alt < (-_thermal_start_pos.z) || _vario.smoothed_climb_rate < 0.0) {
+        } else if (alt < (-_thermal_start_pos.z) || _vario.get_filtered_climb() < 0.0) {
             result = LoiterStatus::ALT_LOST;
         } else if (check_drift(prev_wp, next_wp)) {
             result = LoiterStatus::DRIFT_EXCEEDED;
@@ -270,7 +271,7 @@ void SoaringController::init_thermalling()
     _thermal_start_time_us = AP_HAL::micros64();
     _thermal_start_pos = position;
 
-    _vario.reset_filter(0.0);
+    _vario.reset_climb_filter(0.0);
 
     _position_x_filter.reset(_ekf.X[2]);
     _position_y_filter.reset(_ekf.X[3]);
@@ -298,7 +299,7 @@ void SoaringController::update_thermalling()
         return;
     }
 
-    Vector3f wind_drift = _ahrs.wind_estimate()*deltaT*_vario.smoothed_climb_rate/_ekf.X[0];
+    Vector3f wind_drift = _ahrs.wind_estimate()*deltaT*_vario.get_filtered_climb()/_ekf.X[0];
 
     // update the filter
     _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y);

libraries/AP_Soaring/AP_Soaring.h

@@ -118,7 +118,7 @@ public:
 
     float get_vario_reading() const
     {
-        return _vario.displayed_reading;
+        return _vario.get_displayed_value();
     }
 
     void update_vario();

libraries/AP_Soaring/Variometer.cpp

@@ -22,12 +22,12 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
     if (!_ahrs.airspeed_estimate(aspd)) {
             aspd = _aparm.airspeed_cruise_cm / 100.0f;
     }
-    _aspd_filt = _sp_filter.apply(aspd);
+
+    float aspd_filt = _sp_filter.apply(aspd);
 
     // Constrained airspeed.
     const float minV = sqrtf(polar_K/1.5);
-    _aspd_filt_constrained = _aspd_filt>minV ? _aspd_filt : minV;
-
+    _aspd_filt_constrained = aspd_filt>minV ? aspd_filt : minV;
 
     tau = calculate_circling_time_constant(radians(thermal_bank));
 
@@ -43,20 +43,22 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
     float dsp = _vdot_filter.apply(temp);
 
     // Now we need to high-pass this signal to remove bias.
-    _vdot_filter2.set_cutoff_frequency(1/(20*tau));
-    float dsp_bias = _vdot_filter2.apply(temp, dt);
+    _vdotbias_filter.set_cutoff_frequency(1/(20*tau));
+    float dsp_bias = _vdotbias_filter.apply(temp, dt);
     
     float dsp_cor = dsp - dsp_bias;
 
 
     Vector3f velned;
+
+    float raw_climb_rate = 0.0f;
     if (_ahrs.get_velocity_NED(velned)) {
         // if possible use the EKF vertical velocity
         raw_climb_rate = -velned.z;
     }
     
     _climb_filter.set_cutoff_frequency(1/(3*tau));
-    smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt);
+    float smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt);
 
     // Compute still-air sinkrate
     float roll = _ahrs.roll;
@@ -65,8 +67,9 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
     reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate;
     
 
-    filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading;                       // Apply low pass timeconst filter for noise
-    displayed_reading = TE_FILT_DISPLAYED * reading + (1 - TE_FILT_DISPLAYED) * displayed_reading;
+    float filtered_reading = _trigger_filter.apply(reading, dt); // Apply low pass timeconst filter for noise
+
+    _audio_filter.apply(reading, dt); // Apply low pass timeconst filter for noise
 
     _prev_update_time = AP_HAL::micros64();
 
@@ -95,7 +98,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
                        (double)roll,
                        (double)reading,
                        (double)filtered_reading,
-                       (double)raw_climb_rate,
+                       (double)_raw_climb_rate,
                        (double)smoothed_climb_rate,
                        (double)_expected_thermalling_sink,
                        (double)dsp,

libraries/AP_Soaring/Variometer.h

@@ -10,10 +10,6 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity.
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 #include <Filter/AverageFilter.h>
 
-#define ASPD_FILT 0.05
-#define TE_FILT 0.03
-#define TE_FILT_DISPLAYED 0.15
-
 class Variometer {
 
     const AP_Vehicle::FixedWing &_aparm;
@@ -21,7 +17,8 @@ class Variometer {
     // store time of last update
     uint64_t _prev_update_time;
 
-    float _aspd_filt;
+    float _raw_climb_rate;
+
     float _aspd_filt_constrained;
 
     float _expected_thermalling_sink;
@@ -31,26 +28,41 @@ class Variometer {
 
     AverageFilterFloat_Size5 _sp_filter;
 
-    // low pass filter @ 30s time constant
+    /*
+     low pass filters for various purposes.
+     */
+    // Climb rate filter for monitoring progress in thermal.
     LowPassFilter<float> _climb_filter{1/60.0};
-    LowPassFilter<float> _vdot_filter2{1/60.0};
+
+    // Fast filter for mavlink/audio vario output.
+    LowPassFilter<float> _audio_filter{1/0.71};
+
+    // Slower filter for deciding to enter THERMAL mode.
+    LowPassFilter<float> _trigger_filter{1/4.06};
+
+    // Longitudinal acceleration bias filter.
+    LowPassFilter<float> _vdotbias_filter{1/60.0};
 
 public:
     Variometer(const AP_Vehicle::FixedWing &parms);
     float alt;
     float reading;
-    float filtered_reading;
-    float displayed_reading;
-    float raw_climb_rate;
-    float smoothed_climb_rate;
     float tau;
 
     void update(const float thermal_bank, const float polar_K, const float polar_CD0, const float polar_B);
     float calculate_aircraft_sinkrate(float phi, const float polar_K, const float polar_CD0, const float polar_B) const;
 
-    void reset_filter(float value) { _climb_filter.reset(value);}
+    void reset_climb_filter(float value) { _climb_filter.reset(value);}
 
-    float get_airspeed(void) const {return _aspd_filt;};
+    void reset_trigger_filter(float value) { _trigger_filter.reset(value);}
+
+    float get_airspeed(void) const {return _aspd_filt_constrained;};
+
+    float get_displayed_value(void) const {return _audio_filter.get();};
+
+    float get_filtered_climb(void) const {return _climb_filter.get();};
+
+    float get_trigger_value(void) const {return _trigger_filter.get();};
 
     float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;};