/// <summary>
/// Finds a point on the glide slope intercept where the angle between
/// vessel and the point is lateralInterceptAngle degrees.
/// </summary>
/// <param name="finalApproachVector"></param>
/// <param name="lateralAngleOfFinalApproachVector"></param>
/// <returns></returns>
private Vector3d FindVectorToGlideslopeIntercept(Vector3d finalApproachVector, double lateralAngleOfFinalApproachVector)
{
// Determine the three angles of the triangle, one of which is
// the lateral angle of final approach vector, and the other is
// 180 - lateral intercept angle.
double theta = 180 - lateralInterceptAngle;
double omega = 180 - lateralAngleOfFinalApproachVector - theta;
// We know the lateral distance to the final approach point, we
// want to find a point on the glide slope which we can
// intercept at a given angle.
double dist = (LateralDistance(vesselState.CoM, finalApproachVector) * Math.Sin(UtilMath.Deg2Rad * omega)) / Math.Sin(UtilMath.Deg2Rad * theta);
// If this is a bad intercept, proceed to IAP.
if (dist < lateralDistanceFromTouchdownToFinalApproach)
{
approachState = AutolandApproachState.IAP;
return(runway.GetPointOnGlideslope(glideslope, GetAutolandLateralDistanceFromTouchdownToFinalApproach() - runway.touchdownPoint));
}
return(runway.GetPointOnGlideslope(glideslope, dist + lateralDistanceFromTouchdownToFinalApproach));
}
/// <summary>
/// Computes and returns the target vector for approach and autoland.
/// </summary>
/// <returns></returns>
public Vector3d GetAutolandTargetVector()
{
// positions of the start and end of the runway
Vector3d runwayStart = runway.GetVectorToTouchdown();
Vector3d runwayEnd = runway.End();
// get the initial and final approach vectors
Vector3d initialApproachVector = runway.GetPointOnGlideslope(glideslope, GetAutolandLateralDistanceFromTouchdownToFinalApproach() - runway.touchdownPoint);
Vector3d finalApproachVector = runway.GetPointOnGlideslope(glideslope, lateralDistanceFromTouchdownToFinalApproach - runway.touchdownPoint);
// determine whether the vessel is within the approach cone or not
Vector3d finalApproachVectorProjectedOnGroundPlane = finalApproachVector.ProjectOnPlane(runway.Up());
Vector3d initialApproachVectorProjectedOnGroundPlane = initialApproachVector.ProjectOnPlane(runway.Up());
Vector3d runwayDirectionVectorProjectedOnGroundPlane = (runwayEnd - runwayStart).ProjectOnPlane(runway.Up());
double lateralAngleOfFinalApproachVector = Vector3d.Angle(finalApproachVectorProjectedOnGroundPlane, runwayDirectionVectorProjectedOnGroundPlane);
double lateralAngleOfInitialApproachVector = Vector3d.Angle(initialApproachVectorProjectedOnGroundPlane, runwayDirectionVectorProjectedOnGroundPlane);
if (approachState == AutolandApproachState.START)
{
if (lateralAngleOfFinalApproachVector < lateralApproachConeAngle)
{
// We are within the approach cone, we can skip IAP and
// instead start intercepting the glideslope.
approachState = AutolandApproachState.GLIDESLOPEINTERCEPT;
return(FindVectorToGlideslopeIntercept(finalApproachVector, lateralAngleOfFinalApproachVector));
}
approachState = AutolandApproachState.IAP;
return(initialApproachVector);
}
else if (approachState == AutolandApproachState.IAP)
{
if (lateralAngleOfInitialApproachVector > 180 - lateralApproachConeAngle)
{
// We are within the "bad" cone. We have to go all the way
// to IAP without cutting corners.
return(initialApproachVector);
}
if (lateralAngleOfFinalApproachVector < lateralApproachConeAngle)
{
// We are in the approach cone, start glideslope intercept.
approachState = AutolandApproachState.GLIDESLOPEINTERCEPT;
return(FindVectorToGlideslopeIntercept(finalApproachVector, lateralAngleOfFinalApproachVector));
}
return(initialApproachVector);
}
else if (approachState == AutolandApproachState.GLIDESLOPEINTERCEPT)
{
Vector3d vectorToGlideslopeIntercept = FindVectorToGlideslopeIntercept(finalApproachVector, lateralAngleOfFinalApproachVector);
// Determine whether we should start turning towards FAP.
double estimatedTimeToTurn = lateralInterceptAngle / GetAutolandMaxRateOfTurn();
double timeToGlideslopeIntercept = LateralDistance(vesselState.CoM, vectorToGlideslopeIntercept) / vesselState.speedSurfaceHorizontal;
if (estimatedTimeToTurn >= timeToGlideslopeIntercept)
{
approachState = AutolandApproachState.FAP;
return(finalApproachVector);
}
// Otherwise, continue flying towards the glideslope intercept.
return(vectorToGlideslopeIntercept);
}
else if (approachState == AutolandApproachState.FAP)
{
if (lateralAngleOfFinalApproachVector > lateralInterceptAngle)
{
// Cancel final approach, go back to initial approach.
approachState = AutolandApproachState.IAP;
return(initialApproachVector);
}
double timeToFAP = LateralDistance(vesselState.CoM, finalApproachVector) / vesselState.speedSurfaceHorizontal;
if (GetAutolandAlignmentError(finalApproachVector) < 3.0 && timeToFAP < secondsThresholdToNextWaypoint)
{
approachState = AutolandApproachState.TOUCHDOWN;
return(runway.GetVectorToTouchdown());
}
return(finalApproachVector);
}
else if (approachState == AutolandApproachState.TOUCHDOWN)
{
double timeToTouchdown = LateralDistance(vesselState.CoM, runway.GetVectorToTouchdown()) / vesselState.speedSurfaceHorizontal;
if (vesselState.altitudeTrue < startFlareAtAltitude + 10)
{
approachState = AutolandApproachState.WAITINGFORFLARE;
return(runway.End());
}
return(runway.GetVectorToTouchdown());
}
else if (approachState == AutolandApproachState.WAITINGFORFLARE)
{
if (vesselState.altitudeTrue < startFlareAtAltitude)
{
approachState = AutolandApproachState.FLARE;
flareStartAoA = vesselState.AoA;
}
return(runway.End());
}
else if (approachState == AutolandApproachState.FLARE)
{
if (vessel.Landed)
{
touchdownMomentAoA = vesselState.AoA;
touchdownMomentSpeed = vesselState.speedSurfaceHorizontal;
approachState = AutolandApproachState.ROLLOUT;
}
return(runway.End());
}
else if (approachState == AutolandApproachState.ROLLOUT)
{
if (vesselState.speedSurface < 1.0)
{
AutopilotOff();
}
return(runway.End());
}
Debug.Assert(false);
return(runway.Start());
}
