示例#1
0
        public FlightPathLineGeometry(  int _pathNumber,   linearFeatureCoverageSummary _LFSum)
        {
            ////////////////////////////////////////////////////////////////////////////////
            //path: a set of smoothed trajectory points defined from the mission planned
            //compute those variables that are constant across the path
            ////////////////////////////////////////////////////////////////////////////////

            LFSum = _LFSum;
            pathNumber = _pathNumber;

            UTM2Geodetic utm = new UTM2Geodetic();

            //set up the polygon (point list) math procedures
            polyMath = new polygonMath(LFSum.paths[pathNumber].pathUTM);

            //get the semi-infinite line extending beyond the path at the start and end
            int count = LFSum.paths[pathNumber].pathGeoDeg.Count;

            unitAwayFromEndUTM = new PointD();
            unitAwayFromStartUTM = new PointD();
            //semi-infinite line use for drawing a line extending beyond the path end
            unitAwayFromEndUTM   = LFSum.paths[pathNumber].pathUTM[count - 1] - LFSum.paths[pathNumber].pathUTM[count - 2];
            unitAwayFromStartUTM = LFSum.paths[pathNumber].pathUTM[0]         - LFSum.paths[pathNumber].pathUTM[1];
            double delSMag = Math.Sqrt(unitAwayFromStartUTM.X * unitAwayFromStartUTM.X + unitAwayFromStartUTM.Y * unitAwayFromStartUTM.Y);
            unitAwayFromStartUTM = unitAwayFromStartUTM / delSMag;
            double delEMag = Math.Sqrt(unitAwayFromEndUTM.X * unitAwayFromEndUTM.X + unitAwayFromEndUTM.Y * unitAwayFromEndUTM.Y);
            unitAwayFromEndUTM = unitAwayFromEndUTM / delEMag;

            //semi-infinite line ate start and end in UTM coordinates
            PointD semiInfiniteFLstartUTM = LFSum.paths[pathNumber].pathUTM[0] + 10000.0 * unitAwayFromStartUTM;
            PointD semiInfiniteFLendUTM = LFSum.paths[pathNumber].pathUTM[count - 1] + 10000.0 * unitAwayFromEndUTM;

            //convert to geodetic
            semiInfiniteFLstartGeo = new PointD();
            semiInfiniteFLendGeo   = new PointD();
            utm.UTMtoLL(semiInfiniteFLstartUTM, LFSum.UTMZone, ref semiInfiniteFLstartGeo);
            utm.UTMtoLL(semiInfiniteFLendUTM, LFSum.UTMZone, ref semiInfiniteFLendGeo);

            //compute the path length
            pathlengthMeters = 0.0;
            for (int i = 1; i < LFSum.paths[pathNumber].pathUTM.Count; i++)
            {
                double delX = LFSum.paths[pathNumber].pathUTM[i].X - LFSum.paths[pathNumber].pathUTM[i - 1].X;
                double delY = LFSum.paths[pathNumber].pathUTM[i].Y - LFSum.paths[pathNumber].pathUTM[i - 1].Y;
                double magDel = Math.Sqrt(delX * delX + delY * delY);
                pathlengthMeters += magDel;
            }

            //number of expected photocenters
            numPhotoCenters = (int)(pathlengthMeters / LFSum.photocenterSpacing + 1.0);
        }
        public linearFeatureCoverageSummary readLinearFeatureCoverageData(XmlReader tr, String ProjectName)
        {
            ///////////////////////////////////////////////////////////////////////
            //read in the input kml describing the line feature coverage
            ///////////////////////////////////////////////////////////////////////

            //input kml linear feature -- dont need this
            //project map bounds
            //path information (trigger spacing, origin, UTM zone)
            //table of information -- need some of this
            //takeoff airport -- dont need this
            //along-path map bounds -- need this
            //path specific data -- dont need this
            //smoothed trajectory -- need this
            //center projection -- dont need
            //image endpoints  -- dont need

            bool completedreadingProjectMapBounds       = false;
            bool completedReadingPathInformation        = false;
            bool completedReadingSmoothedPathPoints     = false;

            linearFeatureCoverageSummary LFSummary = new linearFeatureCoverageSummary();
            LFSummary.gridOrigin = new PointD();

            //get the project map bounds
            //how do I exit this loop ???
            //TODO:  test to see if the kml file projectName is the same as the kml fileName
            while (tr.Read() && !completedreadingProjectMapBounds)
            {
                if (tr.EOF) break;
                if (tr.IsStartElement() && tr.Name == "GroundOverlay")
                {
                    while (tr.Read() && !completedreadingProjectMapBounds)
                    {
                        if (tr.IsStartElement() && tr.Name == "name")
                        {
                            tr.Read();
                            //we are looking for the name "project Map" where we will get the map bounds
                            if (tr.Value == "Project Map")
                            {
                                int numBounds = 0;
                                while (tr.Read() && !completedreadingProjectMapBounds)
                                {
                                    if (tr.IsStartElement() && tr.Name == "north")
                                    {
                                        tr.Read();
                                        LFSummary.ProjectImage.northDeg = Convert.ToDouble(tr.Value);

                                        numBounds++;
                                    }
                                    if (tr.IsStartElement() && tr.Name == "south")
                                    {
                                        tr.Read();
                                        LFSummary.ProjectImage.southDeg = Convert.ToDouble(tr.Value);
                                        numBounds++;
                                    }
                                    if (tr.IsStartElement() && tr.Name == "east")
                                    {
                                        tr.Read();
                                        LFSummary.ProjectImage.eastDeg = Convert.ToDouble(tr.Value);
                                        numBounds++;
                                    }
                                    if (tr.IsStartElement() && tr.Name == "west")
                                    {
                                        tr.Read();
                                        LFSummary.ProjectImage.westDeg = Convert.ToDouble(tr.Value);
                                        numBounds++;
                                    }
                                    if (numBounds == 4) completedreadingProjectMapBounds = true;
                                }
                            }
                        }
                    }
                }
            }// end of:   get the project map bounds
            if (tr.EOF)
            {
                MessageBox.Show("Bad map bounds format for the input kml file -- exiting");
                Environment.Exit(-1);
            }

            int numPathInfo = 0;
            while (tr.Read() && !completedReadingPathInformation)
            {
                if (tr.EOF) break;
                if (tr.IsStartElement() && tr.Name == "downRangePhotoSpacingMeters")
                {
                    tr.Read();
                    LFSummary.photocenterSpacing = Convert.ToDouble(tr.Value);
                    numPathInfo++;
                }

                if (tr.IsStartElement() && tr.Name == "numberOfPaths")
                {
                    tr.Read();
                    LFSummary.numberParallelPaths = Convert.ToInt32(tr.Value);
                    numPathInfo++;
                }

                if (tr.IsStartElement() && tr.Name == "UTMZone")
                {
                    tr.Read();
                    LFSummary.UTMZone = tr.Value;
                    numPathInfo++;
                }

                if (tr.IsStartElement() && tr.Name == "gridOriginUTMNorthing")
                {
                    tr.Read();
                    LFSummary.gridOrigin.Y = Convert.ToDouble(tr.Value);
                    numPathInfo++;
                }

                if (tr.IsStartElement() && tr.Name == "gridOriginUTMEasting")
                {
                    tr.Read();
                    LFSummary.gridOrigin.X = Convert.ToDouble(tr.Value);
                    numPathInfo++;
                }

                if (tr.IsStartElement() && tr.Name == "proNavGain")
                {
                    tr.Read();
                    LFSummary.proNavGain = Convert.ToDouble(tr.Value);
                    numPathInfo++;
                }
                if (tr.IsStartElement() && tr.Name == "rabbitAheadDistance")
                {
                    tr.Read();
                    LFSummary.plannedRabbitDistanceAhead = Convert.ToDouble(tr.Value);
                    numPathInfo++;
                }
                if (tr.IsStartElement() && tr.Name == "flightAltAGLft")
                {
                    tr.Read();
                    LFSummary.flightAltAGLft = Convert.ToDouble(tr.Value);
                    numPathInfo++;
                }

                if (numPathInfo == 8)completedReadingPathInformation = true;

            }  // end of:   get the path info
            if (tr.EOF)
            {
                MessageBox.Show("Bad path info format for the input kml file -- exiting");
                Environment.Exit(-1);
            }

            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
            //read in the bounds of the mission images ProjectName_Background\background_YY.jpg
            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

            LFSummary.paths = new List<pathDescription>();

            int numPathsProcessedForBounds = 0;
            while (tr.Read() && numPathsProcessedForBounds < LFSummary.numberParallelPaths)
            {
                if (tr.EOF) break;
                //read til we get the top of the next set of image bounds for a path
                if (tr.IsStartElement() && tr.Name == "numBGImagesThisPath")
                {
                    tr.Read();
                    int numImagesThisPath = Convert.ToInt32(tr.Value);
                    int pathImageBoundsRead = 0;
                    pathDescription path = new pathDescription();
                    path.imageBounds = new List<ImageBounds>();

                    //read off a complete set of bounds for a path
                    while (tr.Read() && pathImageBoundsRead < numImagesThisPath)  //loop over all image bounds in the folder
                    {
                        //read a complete set of image bounds
                        ImageBounds ib = new ImageBounds(); // fill an image bounds structure
                        int numBounds = 0;
                        while (tr.Read() && numBounds < 4)
                        {
                            if (tr.IsStartElement() && tr.Name == "north")
                            {
                                tr.Read();
                                ib.northDeg = Convert.ToDouble(tr.Value);
                                numBounds++;
                            }

                            if (tr.IsStartElement() && tr.Name == "east")
                            {
                                tr.Read();
                                ib.eastDeg = Convert.ToDouble(tr.Value);
                                numBounds++;
                            }

                            if (tr.IsStartElement() && tr.Name == "south")
                            {
                                tr.Read();
                                ib.southDeg = Convert.ToDouble(tr.Value);
                                numBounds++;
                            }

                            if (tr.IsStartElement() && tr.Name == "west")
                            {
                                tr.Read();
                                ib.westDeg = Convert.ToDouble(tr.Value);
                                numBounds++;
                            }
                        }
                        path.imageBounds.Add(ib);

                        pathImageBoundsRead++;
                    }
                    LFSummary.paths.Add(path);
                    numPathsProcessedForBounds++;
               }
            }  //end of reading in the map bounds along the paths

            if (tr.EOF)
            {
                MessageBox.Show("Bad path info format for the input kml file -- exiting");
                Environment.Exit(-1);
            }

            //read in the smoothed trajectory for each path
            UTM2Geodetic utm = new UTM2Geodetic();

            //we are ready to read off the coordinates of the smoothed trajectory
            int numPathsProcessed = 0;
            while (tr.Read() && !completedReadingSmoothedPathPoints)
            {
                if (tr.EOF) break;
                if (tr.IsStartElement() && tr.Name == "name")
                {
                    tr.Read();
                    //we will encounter this statement "numberParallelPaths"  times
                    if (tr.Value == "Smoothed Linear Feature Trajectory")
                    {

                        pathDescription path = LFSummary.paths[numPathsProcessed];
                        path.pathGeoDeg = new List<PointD>();
                        path.pathUTM = new List<PointD>();
                        path.commandedAltAlongPath = new List<double>();
                        bool thisPathComplete = false;
                        while (tr.Read() && !thisPathComplete)
                        {
                            //locate the coordinate tag for this path
                            if (tr.IsStartElement() && tr.Name == "coordinates")
                            {
                                tr.Read(); //read the complete coordinate dataset
                                char[] delimiterChars = { ',', ' ', '\t', '\n', '\r' };   //these delimiters were determined by looking at a file ...
                                //create a character string with all characters separated by a delimeter
                                string[] coordinateValues = tr.Value.ToString().Split(delimiterChars);

                                //the "value" is the text between the <coordinates> and </coordinates>
                                //below we read the complete string value and split it into separate substrings -- ugly but it works
                                //the substrings contain the individual coordinate values with some ""s and the heigts are "0".

                                //get the quantitative lat/long values from the text array ... there are a number of ""s in the text file ...
                                //each Google point has three values : longitude, Latitude, height -- we assume the height is zero here
                                int k = 0; int i = 0;
                                while (i < coordinateValues.Count())
                                {
                                    if (coordinateValues[i] != "")
                                    {
                                        double lat = Convert.ToDouble(coordinateValues[i + 1]);
                                        double lon = Convert.ToDouble(coordinateValues[i]);
                                        double alt = Convert.ToDouble(coordinateValues[i + 2]);

                                        path.pathGeoDeg.Add(new PointD(lon, lat) );
                                        path.commandedAltAlongPath.Add(alt);

                                        //convert the geodetic to UTM
                                        double UTMNorthing = 0.0, UTMEasting = 0.0;
                                        utm.LLtoUTM(lat * utm.Deg2Rad, lon * utm.Deg2Rad, ref UTMNorthing, ref UTMEasting, ref LFSummary.UTMZone, true);
                                        path.pathUTM.Add(new PointD(UTMEasting, UTMNorthing));

                                        k++;  //index of the storage array

                                        //increment the split array by 3 because the points are lat,lon,height
                                        i += 3;  //increment by 3 to get the next coordinate
                                    }
                                    else i++;  //here we skip the ""s in the text array
                                }
                                thisPathComplete = true;
                                numPathsProcessed++;
                            }

                        }  //end of while numPathsProcessed < LFSummary.numberParallelPaths

                        if (numPathsProcessed == LFSummary.numberParallelPaths)
                                   completedReadingSmoothedPathPoints = true;
                    }
                }
            }
            if (tr.EOF)
            {
                MessageBox.Show("Bad smoothed trajectory format for the input kml file -- exiting");
                Environment.Exit(-1);
            }

            return LFSummary;

            //////////////////////////////////////////////////////////////
            // the Project Summary for Waldo_FCS is Complete
            //////////////////////////////////////////////////////////////
        }
