public void CreateInitialPath(double InitialYaw, double Lat, double Lon, double Alt, double Velocity)
{
isInitialPath = true;
isTurn180 = false;
pathGenerator.InitialYaw = InitialYaw;
pathGenerator.InitialPosition = new PointLatLngAlt(Lat, Lon, Alt);
pathGenerator.InitialEnuPosition = initialEnuPosition;
pathGenerator.InitialPitch = 0;
pathGenerator.newPath();
pathGenerator.CurrentVelocity = Velocity;
pathGenerator.StraightLine(pathGenerator.TurnRadius * 1.5);
// Generate path to initial point of the rectangle generated path
pathGenerator.GeneratePath(currentPath.PointList[currentObjective], currentPath.Psi_f[currentObjective]);
// Initial path
auxPath = currentPath;
currentPath = pathGenerator.Path;
ResetArc();
}
public void GenerateLandingPath(PointLatLngAlt WP1, PointLatLngAlt WP2, bool NewPath)
{
if (NewPath)
{
pathGenerator.newPath();
}
if (pointListObjectivesLanding == null)
{
pointListObjectivesLanding = new List <PointLatLngAlt>();
double waypointAngle = mapTools.GetSlopeFromNord(WP1, WP2);
pointListObjectivesLanding.Add(mapTools.OffsetInMeters(WP2, Math.Cos(waypointAngle) * 30, Math.Sin(waypointAngle) * 30, 0));
pointListObjectivesLanding.Add(WP2);
}
PathGenerator4D pathGeneratorLanding = new PathGenerator4D();
isLanding = false;
double d1 = mapTools.GetDistance(WP1, pointListObjectivesLanding[0]);
double d2 = mapTools.GetDistance(WP1, pointListObjectivesLanding[1]);
double landSlope = 0;
PointLatLngAlt initialPosition;
if (d1 > d2)
{
landSlope = mapTools.GetSlopeFromNord(pointListObjectivesLanding[0], pointListObjectivesLanding[1]);
initialPosition = pointListObjectivesLanding[0];
}
else
{
landSlope = mapTools.GetSlopeFromNord(pointListObjectivesLanding[1], pointListObjectivesLanding[0]);
initialPosition = pointListObjectivesLanding[1];
}
pathGeneratorLanding.newPath();
pathGeneratorLanding.InitialPosition = initialPosition;
pathGeneratorLanding.InitialYaw = landSlope;
double flareAlt = 2;
double flareDistance = 20;
double descentDistance = 100;
double descentAlt = 30;
double landTrackDistance = mapTools.GetDistanceMeters(pointListObjectivesLanding[0], pointListObjectivesLanding[1]);
pathGeneratorLanding.InitialPitch = 0;
pathGeneratorLanding.StraightLine(landTrackDistance / 3, 0, 0);
pathGeneratorLanding.StraightLine(landTrackDistance / 3, 0, 75);
pathGeneratorLanding.StraightLine(landTrackDistance / 3, 0, 140);
pathGeneratorLanding.InitialPitch = Math.Atan2(flareAlt, flareDistance);
pathGeneratorLanding.StraightLine(flareDistance / 2, 150, 160);
pathGeneratorLanding.StraightLine(flareDistance / 2, 160, 170);
pathGeneratorLanding.InitialPitch = Math.Atan2(descentAlt, descentDistance);
pathGeneratorLanding.StraightLine(descentDistance, 170, 175);
double d3 = mapTools.GetDistanceMeters(pathGeneratorLanding.InitialPosition, pathGenerator.InitialPosition);
double alt1 = pathGenerator.InitialPosition.Alt - pathGeneratorLanding.InitialPosition.Alt;
//pathGeneratorLanding.InitialPitch = Math.Atan2(alt1, d3);
//double initialYaw_1 = pathGenerator.
//pathGeneratorLanding.GeneratePath(pathGenerator.InitialPosition, initialYaw_1);
pathGeneratorLanding.RoutePoints.Reverse();
pathGeneratorLanding.Velocity.Reverse();
pathGeneratorLanding.cCurvature.Reverse();
pathGeneratorLanding.Psi_f.Reverse();
for (int i = 0; i < pathGeneratorLanding.Psi_f.Count; i++)
{
if (pathGeneratorLanding.Psi_f[i] <= 0)
{
pathGeneratorLanding.Psi_f[i] += Math.PI;
}
else
{
pathGeneratorLanding.Psi_f[i] -= Math.PI;
}
}
pathGeneratorLanding.Theta_f.Reverse();
pathGenerator.GeneratePath(pathGeneratorLanding.RoutePoints[0], pathGeneratorLanding.Psi_f[0]);
landingPoint = pathGenerator.RoutePoints.Count - 1;
pathGenerator.AddPath(pathGeneratorLanding.RoutePoints, pathGeneratorLanding.Velocity, pathGeneratorLanding.cCurvature, pathGeneratorLanding.Psi_f, pathGeneratorLanding.Theta_f);
}