Exemple #1
0
 public Position(Position position)
 {
     this.Latitude = position.Latitude;
     this.Longitude = position.Longitude;
     this.Altitude = position.Altitude;
     this.Press_Altitude = position.Press_Altitude;
 }
Exemple #2
0
        public void Init(Telemetry tel)
        {
            // Do not move N computation below, N is used by LLAtoECEF method!
            gpsLLA = tel.uavPosition;
            gpsLLA.Press_Altitude = gpsLLA.Altitude - localGeoidHeight;

            N = a / Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(gpsLLA.Latitude), 2));
            cosLat = Math.Cos(gpsLLA.Latitude);
            sinLat = Math.Sin(gpsLLA.Latitude);
            cosLon = Math.Cos(gpsLLA.Longitude);
            sinLon = Math.Sin(gpsLLA.Longitude);

            gpsECEF = LLAtoECEF(gpsLLA);

            uavAngles = tel.uavAngles;
            cosYaw = Math.Cos(uavAngles.Yaw);
            sinYaw = Math.Sin(uavAngles.Yaw);
            if (zeroUavPitchRoll)
            {
                cosPitch = cosRoll = 1;
                sinPitch = sinRoll = 0;
            }
            else
            {
                cosPitch = Math.Cos(uavAngles.Pitch);
                sinPitch = Math.Sin(uavAngles.Pitch);
                cosRoll = Math.Cos(uavAngles.Roll);
                sinRoll = Math.Sin(uavAngles.Roll);
            }

            R_ECEF_LNED = null;
            R_LNED_ECEF = null;
            R_LNED_BODY = null;
            R_BODY_LNED = null;
        }
Exemple #3
0
 public Telemetry()
 {
     uavAngles = new Angles();
     uavPosition = new Position();
     uavAcceleration = new Acceleration();
     uavSpeed = new Speed();
 }
Exemple #4
0
        public Position RoundEarthNE_2_LatLon(double posNorth_ft, double posEast_ft, double OrLat_rad, double OrLon_rad)
        {
            //     Formula:  lat = posNorth_ft / EarthRadius_ft + lat_origin
            //     Formula:  lon = (posEast_ft / (EarthRadius_ft * cos ( lat ))) + lon_origin

            Position ECEFWp = new Position();
            ECEFWp.Latitude = posNorth_ft / EARTH_RADIUS_FT + OrLat_rad;
            ECEFWp.Longitude = (posEast_ft / (EARTH_RADIUS_FT * Math.Cos(ECEFWp.Latitude)));

            return ECEFWp;
        }
        //public ServiceDescription GetDescription()
        //{
        //    ServiceDescription s = new ServiceDescription();
        //    s.Name = "DEM Manager Service";
        //    s.Description = "This service provides a set of DEM support functions.";

        //    s.Functions = new FunctionDescription[3];

        //    s.Functions[0] = new FunctionDescription();
        //    s.Functions[0].Name = "GetAltitude";
        //    s.Functions[0].parameters = new ParameterDescription[1];
        //    s.Functions[0].parameters[0] = new ParameterDescription();
        //    s.Functions[0].parameters[0].Name = "point";
        //    s.Functions[0].parameters[0].Type = "USAL.Position";
        //    s.Functions[0].ReturnType = "double";

        //    s.Functions[1] = new FunctionDescription();
        //    s.Functions[1].Name = "SetPrecision";
        //    s.Functions[1].parameters = new ParameterDescription[1];
        //    s.Functions[1].parameters[0] = new ParameterDescription();
        //    s.Functions[1].parameters[0].Name = "precision";
        //    s.Functions[1].parameters[0].Type = "Dem.Precison";
        //    s.Functions[1].ReturnType = "void";

        //    s.Functions[2] = new FunctionDescription();
        //    s.Functions[2].Name = "SetWorkingArea";
        //    s.Functions[2].parameters = new ParameterDescription[2];
        //    s.Functions[2].parameters[0] = new ParameterDescription();
        //    s.Functions[2].parameters[0].Name = "bottomLeft";
        //    s.Functions[2].parameters[0].Type = "USAL.Position";
        //    s.Functions[2].parameters = new ParameterDescription[2];
        //    s.Functions[2].parameters[1] = new ParameterDescription();
        //    s.Functions[2].parameters[1].Name = "topRight";
        //    s.Functions[2].parameters[1].Type = "USAL.Position";
        //    s.Functions[2].ReturnType = "void";

        //    return s;
        //}

        private WgsPoint convertUsalPosToWgsPoint(USAL.Position usal_wp)
        {
            return(new WgsPoint(usal_wp.Latitude * 180 / Math.PI, usal_wp.Longitude * 180 / Math.PI, usal_wp.Altitude));
        }
Exemple #6
0
 // LLA to LNED
 public Vector3 LLAtoLNED(Position LLA)
 {
     return ECEFtoLNED(LLAtoECEF(LLA));
 }
