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