Ejemplo n.º 1
0
        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);
        }
Ejemplo n.º 2
0
 public Navigator4D()
 {
     pathGenerator     = new PathGenerator4D();
     pointsOfInterests = new List <PointLatLng>();
 }