From 936d4232c9505aec89de10e118a44fed62bd25ee Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Sat, 8 Jun 2019 13:51:00 +0100
Subject: [PATCH] AP_Soaring: Add a 60s first order filter on climb rate. If
 this becomes negative exit thermalling.

---
 libraries/AP_Soaring/AP_Soaring.cpp |  6 ++++--
 libraries/AP_Soaring/Variometer.cpp | 13 ++++++++-----
 libraries/AP_Soaring/Variometer.h   |  6 ++++++
 3 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index effac7f984..8a4555520e 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -199,10 +199,10 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
             if (result != _cruise_criteria_msg_last) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: W %f.3 R %f.3 th %f.1 alt %dm Mc %dm", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (int32_t)alt, (int32_t)mcCreadyAlt);
             }
-        } else if (alt < _thermal_start_alt) {
+        } else if (alt < _thermal_start_alt || _vario.smoothed_climb_rate < 0.0) {
             result = ALT_LOST;
             if (result != _cruise_criteria_msg_last) {
-                gcs().send_text(MAV_SEVERITY_INFO, "Altitude lost from entry = %dm", (int32_t)_thermal_start_alt);
+                gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
             }
         }
     }
@@ -252,6 +252,8 @@ void SoaringController::init_thermalling()
     _prev_update_time = AP_HAL::micros64();
     _thermal_start_time_us = AP_HAL::micros64();
     _thermal_start_alt = _vario.alt;
+
+    _vario.reset_filter(0.0);
 }
 
 void SoaringController::init_cruising()
diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp
index 6a6825c0b7..fa49069a1e 100644
--- a/libraries/AP_Soaring/Variometer.cpp
+++ b/libraries/AP_Soaring/Variometer.cpp
@@ -10,6 +10,7 @@ Variometer::Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
     _ahrs(ahrs),
     _aparm(parms)
 {
+    _climb_filter = LowPassFilter<float>(1.0/60.0);
 }
 
 void Variometer::update(const float polar_K, const float polar_B, const float polar_Cd0)
@@ -26,12 +27,12 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
     // take 5 point moving average
     float dsp = _vdot_filter.apply(temp);
 
-    float dh = 0;
     Vector3f velned;
     if (_ahrs.get_velocity_NED(velned)) {
         // if possible use the EKF vertical velocity
-        dh = -velned.z;
+        raw_climb_rate = -velned.z;
     }
+    smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, (AP_HAL::micros64() - _prev_update_time)/1e6);
 
     float aspd = 0;
     if (!_ahrs.airspeed_estimate(aspd)) {
@@ -44,7 +45,7 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
     // Compute still-air sinkrate
     float sinkrate = correct_netto_rate(0.0f,  roll, _aspd_filt, polar_K, polar_Cd0, polar_B);
 
-    reading = dh + dsp*_aspd_filt/GRAVITY_MSS + sinkrate;
+    reading = raw_climb_rate + dsp*_aspd_filt/GRAVITY_MSS + sinkrate;
     
 
     filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading;                       // Apply low pass timeconst filter for noise
@@ -52,14 +53,16 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
 
     _prev_update_time = AP_HAL::micros64();
 
-    AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff",
+    AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc", "Qffffffff",
                        AP_HAL::micros64(),
                        (double)0.0,
                        (double)_aspd_filt,
                        (double)alt,
                        (double)roll,
                        (double)reading,
-                       (double)filtered_reading);
+                       (double)filtered_reading,
+                       (double)raw_climb_rate,
+                       (double)smoothed_climb_rate);
 }
 
 
diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h
index 4aa3482078..e3a710d123 100644
--- a/libraries/AP_Soaring/Variometer.h
+++ b/libraries/AP_Soaring/Variometer.h
@@ -34,15 +34,21 @@ class Variometer {
 
     AverageFilterFloat_Size5 _sp_filter;
 
+    // low pass filter @ 30s time constant
+    LowPassFilter<float> _climb_filter;
+
 public:
     Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
     float alt;
     float reading;
     float filtered_reading;
     float displayed_reading;
+    float raw_climb_rate;
+    float smoothed_climb_rate;
 
     void update(const float polar_K, const float polar_CD0, const float polar_B);
     float correct_netto_rate(float climb_rate, float phi, float aspd, const float polar_K, const float polar_CD0, const float polar_B);
 
+    void reset_filter(float value) { _climb_filter.reset(value);}
 };