示例#3
0
        //constructor for the linearFeature coverage form
        public Mission(String _FlightPlanFolder, String _MissionDataFolder, String MissionDateStringNameIn, int _missionNumber, 
            linearFeatureCoverageSummary _LFSum, LogFile _logFile,
            NavInterfaceMBed navIF_In, SDKHandler cameraIn, bool simulatedMission_, bool hardwareAttached_, Image _projectImage)
        {
            InitializeComponent();

            coverageType = COVERAGE_TYPE.linearFeature;

            this.FormBorderStyle = System.Windows.Forms.FormBorderStyle.None;

            posVel_ = new PosVel();

            //set the mission image
            this.Width = (int)(mapScaleFactor * mapWidth);
            this.Height = (int)(mapScaleFactor * mapHeight);
            //this.Width = 640;    //pixel height of the form
            //this.Height = 480;   //pixel width of the form

            //retrieve local variables from the arguments
            missionNumber = _missionNumber;
            LFSum = _LFSum;
            MissionDataFolder = _MissionDataFolder;
            FlightPlanFolder = _FlightPlanFolder;

            navIF_ = navIF_In;
            camera = cameraIn;
            logFile = _logFile;
            MissionDateStringName = MissionDateStringNameIn;

            projectImage = _projectImage;

            //NOTE: if the simulatedMission=true, we will always generate the platform state from the software
            // If hardwareAttached=true, we will collect the IMU and GPS
            simulatedMission = simulatedMission_;
            hardwareAttached = hardwareAttached_;

            //st up the form to allow keydown events only in the simulation
            if (simulatedMission)
            {
                this.KeyPreview = true;
            }

            timeFromTrigger     = new Stopwatch();
            //showMessage       = new Stopwatch();
            elapsedTime         = new Stopwatch();
            getPosVelTimer      = new Stopwatch();

            //placeholder for the first of the path image bounds
            ib = LFSum.paths[0].imageBounds[0];  //placeholder for the project image bounds

            //multiplier used for pix-to-geodetic conversion for the project map -- scales lat/lon to pixels
            //NOTE -- we do the drawing on top of a bitmap sized to the mapWidth, mapHeight -- then stretch to fit the actual screen
            lon2PixMultiplier =  mapWidth /  (ib.eastDeg - ib.westDeg);
            lat2PixMultiplier = -mapHeight / (ib.northDeg - ib.southDeg);  //"-" cause vertical map direction is positive towards the south
            //lon2PixMultiplier =  mapWidth / (ib.eastDeg - ib.westDeg);
            //lat2PixMultiplier = -mapHeight / (ib.northDeg - ib.southDeg);  //"-" cause vertical map direction is positive towards the south

            platFormPosVel = new PosVel();
            platFormPosVel.GeodeticPos = new PointD(0.0, 0.0);
            platFormPosVel.UTMPos = new PointD(0.0, 0.0);

            //this will hold the locations of the aircraft over a period of time
            crumbTrail = new Point[numberCrumbTrailPoints];

            labelPilotMessage.Visible = false;

            //form the along-Path distance at each point (vertex)
            //will be used for interpolating the commanded altitude along the path
            for (int j = 0; j < LFSum.paths.Count; j++ )
            {
                LFSum.paths[j].alongPathDistanceAtVertex = new List<double>();
                double cumulativeDistance = 0;
                for (int i=0; i<LFSum.paths[j].pathUTM.Count; i++)
                    if (i == 0) LFSum.paths[j].alongPathDistanceAtVertex.Add(0.0);
                else
                {
                    double delX = LFSum.paths[j].pathUTM[i].X - LFSum.paths[j].pathUTM[i - 1].X;
                    double delY = LFSum.paths[j].pathUTM[i].Y - LFSum.paths[j].pathUTM[i - 1].Y;
                    cumulativeDistance += Math.Sqrt(delX * delX + delY * delY);
                    LFSum.paths[j].alongPathDistanceAtVertex.Add(cumulativeDistance);
                }
            }
        }
