Line data    Source code

       1             : /* SpeedToFly class by Samuel Tabor, 2021.
       2             : 
       3             : Calculates the optimal speed to fly given drag polar, expected climb rate in next thermal and
       4             : horizontal and vertical air movement between thermals.
       5             : */
       6             : #include "SpeedToFly.h"
       7             : 
       8       34129 : void SpeedToFly::update(float Wx, float Wz, float Wexp, float CLmin, float CLmax)
       9             : {
      10             :     // The solution to the speed-to-fly problem does not have a closed form solution. A Newton
      11             :     // method with some additional checks will converge to an acceptable level within 3-4 iterations.
      12             :     // However, to keep the computation constant per function call we just do a single iteration using
      13             :     // the previous approximation as a starting point.
      14             :     // This gives good accuracy as the inputs don't change rapidly. It would also be possible to store 
      15             :     // the inputs and converge the solution over 3-4 function calls, but this real-time iteration
      16             :     // approach gives better accuracy in tests as well as simpler code.
      17             : 
      18       34129 :     Wz *= -1.0f; // Sink defined positive.
      19             : 
      20       34129 :     float sqrtfk   = sqrtf(_polarParams.K);
      21       34129 :     float minSink = (sqrtfk/sqrtf(CLmax)*(_polarParams.CD0 + _polarParams.B*CLmax*CLmax))/CLmax;
      22             : 
      23       34129 :     if (!is_positive(minSink+Wz+Wexp)) {
      24             :         // Special case. If lift is greater than min sink speed, fly at min sink
      25             :         // speed.
      26        1280 :         _CL_estimate = CLmax;
      27        1280 :         return;
      28             :     }
      29             : 
      30       32849 :     float CD0 = _polarParams.CD0;
      31       32849 :     float B   = _polarParams.B;
      32       32849 :     float Wxp =  Wx/sqrtfk;
      33       32849 :     float WZ  = (Wz + Wexp)/sqrtfk;
      34             : 
      35             :     // Guess starting location.
      36       32849 :     float CL  = _CL_estimate>0 ? _CL_estimate : 0.5f*(CLmax+CLmin);
      37             : 
      38       32849 :     float t0 = powf(CL,1.5f);
      39       32849 :     float t1 = CD0 + B*CL*CL + t0*WZ;
      40       32849 :     float t2 = 1.5f*sqrtf(CL)*WZ + 2.0f*B*CL;
      41             :     
      42       32849 :     float Jd  = (1.5f*sqrtf(CL)*Wxp + 1.0f)/t1 - (t2*(CL + t0*Wxp))/(t1*t1);
      43             :     
      44       32849 :     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);
      45             : 
      46             :     // Check we're heading to a maximum, not a minimum!!
      47       32849 :     if (is_positive(Jdd)) {
      48             :         // Alternate mode, go uphill.
      49          85 :         CL = CL + 0.1 * (Jd>0.0f ? 1.0f : -1.0f);
      50             :     } else {
      51             :         // Newton should work.
      52       32764 :         CL = CL - Jd/Jdd;
      53             :     }
      54             : 
      55       32849 :     _CL_estimate = CL;
      56       32849 :     _CL_estimate = constrain_float(_CL_estimate, CLmin, CLmax);
      57             : }