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 ////////////////////////////////////////////////////////////// }
//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); } } }
//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 }