AP_Soaring: Calculate expected thermalling sink live and avoid divide by zero by limiting airspeed to that corresponding to CL max (generously assumed 1.5 for glider).

libraries/AP_Soaring/AP_Soaring.cpp

@@ -213,12 +213,11 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
             gcs().send_text(MAV_SEVERITY_ALERT, "Reached lower alt = %dm", (int16_t)alt);
         }
     } else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) {
-        const float thermalability = (_ekf.X[0]*expf(-powf(_aparm.loiter_radius / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK;
         const float mcCreadyAlt = McCready(alt);
-        if (thermalability < mcCreadyAlt) {
+        if (_thermalability < mcCreadyAlt) {
             result = THERMAL_WEAK;
             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);
+                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_pos.z) || _vario.smoothed_climb_rate < 0.0) {
             result = ALT_LOST;
@@ -303,8 +302,16 @@ void SoaringController::update_thermalling()
 
     Vector3f wind_drift = _ahrs.wind_estimate()*deltaT;
 
+    // update the filter
+    _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y);
+
+    
+    _thermalability = (_ekf.X[0]*expf(-powf(_aparm.loiter_radius / _ekf.X[1], 2))) - _vario.get_exp_thermalling_sink();
+
+    _prev_update_time = AP_HAL::micros64();
+
     // write log - save the data.
-    AP::logger().Write("SOAR", "TimeUS,nettorate,x0,x1,x2,x3,north,east,alt,dx_w,dy_w", "Qffffffffff",
+    AP::logger().Write("SOAR", "TimeUS,nettorate,x0,x1,x2,x3,north,east,alt,dx_w,dy_w,th", "Qfffffffffff",
                                            AP_HAL::micros64(),
                                            (double)_vario.reading,
                                            (double)_ekf.X[0],
@@ -315,12 +322,8 @@ void SoaringController::update_thermalling()
                                            current_position.y,
                                            (double)_vario.alt,
                                            (double)wind_drift.x,
-                                           (double)wind_drift.y);
+                                           (double)wind_drift.y,
-
+                                           (double)_thermalability);
-    // update the filter
-    _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y);
-
-    _prev_update_time = AP_HAL::micros64();
 }
 
 void SoaringController::update_cruising()

libraries/AP_Soaring/AP_Soaring.h

@@ -16,7 +16,6 @@
 #include "Variometer.h"
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 
-#define EXPECTED_THERMALLING_SINK 0.7
 #define INITIAL_THERMAL_STRENGTH 2.0
 #define INITIAL_THERMAL_RADIUS 30.0 //150.0
 #define INITIAL_STRENGTH_COVARIANCE 0.0049
@@ -50,6 +49,9 @@ class SoaringController {
 
     float McCready(float alt);
 
+    float _thermalability;
+    float _expected_sink;
+
 protected:
     AP_Int8 soar_active;
     AP_Int8 soar_active_ch;

libraries/AP_Soaring/Variometer.cpp

@@ -42,10 +42,15 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
 
     float roll = _ahrs.roll;
 
-    // Compute still-air sinkrate
+    // Constrained airspeed.
-    float sinkrate = correct_netto_rate(0.0f,  roll, _aspd_filt, polar_K, polar_Cd0, polar_B);
+    const float minV = sqrt(polar_K/1.5);
+    float aspd_filt_constrained = _aspd_filt>minV ? _aspd_filt : minV;
 
-    reading = raw_climb_rate + dsp*_aspd_filt/GRAVITY_MSS + sinkrate;
+
+    // Compute still-air sinkrate
+    float sinkrate = correct_netto_rate(0.0f,  roll, aspd_filt_constrained, polar_K, polar_Cd0, polar_B);
+
+    reading = raw_climb_rate + dsp*aspd_filt_constrained/GRAVITY_MSS + sinkrate;
     
 
     filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading;                       // Apply low pass timeconst filter for noise
@@ -56,13 +61,16 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
     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)aspd_filt_constrained,
                        (double)alt,
                        (double)roll,
                        (double)reading,
                        (double)filtered_reading,
                        (double)raw_climb_rate,
                        (double)smoothed_climb_rate);
+
+    float expected_roll = asinf(constrain_float(powf(aspd_filt_constrained,2)/(GRAVITY_MSS*_aparm.loiter_radius),-1.0, 1.0));
+    _expected_thermalling_sink = correct_netto_rate(0.0, expected_roll, aspd_filt_constrained, polar_K, polar_Cd0, polar_B);
 }
 
 
@@ -80,6 +88,7 @@ float Variometer::correct_netto_rate(float climb_rate,
     float netto_rate;
     float cosphi;
     CL0 = polar_K / (aspd * aspd);
+
     C1 = polar_CD0 / CL0;  // constant describing expected angle to overcome zero-lift drag
     C2 = polar_B * CL0;    // constant describing expected angle to overcome lift induced drag at zero bank
 

libraries/AP_Soaring/Variometer.h

@@ -28,6 +28,7 @@ class Variometer {
     float _last_aspd;
     float _last_roll;
     float _last_total_E;
+    float _expected_thermalling_sink;
 
     // declares a 5point average filter using floats
     AverageFilterFloat_Size5 _vdot_filter;
@@ -50,5 +51,9 @@ public:
     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);}
+
+    float get_airspeed(void) {return _aspd_filt;};
+
+    float get_exp_thermalling_sink(void) {return _expected_thermalling_sink;};
 };