You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
57 lines
2.2 KiB
57 lines
2.2 KiB
/* 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); |
|
}
|
|
|