AP_Soaring: Add speed-to-fly calculation used if SOAR_CRSE_ARSPD<0.

libraries/AP_Soaring/AP_Soaring.cpp

@@ -150,7 +150,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
 
     // @Param: CRSE_ARSPD
     // @DisplayName: Specific setting for airspeed when cruising.
-    // @Description: If non-zero this airspeed will be used when cruising.
+    // @Description: If non-zero this airspeed will be used when cruising. If set to -1, airspeed will be selected based on speed-to-fly theory.
     // @Range: 5 50
     // @User: Advanced
     AP_GROUPINFO("CRSE_ARSPD", 21, SoaringController, soar_cruise_airspeed, 0),
@@ -168,6 +168,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
 SoaringController::SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms) :
     _tecs(tecs),
     _vario(parms,_polarParams),
+    _speedToFly(_polarParams),
     _aparm(parms),
     _throttle_suppressed(true)
 {
@@ -369,8 +370,31 @@ void SoaringController::update_thermalling()
 
 void SoaringController::update_cruising()
 {
-    // Reserved for future tasks that need to run continuously while in FBWB or AUTO mode,
-    // for example, calculation of optimal airspeed and flap angle.
+    // Calculate the optimal airspeed for the current conditions of wind along current direction,
+    // expected lift in next thermal and filtered sink rate.
+
+    Vector3f wind    = AP::ahrs().wind_estimate();
+    Vector3f wind_bf = AP::ahrs().earth_to_body(wind);
+
+    const float wx = wind_bf.x;
+
+    const float wz = _vario.get_stf_value();
+
+    // Constraints on the airspeed calculation.
+    const float CLmin = _polarParams.K/(_aparm.airspeed_max*_aparm.airspeed_max);
+    const float CLmax = _polarParams.K/(_aparm.airspeed_min*_aparm.airspeed_min);
+
+    // Update the calculation.
+    _speedToFly.update(wx, wz, thermal_vspeed, CLmin, CLmax);
+
+    AP::logger().WriteStreaming("SORC", "TimeUS,wx,wz,wexp,CLmin,CLmax,Vopt", "Qffffff",
+                                       AP_HAL::micros64(),
+                                       (double)wx,
+                                       (double)wz,
+                                       (double)thermal_vspeed,
+                                       (double)CLmin,
+                                       (double)CLmax,
+                                       (double)_speedToFly.speed_to_fly());
 }
 
 void SoaringController::update_vario()
@@ -499,6 +523,9 @@ float SoaringController::get_thermalling_target_airspeed()
 
 float SoaringController::get_cruising_target_airspeed()
 {
+    if (soar_cruise_airspeed<0) {
+        return _speedToFly.speed_to_fly();
+    }
     return soar_cruise_airspeed;
 }
 

libraries/AP_Soaring/AP_Soaring.h

@@ -15,6 +15,7 @@
 #include "ExtendedKalmanFilter.h"
 #include "Variometer.h"
 #include <AP_TECS/AP_TECS.h>
+#include "SpeedToFly.h"
 
 #ifndef HAL_SOARING_ENABLED
  #define HAL_SOARING_ENABLED !HAL_MINIMIZE_FEATURES
@@ -32,6 +33,8 @@ class SoaringController {
     ExtendedKalmanFilter _ekf{};
     AP_TECS &_tecs;
     Variometer _vario;
+    SpeedToFly _speedToFly;
+
     const AP_Vehicle::FixedWing &_aparm;
 
     // store aircraft location at last update

libraries/AP_Soaring/SpeedToFly.cpp

@@ -0,0 +1,57 @@
+/* SpeedToFly class by Samuel Tabor, 2021.
+
+Calculates the optimal speed to fly given drag polar, expected climb rate in next thermal and
+horizontal and vertical air movement between thermals.
+*/
+#include "SpeedToFly.h"
+
+void SpeedToFly::update(float Wx, float Wz, float Wexp, float CLmin, float CLmax)
+{
+    // The solution to the speed-to-fly problem does not have a closed form solution. A Newton
+    // method with some additional checks will converge to an acceptable level within 3-4 iterations.
+    // However, to keep the computation constant per function call we just do a single iteration using
+    // the previous approximation as a starting point.
+    // This gives good accuracy as the inputs don't change rapidly. It would also be possible to store 
+    // the inputs and converge the solution over 3-4 function calls, but this real-time iteration
+    // approach gives better accuracy in tests as well as simpler code.
+
+    Wz *= -1.0f; // Sink defined positive.
+
+    float sqrtfk   = sqrtf(_polarParams.K);
+    float minSink = (sqrtfk/sqrtf(CLmax)*(_polarParams.CD0 + _polarParams.B*CLmax*CLmax))/CLmax;
+
+    if (!is_positive(minSink+Wz+Wexp)) {
+        // Special case. If lift is greater than min sink speed, fly at min sink
+        // speed.
+        _CL_estimate = CLmax;
+        return;
+    }
+
+    float CD0 = _polarParams.CD0;
+    float B   = _polarParams.B;
+    float Wxp =  Wx/sqrtfk;
+    float WZ  = (Wz + Wexp)/sqrtfk;
+
+    // Guess starting location.
+    float CL  = _CL_estimate>0 ? _CL_estimate : 0.5f*(CLmax+CLmin);
+
+    float t0 = powf(CL,1.5f);
+    float t1 = CD0 + B*CL*CL + t0*WZ;
+    float t2 = 1.5f*sqrtf(CL)*WZ + 2.0f*B*CL;
+    
+    float Jd  = (1.5f*sqrtf(CL)*Wxp + 1.0f)/t1 - (t2*(CL + t0*Wxp))/(t1*t1);
+    
+    float Jdd = 2.0f*t2*t2*(CL + t0*Wxp)/powf(t1,3) - (2.0f*t2*(1.5f*sqrtf(CL)*Wxp + 1.0f))/(t1*t1) - ((2.0f*B + 0.75f*WZ/sqrtf(CL))*(CL + t0*Wxp))/(t1*t1) + 0.75f*Wxp/(sqrtf(CL)*t1);
+
+    // Check we're heading to a maximum, not a minimum!!
+    if (is_positive(Jdd)) {
+        // Alternate mode, go uphill.
+        CL = CL + 0.1 * (Jd>0.0f ? 1.0f : -1.0f);
+    } else {
+        // Newton should work.
+        CL = CL - Jd/Jdd;
+    }
+
+    _CL_estimate = CL;
+    _CL_estimate = constrain_float(_CL_estimate, CLmin, CLmax);
+}

libraries/AP_Soaring/SpeedToFly.h

@@ -0,0 +1,22 @@
+/* SpeedToFly class by Samuel Tabor, 2021.
+
+Calculates the optimal speed to fly given drag polar, expected climb rate in next thermal and
+horizontal and vertical air movement between thermals.
+*/
+#pragma once
+
+#include "Variometer.h"
+
+class SpeedToFly {
+
+    float _CL_estimate = -1.0f;
+
+    Variometer::PolarParams &_polarParams;
+
+public:
+    SpeedToFly(Variometer::PolarParams &polarParams) :_polarParams(polarParams) {}
+
+    void update(float Wx, float Wz, float Wexp, float CLmin, float CLmax);
+
+    float speed_to_fly(void) {return _CL_estimate>0 ? sqrtf(_polarParams.K/_CL_estimate) : -1.0f;};
+};

libraries/AP_Soaring/Variometer.cpp

@@ -67,10 +67,13 @@ void Variometer::update(const float thermal_bank)
 
     reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate;
     
+    // Update filters.
 
-    float filtered_reading = _trigger_filter.apply(reading, dt); // Apply low pass timeconst filter for noise
+    float filtered_reading = _trigger_filter.apply(reading, dt);
 
-    _audio_filter.apply(reading, dt); // Apply low pass timeconst filter for noise
+    _audio_filter.apply(reading, dt);
+
+    _stf_filter.apply(reading, dt);
 
     _prev_update_time = AP_HAL::micros64();
 

libraries/AP_Soaring/Variometer.h

@@ -43,6 +43,9 @@ class Variometer {
     // Longitudinal acceleration bias filter.
     LowPassFilter<float> _vdotbias_filter{1/60.0};
 
+    // Speed to fly vario filter.
+    LowPassFilter<float> _stf_filter{1/20.0};
+
 public:
     struct PolarParams {
         AP_Float K;
@@ -71,6 +74,8 @@ public:
 
     float get_trigger_value(void) const {return _trigger_filter.get();};
 
+    float get_stf_value(void) const {return _stf_filter.get();};
+
     float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;};
 
     float calculate_circling_time_constant(const float thermal_bank);