public static bool imageBoundAContainedInImageBoundB(ImageBounds A, ImageBounds B)
{
if (
A.eastDeg < B.eastDeg && A.eastDeg > B.westDeg &&
A.westDeg > B.westDeg && A.westDeg < B.eastDeg &&
A.northDeg < B.northDeg && A.northDeg > B.southDeg &&
A.southDeg > B.southDeg && A.southDeg < B.northDeg) return true;
else return false;
}
public ProjectSummary readPolygonCoverageData(XmlReader tr, String ProjectName)
{
///////////////////////////////////////////////////////////////////////
//read in the original client polygon outline of the site
///////////////////////////////////////////////////////////////////////
bool completedProjectPolygon = false;
bool completedProjectData = false;
bool completedMissionImageBounds = false;
ProjectSummary projSum = new ProjectSummary();
projSum.coverageType = COVERAGE_TYPE.polygon;
projSum.ProjectName = ProjectName; //this is the name of the kml file -- maybe should use the name of the placemark???
projSum.ProjectPolygon = new List<PointD>();
projSum.msnSum = new List<MissionSummary>();
//photocenters are on a regular grid with a fixed origin -- redined from the input file
projSum.gridOrigin = new PointD(0.0, 0.0);
//inspecting the header is complete --- new interrogate the remainder
while (tr.Read() && !completedProjectData)
{
if (tr.IsStartElement() && tr.Name == "Placemark")
{
while (tr.Read() && !completedProjectPolygon)
{
if (tr.IsStartElement() && tr.Name == "name")
{
tr.Read();
//the Placemark name should be the same as the mission plan kml name
if (tr.Value == ProjectName) //here we check to see if the placemark name is the input kml filename
{
while (tr.Read() && !completedProjectPolygon)
{
if (tr.IsStartElement() && tr.Name == "coordinates")
{
tr.Read();
char[] delimiterChars = { ',', ' ', '\t', '\n', '\r' }; //these delimiters were determined by looking at a file ...
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]);
projSum.ProjectPolygon.Add(new PointD(lon, lat));
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
}
completedProjectPolygon = true;
}
}
}
else
{
MessageBox.Show("The mission plan file has the wrong format \n The Placemark name is not the same as the kml file name");
}
}
}
} //end of getting projectPolygon stored as a Placemark
//////////////////////////////////////////////////////////////////////////////////
//read in the Project-specific Data for this Project
//////////////////////////////////////////////////////////////////////////////////
//get the geodetic bopunds of the project polygon
if (tr.IsStartElement() && tr.Name == "north" && !completedProjectData)
{
tr.Read();
projSum.ProjectImage.northDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "south" && !completedProjectData)
{
tr.Read();
projSum.ProjectImage.southDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "east" && !completedProjectData)
{
tr.Read();
projSum.ProjectImage.eastDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "west" && !completedProjectData)
{
tr.Read();
projSum.ProjectImage.westDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "downRangePhotoSpacingMeters" && !completedProjectData)
{
tr.Read();
projSum.downrangeTriggerSpacing = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "crossRangeSwathWidth" && !completedProjectData)
{
tr.Read();
projSum.crossRangeSwathWidth = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "numberOfMissons" && !completedProjectData)
{
tr.Read();
projSum.numberOfMissions = Convert.ToInt32(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "UTMZone" && !completedProjectData)
{
tr.Read();
projSum.UTMZone = tr.Value;
}
if (tr.IsStartElement() && tr.Name == "gridOriginUTMNorthing" && !completedProjectData)
{
tr.Read();
projSum.gridOrigin.Y = 1.0;
projSum.gridOrigin.Y = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "gridOriginUTMEasting" && !completedProjectData)
{
tr.Read();
projSum.gridOrigin.X = Convert.ToDouble(tr.Value);
///////////////////////////////////////////////////////////////////////////////////
completedProjectData = true; ///Easting is the last one of the data elements
///////////////////////////////////////////////////////////////////////////////////
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//read in the bounds of the mission images -- images are stored separately in a folder: ProjectName_Background\background_YY.jpg
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
List<ImageBounds> MissionImage = new List<ImageBounds>(); // we will use this later to fill the mission Summarys
while (tr.Read() && !completedMissionImageBounds)
{
if (tr.IsStartElement() && tr.Name == "name") //takes us to the folder containing the mission images
{
tr.Read();
if (tr.Value == " Summary Images") //verifies that this is the correct folder
{
int missionImageBoundRead = 0;
while (tr.Read() && missionImageBoundRead < projSum.numberOfMissions) //loop over all image bounds in the folder
{
if (tr.IsStartElement() && tr.Name == "name") //found the name element for next mission image bounds
{
while (tr.Read() && missionImageBoundRead < projSum.numberOfMissions)
{
if (tr.Value == " Summary_" + missionImageBoundRead.ToString("D3") + " ") //found correct mission name
{
ImageBounds ib = new ImageBounds(); // fill an image bounds structure
while (tr.Read())
{
if (tr.IsStartElement() && tr.Name == "north")
{
tr.Read();
ib.northDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "east")
{
tr.Read();
ib.eastDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "south")
{
tr.Read();
ib.southDeg = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "west")
{
tr.Read();
ib.westDeg = Convert.ToDouble(tr.Value);
MissionImage.Add(ib); //fill the image bounds structure
missionImageBoundRead++; //the west is always the last element
break;
}
}
}
}
}
}
completedMissionImageBounds = true;
}
}
}
/////////////////////////////////////////////////////////////////////////////
// read in the per-mission data
// the sets of information
// (1) mission-specific parameters
// (2) mission-specfic polygon
// (3) flight line ends
/////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
//read in the mission-specific data values for this mission
//////////////////////////////////////////////////////////////////////
int missionNumberCounter = 0;
int flightLineNumber = 0; //flight line number increment sequentially across each mission --- are not renumbered at a mission
while (missionNumberCounter < projSum.numberOfMissions)
{
bool completedPerMissionData = false;
//we are at the top of a mission summary dataset
MissionSummary msnSum = new MissionSummary();
while (tr.Read() && !completedPerMissionData) //loop here til finished the mission-specific parameters
{
if (tr.IsStartElement() && tr.Name == "missionNumber")
{
tr.Read();
int missionNumber = Convert.ToInt32(tr.Value);
while (tr.Read() && !completedPerMissionData)
{
if (tr.IsStartElement() && tr.Name == "numFLs")
{
tr.Read();
msnSum.numberOfFlightlines = Convert.ToInt32(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "flightAltMSL")
{
tr.Read();
msnSum.flightAltMSLft = Convert.ToDouble(tr.Value);
completedPerMissionData = true;
}
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////
//read in the mission-specific polygon formed from flight line endpoints
////////////////////////////////////////////////////////////////////////////////////////////////
bool completedThisMissionPolygon = false;
while (tr.Read() && !completedThisMissionPolygon) //loop here til finished the mission-specific polygon
{
msnSum.missionGeodeticPolygon = new List<PointD>();
while (tr.Read() && !completedThisMissionPolygon)
{
//first get the mission polygon
if (tr.IsStartElement() && tr.Name == "name")
{
tr.Read();
if (tr.Value == " MissionPolygon_" + missionNumberCounter.ToString("D2") + " ")
{
while (tr.Read())
{
if (tr.IsStartElement() && tr.Name == "coordinates")
{
tr.Read();
char[] delimiterChars = { ',', ' ', '\t', '\n', '\r' }; //these delimiters were determined by looking at a file ...
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]);
msnSum.missionGeodeticPolygon.Add(new PointD(lon, lat));
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
}
completedThisMissionPolygon = true;
break;
}
}
}
}
}
} //end of completedThisMissionPolygon
////////////////////////////////////////////////////////////////
//read the flight line data for each mission
////////////////////////////////////////////////////////////////
bool completedThisMissionFlightLines = false;
int flightlinesCounterInMission = 0;
double FLLengthMeters = 0;
msnSum.FlightLinesCurrentPlan = new List<endPoints>();
while (tr.Read() && !completedThisMissionFlightLines) //loop here til read in all the flight lines for this mission
{
//first get the mission polygon
if (tr.IsStartElement() && tr.Name == "name")
{
tr.Read();
// FlightlineNumber_000
if (tr.Value == " FlightlineNumber_" + flightLineNumber.ToString("D3") + " ")
{
while (tr.Read())
{
// <lengthMeters>25266.9</lengthMeters>
if (tr.IsStartElement() && tr.Name == "lengthMeters")
{
tr.Read();
FLLengthMeters = Convert.ToDouble(tr.Value);
}
if (tr.IsStartElement() && tr.Name == "coordinates")
{
tr.Read();
char[] delimiterChars = { ',', ' ', '\t', '\n', '\r' }; //these delimiters were determined by looking at a file ...
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;
PointD[] ends = new PointD[2];
while (i < coordinateValues.Count())
{
if (coordinateValues[i] != "")
{
double lat = Convert.ToDouble(coordinateValues[i + 1]);
double lon = Convert.ToDouble(coordinateValues[i]);
ends[k] = new PointD(lon, lat);
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
}
//NOTE: the structure for the flight line info includes the original sequally-numbered flight line numbers
// But the FlightLinesCurrentPlan List is indexed from [0] to numFlightLinesThis mission
//photocenter offset (0) added to allow numbering adjustment for reflown lines
msnSum.FlightLinesCurrentPlan.Add(new endPoints(flightLineNumber, ends[0], ends[1],FLLengthMeters,0));
flightlinesCounterInMission++;
flightLineNumber++;
if (flightlinesCounterInMission == msnSum.numberOfFlightlines)
{
completedThisMissionFlightLines = true;
msnSum.MissionImage = MissionImage[missionNumberCounter]; //MissionImage was filled earlier
missionNumberCounter++; //increment the mission counter
}
//completedThisMissionFlightLines = true;
break;
}
}
}
}
}
//at this time declare the mission as zero-percent complete
//fill this in when we anayze the prior flown missions
msnSum.percentComplete = 0;
projSum.msnSum.Add(msnSum);
} //end of collectiong the mission data
return projSum;
//////////////////////////////////////////////////////////////
// the Project Summary for Waldo_FCS is Complete
//////////////////////////////////////////////////////////////
}
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
//////////////////////////////////////////////////////////////
}
private Point GeoToPix(PointD LonLat, ImageBounds _ib, double _lat2PixMultiplier, double _lon2PixMultiplier)
{
///////////////////////////////////////////////////////////////////////////////////
//maps are known to represent a rectangle of latitude and longitude
//displayed bmp images are 640*lat2PixMultiplier X 480*lat2PixMultiplier
//This procedure computes pixel coordinates from geodetic coordinates
//bmp image pixels are sclaed to geodetic coordinates using the elements of ib
///////////////////////////////////////////////////////////////////////////////////
Point pt = new Point();
pt.Y = Convert.ToInt32((LonLat.Y - _ib.northDeg) * _lat2PixMultiplier); //this rounds
pt.X = Convert.ToInt32((LonLat.X - _ib.westDeg) * _lon2PixMultiplier); //this rounds
return pt;
}
private void generateNewMergedMap(int pathNumber, int currentAlongPathMap, ImageBounds displayImageBounds)
{
//Form a new mergedMap from currentAlongPathMap+1 and currentAlongPathMap+2
//get new mergedMap bounds
ImageBounds ib0 = LFSum.paths[pathNumber].imageBounds[currentAlongPathMap];
ImageBounds ib1 = LFSum.paths[pathNumber].imageBounds[currentAlongPathMap + 1];
//find the merged geodetic bounds from the map descriptions from the mission plan
ib0 = LFSum.paths[pathNumber].imageBounds[currentAlongPathMap];
ib1 = LFSum.paths[pathNumber].imageBounds[currentAlongPathMap + 1];
//assume merged map boounds are from "currentAlongPathMap + 1" and update as required
mergedMapBounds = ib1;
if (ib0.northDeg > ib1.northDeg) mergedMapBounds.northDeg = ib0.northDeg;
if (ib0.southDeg < ib1.southDeg) mergedMapBounds.southDeg = ib0.southDeg;
if (ib0.eastDeg > ib1.eastDeg) mergedMapBounds.eastDeg = ib0.eastDeg;
if (ib0.westDeg < ib1.westDeg) mergedMapBounds.westDeg = ib0.westDeg;
//map names as stored during the mission plan
alongPathImageName1 = FlightPlanFolder + LFSum.ProjectName + "_Background\\linearMap_" + pathNumber.ToString("D2")
+ "_" + currentAlongPathMap.ToString("D3") + ".png";
alongPathImageName2 = FlightPlanFolder + LFSum.ProjectName + "_Background\\linearMap_" + pathNumber.ToString("D2")
+ "_" + (currentAlongPathMap + 1).ToString("D3") + ".png";
//set the map scaling parameters for the mergedMap
lon2PixMultiplier = mergedMapMultiplier * mapWidth / (mergedMapBounds.eastDeg - mergedMapBounds.westDeg);
lat2PixMultiplier = -mergedMapMultiplier * mapHeight / (mergedMapBounds.northDeg - mergedMapBounds.southDeg);
//below statement allows use of class member ib within the geoToPix and PixToGeo .. klutzy: need to fix this ...
ib = mergedMapBounds;
//alongPath map bounds pixel coordinates within the mergedMap
//used to fill the merged map with the two along-path components
Point alongPathMapNW_0 = GeoToPix(new PointD(ib0.westDeg, ib0.northDeg));
Point alongPathMapNW_1 = GeoToPix(new PointD(ib1.westDeg, ib1.northDeg));
Point alongPathMapSE_0 = GeoToPix(new PointD(ib0.eastDeg, ib0.southDeg));
Point alongPathMapSE_1 = GeoToPix(new PointD(ib1.eastDeg, ib1.southDeg));
//get graphics object that allows us to draw onto the mergedMap bitmap: mergedMap
//from setupLinearMission: mergedMap = new Bitmap(mergedMapMultiplier * mapWidth, mergedMapMultiplier * mapHeight);
Graphics g1 = Graphics.FromImage(mergedMap);
//set pixel-space rectangle within mergedMap wherein we draw the alongPath map 0
Rectangle rect0 = new Rectangle(alongPathMapNW_0.X, alongPathMapNW_0.Y,
alongPathMapSE_0.X - alongPathMapNW_0.X,
alongPathMapSE_0.Y - alongPathMapNW_0.Y);
//set pixel-space rectangle within mergedMap wherein we draw the alongPath map 1
Rectangle rect1 = new Rectangle(alongPathMapNW_1.X, alongPathMapNW_1.Y,
alongPathMapSE_1.X - alongPathMapNW_1.X,
alongPathMapSE_1.Y - alongPathMapNW_1.Y);
//draw the alongPath maps onto the merged map bitmap object
g1.DrawImage(Image.FromFile(alongPathImageName1), rect0);
g1.DrawImage(Image.FromFile(alongPathImageName2), rect1);
//mergedMap.Save(@"C://temp//testImage1.png");
//draw the paths onto the mergedMap that now contains the map components
for (int j = 0; j < LFSum.paths.Count; j++)
{
for (int i = 1; i < LFSum.paths[j].pathGeoDeg.Count; i++)
{
Point p1 = GeoToPix(LFSum.paths[j].pathGeoDeg[i - 1]);
Point p2 = GeoToPix(LFSum.paths[j].pathGeoDeg[i]);
g1.DrawLine(new Pen(Color.Black, 1), p1, p2);
}
}
//show the semi-infinite line
g1.DrawLine(new Pen(Color.Blue, 1), GeoToPix(FPGeometry.semiInfiniteFLstartGeo), GeoToPix(LFSum.paths[pathNumber].pathGeoDeg[0] ));
//recreate the trigger point locations on this new mergedMap
triggerPointsOnMergedMap.Clear();
//Console.WriteLine(" points before in triggerPointsOnMergedMap = " + triggerPointsOnMergedMap.Count.ToString());
foreach (PointD p in LFSum.paths[pathNumber].triggerPoints) //trigger points stored as geodetic
{
//Console.WriteLine(" " + p.X.ToString() + " " + p.Y.ToString());
triggerPointsOnMergedMap.Add(GeoToPix(p));
}
//Console.WriteLine(" points after in triggerPointsOnMergedMap = " + triggerPointsOnMergedMap.Count.ToString());
g1.Dispose();
}
private ImageBounds generateDisplayMapBounds(int pathNumber)
{
//define a 4mi x 3mi UTM rectangle about the current aircraft position
//that is: +/-2 mi (EW) and +/-1.5mi (NS) about the aircraft location
//this defines the geospatial rectangle of the map will be displayed on the Mission Form
//bounds for the 4X3 mi rectangle in UTM
PointD NWdisplayBoundUTM = new PointD(platFormPosVel.UTMPos.X - 2 * 5280.0 * 0.3048, platFormPosVel.UTMPos.Y + 1.5 * 5280.0 * 0.3048);
PointD SEdisplayBoundUTM = new PointD(platFormPosVel.UTMPos.X + 2 * 5280.0 * 0.3048, platFormPosVel.UTMPos.Y - 1.5 * 5280.0 * 0.3048);
//convert the 4X3 rectangle bounds to geodetic
PointD NWdisplayBoundGeo = new PointD();
utm.UTMtoLL(NWdisplayBoundUTM, LFSum.UTMZone, ref NWdisplayBoundGeo);
PointD SEdisplayBoundGeo = new PointD();
utm.UTMtoLL(SEdisplayBoundUTM, LFSum.UTMZone, ref SEdisplayBoundGeo);
ImageBounds displayImageBounds = new ImageBounds();
displayImageBounds.northDeg = NWdisplayBoundGeo.Y;
displayImageBounds.southDeg = SEdisplayBoundGeo.Y;
displayImageBounds.eastDeg = SEdisplayBoundGeo.X;
displayImageBounds.westDeg = NWdisplayBoundGeo.X;
return displayImageBounds;
}
//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);
}
}
}
private void setupLinearFeatureMission(int pathNumber)
{
//////////////////////////////////////
//called from btn_OK_clicked
//initializes the simuation
//////////////////////////////////////
//simulation trajectory start point in pixel coordinates
Point startPlatformPoint = new Point(FlightLineStartPix.X, FlightLineStartPix.Y);
this.lblFlightAlt.Visible = false;
this.lblFlightLines.Visible = false;
this.lblMissionNumber.Visible = false;
btnOK.Visible = true; //dont need this anymore --- reset to visible if we return to a selected mission
btnBack.Text = "EXIT"; // this is a better name because we exit the realtime mission and return to the mission selection Form
//note we can exit a mission in the middle of a line and renter the mission at the exited point.
//get all flightline geometry that is invariant for traveling along this path
FPGeometry = new FlightPathLineGeometry(pathNumber, LFSum);
utm = new UTM2Geodetic();
//initialize the position when in sim mode
if (simulatedMission)
{
////////////////////////////////////////////////////////////////////////////////
//set the position along the semi-infinite line at the start of the first path
////////////////////////////////////////////////////////////////////////////////
PointD startUTM = new PointD();
//simulation is initiated 5000m from the start and headed towards the start
startUTM.X = LFSum.paths[pathNumber].pathUTM[0].X + 2000.0 * FPGeometry.unitAwayFromStartUTM.X + 100.0 * FPGeometry.unitAwayFromStartUTM.Y;
startUTM.Y = LFSum.paths[pathNumber].pathUTM[0].Y + 2000.0 * FPGeometry.unitAwayFromStartUTM.Y - 100.0 * FPGeometry.unitAwayFromStartUTM.X;
PointD startGeo = new PointD();
utm.UTMtoLL(startUTM, LFSum.UTMZone, ref startGeo);
platFormPosVel.UTMPos.X = startUTM.X;
platFormPosVel.UTMPos.Y = startUTM.Y;
platFormPosVel.GeodeticPos.X = startGeo.X;
platFormPosVel.GeodeticPos.Y = startGeo.Y;
//set the altitude at the initial commanded altitude (input in ft)
platFormPosVel.altitude = LFSum.paths[0].commandedAltAlongPath[0] * 0.3048;
//////////////////////////////////////////////////////////
speed = 51.4; // 100 knots
//////////////////////////////////////////////////////////
platFormPosVel.velD = 0.0;
//negative sigh cause velocity towards the start of the path
platFormPosVel.velE = -speed * FPGeometry.unitAwayFromStartUTM.X;
platFormPosVel.velN = -speed * FPGeometry.unitAwayFromStartUTM.Y;
}
FPGeometry.getPlatformToFLGeometry(platFormPosVel);
//pre-load the crumbtrail array prior to the start point
//for (int i = 0; i < numberCrumbTrailPoints; i++) crumbTrail[i] = startPlatformPoint;
//merged maps combine two along-path maps so the moving map shows all the subtended terrain
//set up the first mergedMap
mergedMapBounds = new ImageBounds(); //initialize this using first 2 images
currentAlongPathMap = 0;
triggerCountAlongpath = 0;
mergedMap = new Bitmap(mergedMapMultiplier * mapWidth, mergedMapMultiplier * mapHeight);
//triggerPointsOnMergedMap saves the camera trigger points along the merged map
//used to present the crumb trail
LFSum.paths[pathNumber].triggerPoints = new List<PointD>();
//tempTriggerPoints = new List<PointD>();
triggerPointsOnMergedMap = new List<Point>();
ImageBounds displayImageBounds = generateDisplayMapBounds(pathNumber);
//generate the first merged map for this path
generateNewMergedMap(pathNumber, currentAlongPathMap, displayImageBounds);
//this is hardwired in btn_OK
deltaT = 0.25;
//no crumbtrail used for the linear path
numberCrumbTrailPoints = 5;
bm3 = new Bitmap(mapWidth, mapHeight, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
prepLinearFeatureBitmapForPaint();
}
private void prepLinearFeatureBitmapForPaint()
{
/////////////////////////////////////////////////////////////////////////////////////
//called from realTimeAction
//for the linearFeature --- most of the realtime linearFeature action occurs here
//realTime action is called in a real-time loop that is in btnOK_click
/////////////////////////////////////////////////////////////////////////////////////
//the aircraft position is available from other processes ongoing in the realTimeAction loop
//all trigger responses from camera (image placed on camera HD) are also handled in realTimeAction
//but trigger request is handled here.
//general strategy for mergedMap prep to treat the moving map display
//A set of fixed-size alongpath maps are available from the mission planning
//use a rectangle about the aircraft position to determine if we need a new mergedMap
//a mergedMap is formed from two rectangular alongPathMaps that are centered along the path at regular intervals
//for each aircraft position, determine a 4mi x 3mi geodetic rectangular around the aircraft wherein we will display a map
//test to see if this rectangle is fully contained in the next (currentpath+1) alongPathMap
//if yes, then generate a new mergedMap from currentMap and currentMap + 1
//define a 4mi x 3mi UTM rectangle about the current aircraft position
//that is: +/-2 mi (EW) and +/-1.5mi (NS) about the aircraft location
//this defines the geospatial rectangle of the map that will be displayed on the Mission Form
ImageBounds displayImageBounds = generateDisplayMapBounds(pathNumber);
//we will map this 4X3 mi rectangle about the aircraft location into the display map.
//////////////////////////////////////////////////////////////////////////////////////
//must treat the case where the aircraft moves off the alongPathMap sequence!!!
//test if this displayMap bounds is within the current alongPathMap.
//if yes, do nothing. if no, is it in the next alongPathMap
//if yes, then create a new merged map from the current map and the next map
//if no, then see if the displayMap is in ANY alongPathMap (to handle case where we reenter the alongpathap sequence)
//if yes, then set the currentMap to this alongPathMap
//if no, then set the mergedMap to the a static (non-moving) projectMap
///////////////////////////////////////////////////////////////////////////////////////
bool aircraftWithinAlongPathMaps = true;
//test if this aircraft-centric 4X3 mi rectangle is inside the next alongPathMap
//if yes, then create a new mergedMap using the next alongPathMap
int lastAlongPathMap = LFSum.paths[pathNumber].imageBounds.Count;
//if (currentAlongPathMap < LFSum.paths[pathNumber].imageBounds.Count - 2)
{
if (polygonMath.imageBoundAContainedInImageBoundB(displayImageBounds, LFSum.paths[pathNumber].imageBounds[currentAlongPathMap]) ||
polygonMath.imageBoundAContainedInImageBoundB(displayImageBounds, LFSum.paths[pathNumber].imageBounds[lastAlongPathMap-1]))
{
//do nothing -- the currentAlongPath is correct
if (!mergedMapAvailable)
{
generateNewMergedMap(pathNumber, currentAlongPathMap, displayImageBounds);
mergedMapAvailable = true;
Console.WriteLine("creating a new mergedMap when none available for current alongPathMap " + currentAlongPathMap.ToString());
}
}
else if ( currentAlongPathMap < (LFSum.paths[pathNumber].imageBounds.Count - 2) && //test for running out of alongPathMaps
polygonMath.imageBoundAContainedInImageBoundB(displayImageBounds, LFSum.paths[pathNumber].imageBounds[currentAlongPathMap + 1]))
{
currentAlongPathMap++;
//generate a new mergedMap from which we will cut the portion to display on the moving map
generateNewMergedMap(pathNumber, currentAlongPathMap, displayImageBounds);
mergedMapAvailable = true;
Console.WriteLine("create a new MergedMap at " + missionTimerTicks.ToString() + " currentAlongPathMap= " + currentAlongPathMap.ToString());
}
else
{
//test to see if we are in any alongPathMap
bool notInAnyAlongPathMap = true;
for (int i = 0; i < LFSum.paths[pathNumber].imageBounds.Count; i++)
{
if (polygonMath.imageBoundAContainedInImageBoundB(displayImageBounds, LFSum.paths[pathNumber].imageBounds[i]))
{
currentAlongPathMap = i;
//current map in last alongPathMap --- back it up so we can form mergedMap with currentAlongPathMap+1
if (currentAlongPathMap == LFSum.paths[pathNumber].imageBounds.Count - 1) currentAlongPathMap--;
//generate a new mergedMap from which we will cut the portion to display on the moving map
generateNewMergedMap(pathNumber, currentAlongPathMap, displayImageBounds);
mergedMapAvailable = true;
notInAnyAlongPathMap = false;
Console.WriteLine("Located a new alongPathMap from searching all maps");
break;
}
}
if (notInAnyAlongPathMap) // set the projectMap as a static (non-moving) map
{
aircraftWithinAlongPathMaps = false;
mergedMapAvailable = false;
//re set the map scaling parameters for the overview projectMap
lon2PixMultiplier = mapWidth / (LFSum.ProjectImage.eastDeg - LFSum.ProjectImage.westDeg);
lat2PixMultiplier = -mapHeight / (LFSum.ProjectImage.northDeg - LFSum.ProjectImage.southDeg);
ib = LFSum.ProjectImage;
//lon2PixMultiplier, lat2PixMultiplier, ib -- are used internal to GeoToPix to set the map scaling
//get graphics object that allows us to draw onto the static projectMap
//from setupLinearMission: mergedMap = new Bitmap(mergedMapMultiplier * mapWidth, mergedMapMultiplier * mapHeight);
Graphics g2 = Graphics.FromImage(bm3);
g2.DrawImage(projectImage, 0, 0); //fixed invariant (non-moving map)
//draw the paths onto the projectMap
for (int j = 0; j < LFSum.paths.Count; j++)
{
for (int i = 1; i < LFSum.paths[j].pathGeoDeg.Count; i++)
{
Point p1 = GeoToPix(LFSum.paths[j].pathGeoDeg[i - 1]);
Point p2 = GeoToPix(LFSum.paths[j].pathGeoDeg[i]);
g2.DrawLine(new Pen(Color.Black, 1), p1, p2);
}
}
//show the semi-infinite line on the project map
g2.DrawLine(new Pen(Color.Blue, 1), GeoToPix(FPGeometry.semiInfiniteFLstartGeo), GeoToPix(LFSum.paths[pathNumber].pathGeoDeg[0]));
//recreate the trigger point locations on this project
triggerPointsOnMergedMap.Clear();
//Console.WriteLine(" points before in triggerPointsOnMergedMap = " + triggerPointsOnMergedMap.Count.ToString());
foreach (PointD p in LFSum.paths[pathNumber].triggerPoints) //trigger points stored as geodetic
{
//Console.WriteLine(" " + p.X.ToString() + " " + p.Y.ToString());
triggerPointsOnMergedMap.Add(GeoToPix(p));
}
//show a circle on the projectMap to locate the aircraft
Point acp = GeoToPix(platFormPosVel.GeodeticPos);
g2.DrawEllipse(new Pen(Color.Black,2), acp.X, acp.Y, 3, 3) ;
g2.Dispose();
}
//if in no alongPathMap -- set the merged map to the projectMap
}
}//end of the test of
if (aircraftWithinAlongPathMaps)
{
Graphics g1 = Graphics.FromImage(mergedMap);
//show the past image trigger circles as they are taken
foreach (Point p in triggerPointsOnMergedMap)
{
//draw a circle with diam 6 pixels at each of the trigger points for this mergedMap
g1.DrawEllipse(new Pen(Color.Red, 1), p.X - 3, p.Y - 3, 6, 6);
}
//destination of the portion of the merged map to display on the Mission form (the complete form)
Point[] destPoints = { new Point(0, 0), new Point(mapWidth, 0), new Point(0, mapHeight) };
//form rectangle defining the portion of the merged map to display on the Mission Form
//the merged map has a fixed size and scaling set in setupLinearFeatureMission()
//GeoToPix scaliong is set up for the merged map
Point NWdisplayBoundPix = GeoToPix(new PointD(displayImageBounds.westDeg, displayImageBounds.northDeg)); //scaled to the merged map
Point SEdisplayBoundPix = GeoToPix(new PointD(displayImageBounds.eastDeg, displayImageBounds.southDeg)); //scaled to the merged map
//rectangle in the mergedMap where we get the 4X3 mi map portion
Rectangle displayRect = new Rectangle(NWdisplayBoundPix,
new Size(SEdisplayBoundPix.X - NWdisplayBoundPix.X, SEdisplayBoundPix.Y - NWdisplayBoundPix.Y));
//graphics object for the Mission Form that will be displayed in the Paint event
//bm3 is 640 X 480
Graphics g3 = Graphics.FromImage(bm3);
//place the 4mi X 3 mi portion of the merged map onto the Mission Form
g3.DrawImage(mergedMap, destPoints, displayRect, GraphicsUnit.Pixel);
//show the stick aircraft in the center of the Mission Form
stickAircraftForMovingMap(g3);
//Console.WriteLine(" miss distance (m) = " + (100.0 * FPGeometry.LOSRate * LFSum.plannedRabbitDistanceAhead / FPGeometry.velMag).ToString() );
drawStickPlane(ref g3,
(int)(100.0 * FPGeometry.LOSRate * (LFSum.plannedRabbitDistanceAhead / FPGeometry.velMag)),
(int)(FPGeometry.headingToPath * Rad2Deg) );
g3.Dispose();
setupSteeringBarGFraphic(bm3);
//heading-to-path is +pi to -pi
//only allow photos if the heading-to-path is +/- 30 deg -- tolerance as in the polygon mission
//TGO is computed only if we are with a tolerance distance from the flight line
//if heading-to-path in tolerance and distanceAlongPath < 0 then TGO = -distanceAlongPath / velMag
//if distance to next path start is < pathLength, TGO = distanceToNextPathStart / velMag
//if (heading-to-path in tolerance and distanceAlongPath > 0
//trigger management while within the path endpoints
if (FPGeometry.distanceFromStartAlongPath > triggerCountAlongpath * LFSum.photocenterSpacing &&
FPGeometry.distanceFromStartAlongPath < FPGeometry.pathlengthMeters )
{
//send a request to the mbed to fire the trigger
//the image is snapped about 0.23 seconds after this request
triggerCountAlongpath++;
//LFSum.paths[pathNumber].triggerPoints.Add(platFormPosVel.GeodeticPos);
LFSum.paths[pathNumber].triggerPoints.Add(new PointD(platFormPosVel.GeodeticPos.X, platFormPosVel.GeodeticPos.Y));
//add this trigger point to the mergedMap display
triggerPointsOnMergedMap.Add(GeoToPix(platFormPosVel.GeodeticPos));
//offset = 0 -- no longer used
//missionNumber = -1 for the path coverage tso missionNumber no in photocenter label
kmlTriggerWriter.writePhotoCenterRec(-1, pathNumber, 0, triggerCountAlongpath, platFormPosVel);
Console.WriteLine("snap a picture " + missionTimerTicks.ToString() + " " + triggerCountAlongpath.ToString());
TGO = (FPGeometry.pathlengthMeters - FPGeometry.distanceFromStartAlongPath) / FPGeometry.velMag;
//write the kml file for the trigger
}
if (FPGeometry.distanceFromStartAlongPath < 0)
{
TGO = -FPGeometry.distanceFromStartAlongPath / FPGeometry.velMag;
}
if (inTurnAroundForLinearFeature) TGO = missionTimerTicks / 1000.0 - TimeFromExitingLastPath;
//detect the end of this path and transition to the next path
double pathSwitchExtension = 0.0; ///extends the switch just for the simulation
if (simulatedMission) pathSwitchExtension = 2000.0;
if (FPGeometry.distanceFromStartAlongPath > (FPGeometry.pathlengthMeters + pathSwitchExtension) && !inTurnAroundForLinearFeature)
{
//fire one last trigger at the exct of the flight line
triggerCountAlongpath++;
//set the exit tie for the TGO computation
TimeFromExitingLastPath = 0;
//increment the path counter
pathNumber++;
//detect the last path
if (pathNumber >= LFSum.paths.Count)
{
pathNumber = LFSum.paths.Count - 1;
lastPathHasBeenFlown = true;
}
currentAlongPathMap = 0;
mergedMapAvailable = false;
triggerCountAlongpath = 0;
Console.WriteLine(" switched path : " + pathNumber.ToString());
if (simulatedMission) //end of the line turn management
{
//turn the aircraft around ...
inTurnAroundForLinearFeature = true; //logic flag declaring in the turn
nextPathInitialHeading = FPGeometry.heading + Math.PI; //store the desired heading along the nextPath
double maxBank = 45.0; //max bank in the turn -- enables a fast turn
double turnRadiusAtMaxBank = speed * speed / (9.806 * Math.Tan(maxBank * Deg2Rad)); // turn radius at the max bank
gammaDotInTurn = speed / turnRadiusAtMaxBank; //turn rotation rate at the max bank for coordinated turn
//compute the turn direction to the startpoint of next path
//compute vector from the startPoint of next Path to the current platform
//UTM.X = Easting and UTM.Y = Northing -- vector direction towards the platform
PointD vec = new PointD(
LFSum.paths[pathNumber].pathUTM[0].X - platFormPosVel.UTMPos.X,
LFSum.paths[pathNumber].pathUTM[0].Y - platFormPosVel.UTMPos.Y);
//form cross-product of velocity and above vector. X is to the north & Y to the east for below cross product
turnDirection = platFormPosVel.velN * vec.X - platFormPosVel.velE * vec.Y;
//if crossProduct is positive (Z-axis pointed down) -- turn to the right (CW)
gammaDotInTurn = Math.Sign(turnDirection) * gammaDotInTurn;
}
//note: the path is reversed in the mission plan
//even paths (0, 2, 4, 6) are start-to-end relative to the plan input path
FPGeometry = new FlightPathLineGeometry(pathNumber, LFSum);
FPGeometry.getPlatformToFLGeometry(platFormPosVel);
//clear and reset the trigger file so it can be refilled this path
LFSum.paths[pathNumber].triggerPoints = new List<PointD>();
}
} //end of test for within alongPathMaps
if (!inTurnAroundForLinearFeature)
FPGeometry.getPlatformToFLGeometry(platFormPosVel);
//if sim -- update the sim -- done regardless of the map status
if (simulatedMission)
{
//this is a classic proportional navigation guidance law -- LOS rate is computed in FLGeometry
//the ProNav gain below is 3.0 .....
double gammaDot = 0.0;
if (inTurnAroundForLinearFeature)
{
heading = Math.Atan2(platFormPosVel.velE, platFormPosVel.velN);
gammaDot = gammaDotInTurn;
//find sine and cosine of the angle between the current heading and desired heading
double sin = Math.Cos(heading) * Math.Sin(nextPathInitialHeading) - Math.Sin(heading) * Math.Cos(nextPathInitialHeading);
double cos = Math.Cos(heading) * Math.Cos(nextPathInitialHeading) + Math.Sin(heading) * Math.Sin(nextPathInitialHeading);
//transition out of the turn when we have turned so that we have crossed the desired heading
if (cos > 0 && Math.Sign(turnDirection)*sin < 0) inTurnAroundForLinearFeature = false;
}
else
gammaDot = 3.0 * FPGeometry.LOSRate;
//user inputs a "X" to toggle the autosteering ....
//use can use the right and left arrow keys to steer the plane in heading
if (useAutoSteering && !lastPathHasBeenFlown)
FPGeometry.heading += gammaDot * deltaT;
platFormPosVel.velE = speed * Math.Sin(FPGeometry.heading);
platFormPosVel.velN = speed * Math.Cos(FPGeometry.heading);
platFormPosVel.velD = speed * Math.Sin(simulationPitch);
platFormPosVel.UTMPos.X += platFormPosVel.velE * deltaT;
platFormPosVel.UTMPos.Y += platFormPosVel.velN * deltaT;
platFormPosVel.altitude -= platFormPosVel.velD * deltaT;
//platform position is delivered back the Display preparation in Geodetic
//because thats what the GICS will give us
utm.UTMtoLL(platFormPosVel.UTMPos.Y, platFormPosVel.UTMPos.X, LFSum.UTMZone, ref platFormPosVel.GeodeticPos.Y, ref platFormPosVel.GeodeticPos.X);
}
else // the position and velocity state are provided by the GPS data
{
platFormPosVel.GeodeticPos.X = posVel_.GeodeticPos.X;
platFormPosVel.GeodeticPos.Y = posVel_.GeodeticPos.Y;
platFormPosVel.altitude = posVel_.altitude;
platFormPosVel.velN = posVel_.velN;
platFormPosVel.velE = posVel_.velE;
platFormPosVel.velD = posVel_.velD;
speed = Math.Sqrt(platFormPosVel.velN * platFormPosVel.velN + platFormPosVel.velE * platFormPosVel.velE);
}
}
private void preparePolygonMissionDisplayfixedBackground()
{
/////////////////////////////////////////////////////////////////////////////////////////////////
//prepare that part of the mission display that does not change when you are within a mission
//this includes the basic underlay map and the mission flight lines
//we also change the underlay map here if we exit the zoomed mission map
/////////////////////////////////////////////////////////////////////////////////////////////////
String MissionMap;
if (UseZImapForPolygonMission)
{
//load the Mission Map from the flight maps folder -- prepared with the mission planner
MissionMap = FlightPlanFolder + ps.ProjectName + @"_Background\Background_" + missionNumber.ToString("D2") + ".png";
//ib is used internally to the GeoToPix procedures
ib = ps.msnSum[missionNumber].MissionImage; //placeholder for the Mission image bounds
}
else
{
MissionMap = FlightPlanFolder + ps.ProjectName + @"_Background\ProjectMap.png";
ib = ps.ProjectImage;
}
//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
//the mission maps may be in either PNG or JPG -- based on the implementation of the mission planner
if (File.Exists(MissionMap))
img = Image.FromFile(MissionMap); //get an image object from the stored file
else
{
MessageBox.Show(" there is no mission map present: \n" + MissionMap, "ERROR", MessageBoxButtons.OKCancel);
Application.Exit();
}
//declare a bitmap using the Mission map img width and height for the size specifications
// img.Width, img.Height are 640 X 480 based on the Google map download limits
bm1 = new Bitmap(img.Width, img.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
//the graphics object lets us place graphics into the currently-blank bm1 bitmap
Graphics g = Graphics.FromImage(bm1); //create a graphics object from the base map image
//initialize the bm1 base bitmap with the mission map
//other drawings will be added to the base image to reflect changes from the base image
g.DrawImage(img, 0, 0); //img is the mission background image defined above
//draw all the flightlines onto bm1 --- dont need to do this but once for each time the mission is changed
//draw the flight lines ONCE on the bm1 background image and generate a new background image
for (int i=0; i<ps.msnSum[missionNumber].FlightLinesCurrentPlan.Count; i++)
{
endPoints ep = ps.msnSum[missionNumber].FlightLinesCurrentPlan[i];
//draw the flight lines
if (priorFlownFLs[i])
g.DrawLine(new Pen(Color.Green, 2), GeoToPix(ep.start), GeoToPix(ep.end));
else
g.DrawLine(new Pen(Color.Red, 2), GeoToPix(ep.start), GeoToPix(ep.end));
}
//show the zoomed-in map boundary if we are on the zoomed-out map
if (!UseZImapForPolygonMission)
{
//we are in the zoomed out map --- show the ZI map rectangle on the ZO map
PointD NWGeo = new PointD(ps.msnSum[missionNumber].MissionImage.westDeg, ps.msnSum[missionNumber].MissionImage.northDeg);
PointD SEGeo = new PointD(ps.msnSum[missionNumber].MissionImage.eastDeg, ps.msnSum[missionNumber].MissionImage.southDeg);
Point NWPix = GeoToPix(NWGeo);
Point SEPix = GeoToPix(SEGeo);
g.DrawRectangle(new Pen(Color.Gray, 1), NWPix.X, NWPix.Y, SEPix.X - NWPix.X, SEPix.Y - NWPix.Y);
}
//get the flight line spacing if there is more than one flightline
//this assumes a constant flight line spacing and that the flight lines are parallel
//TODO: the flight line spacing should be in the mission plan -- assume parallel flight lines in UTM
if (ps.msnSum[missionNumber].numberOfFlightlines > 1)
{
//start and end of the first flight line in lat/lon
double latS1 = ps.msnSum[missionNumber].FlightLinesCurrentPlan[0].start.Y;
double lonS1 = ps.msnSum[missionNumber].FlightLinesCurrentPlan[0].start.X;
double latE1 = ps.msnSum[missionNumber].FlightLinesCurrentPlan[0].end.Y;
double lonE1 = ps.msnSum[missionNumber].FlightLinesCurrentPlan[0].end.X;
//start point of the second flight line
double lat2 = ps.msnSum[missionNumber].FlightLinesCurrentPlan[1].start.Y;
double lon2 = ps.msnSum[missionNumber].FlightLinesCurrentPlan[1].start.X;
//convert lat/lon points to UTM so we can use vector arithmetic
PointD lineStart = new PointD(0, 0), lineEnd = new PointD(0, 0), point2Test = new PointD(0, 0);
utm.LLtoUTM(latS1 * Deg2Rad, lonS1 * Deg2Rad, ref lineStart.Y, ref lineStart.X, ref ps.UTMZone, true);
utm.LLtoUTM(latE1 * Deg2Rad, lonE1 * Deg2Rad, ref lineEnd.Y, ref lineEnd.X, ref ps.UTMZone, true);
utm.LLtoUTM(lat2 * Deg2Rad, lon2 * Deg2Rad, ref point2Test.Y, ref point2Test.X, ref ps.UTMZone, true);
//
//double L = Math.Sqrt( (UTMX2 - UTMX1) * (UTMX2 - UTMX1) + (UTMY2 - UTMY1) * (UTMY2 - UTMY1) );
//flightLineSpacing = L * Math.Cos( Math.Atan2( (UTMY2 - UTMY1) , (UTMX2 - UTMX1) ) );
PointD del = lineStart - point2Test;
//angleRad is measured from North positive clockwise
FLangleRad = Math.Atan2((lineEnd.X - lineStart.X), (lineEnd.Y - lineStart.Y));
//confusing: the below math assumes X is to the north and Y is to the east
double D = del.Y * Math.Cos(FLangleRad) + del.X * Math.Sin(FLangleRad);
double Vx = del.Y - D * Math.Cos(FLangleRad);
double Vy = del.X - D * Math.Sin(FLangleRad);
flightLineSpacing = Math.Sqrt(Vx * Vx + Vy * Vy);
//the flight line spacing is also read in from the input kml file ...
//double dd1 = ps.crossRangeSwathWidth - flightLineSpacing;
}
//NOTE: all bitmaps sized to the mapWidth & mapHeight -- stretched to fit screen in Paint
//bm2 will contain the base map plus the semiinfinite extended blue current flightline
bm2 = new Bitmap(img.Width, img.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
//bm3 will add to bm2 the crumb trail, current aircraft location, and photocenters
//these different bitmaps are created to reduce the overhead of preparing the refreshed bitmap
//note that preparing bm3 has the least overhead -- adding minimally to bm2
// img.Width, img.Height are 640 X 480 based on google map download limits
bm3 = new Bitmap(img.Width, img.Height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
//initially set the bm2 and bm3 images to the base map bm1
Graphics g2 = Graphics.FromImage(bm2);
g2.DrawImage(bm1, 0, 0);
Graphics g3 = Graphics.FromImage(bm3);
g3.DrawImage(bm1, 0, 0);
//bm3 is what is drawn at the Paint event at the refresh
g2.Dispose();
g3.Dispose();
setupSteeringBarGFraphic(bm1);
//computes total images from the flight plan
totalImagesThisMission = getTotalImagesThisMission();
}
private PointD PixToGeo(Point pt, ImageBounds _ib, double _lat2PixMultiplier, double _lon2PixMultiplier)
{
///////////////////////////////////////////////////////////////////////////////////
//maps are known to represent a rectangle of latitude and longitude
//displayed bmp images are 640*lat2PixMultiplier X 480*lat2PixMultiplier
//This procedure computes geodetic coordinates from pixel coordinates
//bmp image pixels are scaled to geodetic coordinates using the elements of ib
///////////////////////////////////////////////////////////////////////////////////
PointD Gpt = new PointD(0.0, 0.0); ;
Gpt.X = _ib.westDeg + (double)pt.X / (double)_lon2PixMultiplier;
Gpt.Y = _ib.northDeg + (double)pt.Y / (double)_lat2PixMultiplier;
return Gpt;
}
//constructor for MissionSelection Form for polygon mission
public MissionSelection(ProjectSummary _ps, 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;
ps = _ps;
navIF_ = navIF_In;
camera = cameraIn;
hardwareAttached = hardwareAttached_;
settings = _settings;
MissionDateString = _MissionDateString;
logFile = _logFile;
projectName = ps.ProjectName;
//there is a separate constructor for the linearFeature coverage type
coverageType = COVERAGE_TYPE.polygon;
//getPosVelTimer = new Stopwatch();
utm = new UTM2Geodetic();
/////////////////////////////////////////////////////////////////////////////////////
//set up the project polygon and the individual Mission polygons in pixel units
/////////////////////////////////////////////////////////////////////////////////////
//set of points in Pixels that we use to draw the project polygon onto the project map
//creats space for an array of Point structures tha will hold the project polygon
projectPolyPointsPix = new Point[ps.ProjectPolygon.Count];
//lat/lon image bounds from the mission plan
ib = ps.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
//create the project polygon in pixel units -- once
for (int i = 0; i < ps.ProjectPolygon.Count; i++)
projectPolyPointsPix[i] = GeoToPix(ps.ProjectPolygon[i]); //just uses a linear scaling
//create the mission polygons (one per mission) in pixel units
//used to form the clickable region on the project map
missionPolysInPix = new List<Point[]>();
for (int i = 0; i < ps.msnSum.Count; i++)
{
Point [] pts = new Point[ps.msnSum[i].missionGeodeticPolygon.Count];
for (int j = 0; j < ps.msnSum[i].missionGeodeticPolygon.Count; j++)
pts[j] = GeoToPix(ps.msnSum[i].missionGeodeticPolygon[j]);
missionPolysInPix.Add(pts);
}
}
//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
}