示例#4
0
        //constructor for MissionSelection Form Linear Feature mission
        public MissionSelection(linearFeatureCoverageSummary _LFSum, String _FlightPlanFolder, LogFile _logFile,
            NavInterfaceMBed navIF_In, SDKHandler cameraIn, bool hardwareAttached_, SettingsManager _settings, String _MissionDateString)
        {
            InitializeComponent();

            posVel_ = new PosVel();

            //set the flight plans folder and the Project Summary structure from the prior Project Selection
            FlightPlanFolder = _FlightPlanFolder;
            LFSum = _LFSum;
            navIF_ = navIF_In;
            camera = cameraIn;
            hardwareAttached = hardwareAttached_;
            settings = _settings;
            MissionDateString = _MissionDateString;
            logFile = _logFile;

            projectName = LFSum.ProjectName;

            //this is a specific constructor for the linear feature coverage type
            coverageType = COVERAGE_TYPE.linearFeature;

            getPosVelTimer = new Stopwatch();
            utm = new UTM2Geodetic();

            //lat/lon image bounds from the mission plan
            ib = LFSum.ProjectImage;  //placeholder for the project image bounds NOTE:  this is also used elsewhere

            //multiplier used for pix-to-geodetic conversion for the project map -- scales lat/lon to pixels
            // TODO:  ugly --- cant we do this exactly???
            //lon2PixMultiplier = mapScaleFactor * mapWidth / (ib.eastDeg - ib.westDeg);
            //lat2PixMultiplier = -mapScaleFactor * mapHeight / (ib.northDeg - ib.southDeg);  //"-" cause vertical map direction is positive towards the south
            lon2PixMultiplier =  mapWidth / (ib.eastDeg   - ib.westDeg);
            lat2PixMultiplier = -mapHeight / (ib.northDeg - ib.southDeg);  //"-" cause vertical map direction is positive towards the south
        }