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
}
