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