Exemple #7
0
        // ECEF to LLA
        public Position ECEFtoLLA(Vector3 ECEF)
        {
            double p = Math.Sqrt(ECEF.x * ECEF.x + ECEF.y * ECEF.y);
            double g = Math.Atan2(ECEF.z * a, p * b);

            Position LLA = new Position();
            LLA.Latitude  = Math.Atan2(ECEF.z + ep2 * b * Math.Pow(Math.Sin(g), 3), p - e2 * a * Math.Pow(Math.Cos(g), 3));
            LLA.Longitude = Math.Atan2(ECEF.y, ECEF.x);
            LLA.Altitude  = Convert.ToSingle(p / Math.Cos(LLA.Latitude) - N);
            LLA.Press_Altitude = LLA.Altitude - localGeoidHeight;
            return (LLA);
        }
Exemple #8
0
 // LLA to ECEF
 public Vector3 LLAtoECEF(Position LLA)
 {
     return new Vector3 ((N + LLA.Altitude) * Math.Cos(LLA.Latitude) * Math.Cos(LLA.Longitude),
                         (N + LLA.Altitude) * Math.Cos(LLA.Latitude) * Math.Sin(LLA.Longitude),
                         (N * (1 - e2) + LLA.Altitude) * Math.Sin(LLA.Latitude));
 }
Exemple #9
0
        // Get LLA coordinates of pixel [m,n] (m:row, n:column)
        public Position GetPixelGeolocation(int m, int n)
        {
            Position targetLLA = new Position(nedFrame.GpsLLA);
            Vector3  targetNED = new Vector3();

            ArrayList historic_dem = new ArrayList();
            int index, i = 0;

            if (RTDPSettings.geolocationSettings.zeroCamDistance)
            {
                Vector3 pixelNED = nedFrame.BODYtoLNED(camFrame.PIXELtoBODY(m, n));

                double aux_h = nedFrame.GpsLLA.Press_Altitude;
                double aux_x = pixelNED.x / pixelNED.z;
                double aux_y = pixelNED.y / pixelNED.z;
                double dem_alt, target_alt= 0;

                do
                {
                    dem_alt = GetDemAltitude(targetLLA);
                    index = historic_dem.IndexOf(dem_alt);
                    if (index == -1)
                    {
                        target_alt = dem_alt;
                        targetNED.z = aux_h - target_alt;
                        targetNED.x = targetNED.z * aux_x;
                        targetNED.y = targetNED.z * aux_y;
                        targetLLA = nedFrame.LNEDtoLLA(targetNED);
                        historic_dem.Add(dem_alt);
                        i++;   
                    }
                    else if (index != historic_dem.Count - 1)
                    {
                        double sum_dem = 0;
                        for (int j = index; j < historic_dem.Count; j++)
                            sum_dem += (double)historic_dem[j];
                        target_alt = sum_dem / (historic_dem.Count - index);
                        targetNED.z = aux_h - target_alt;
                        targetNED.x = targetNED.z * aux_x;
                        targetNED.y = targetNED.z * aux_y;
                        targetLLA = nedFrame.LNEDtoLLA(targetNED);
                        i++;   
                   }
                }
                while ((index == -1) && (i < RTDPSettings.geolocationSettings.maxIterations));
            }
            else
            {
                Vector3 pixelNED = nedFrame.BODYtoLNED(camFrame.PIXELtoBODY(m, n));

                double aux_h = nedFrame.GpsLLA.Press_Altitude;
                double aux_x = (pixelNED.x - cameraNED.x) / (pixelNED.z - cameraNED.z);
                double aux_y = (pixelNED.y - cameraNED.y) / (pixelNED.z - cameraNED.z);
                double dem_alt, target_alt= 0;

                do
                {
                    dem_alt = GetDemAltitude(targetLLA);
                    index = historic_dem.IndexOf(dem_alt);
                    if (index == -1)
                    {
                        target_alt = dem_alt;
                        targetNED.z = aux_h - target_alt;
                        targetNED.x = cameraNED.x + (targetNED.z - cameraNED.z) * aux_x;
                        targetNED.y = cameraNED.y + (targetNED.z - cameraNED.z) * aux_y;
                        targetLLA = nedFrame.LNEDtoLLA(targetNED);
                        historic_dem.Add(dem_alt);
                        i++;   
                    }
                    else if (index != historic_dem.Count - 1)
                    {
                        double sum_dem = 0;
                        for (int j = index; j < historic_dem.Count; j++)
                            sum_dem += (double)historic_dem[j];
                        target_alt = sum_dem / (historic_dem.Count - index);
                        targetNED.z = aux_h - target_alt;
                        targetNED.x = cameraNED.x + (targetNED.z - cameraNED.z) * aux_x;
                        targetNED.y = cameraNED.y + (targetNED.z - cameraNED.z) * aux_y;
                        targetLLA = nedFrame.LNEDtoLLA(targetNED);
                        i++;   
                    }
                }
                while ((index == -1) && (i < RTDPSettings.geolocationSettings.maxIterations));
            }
            return targetLLA;
        }
Exemple #10
0
 public double GetDemAltitude(Position lla)
 {
     return (double)DemManager.DemManagerService.GetInstance().FunctionCall("GetAltitude", new object[] { lla });
 }