mirror of https://github.com/ArduPilot/ardupilot
58 lines
2.2 KiB
C++
58 lines
2.2 KiB
C++
/* 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);
|
|
}
|