public void writePositionRec(PosVel platFormPosVel)
{
//test prevents early data from getting recorded.
if (Math.Abs(platFormPosVel.GeodeticPos.X) > 0.0001 && Math.Abs(platFormPosVel.GeodeticPos.Y) > 0.0001 )
FlyKmlFile.WriteLine(String.Format("{0:####.000000},{1:###.000000},{2:#####.00}",
platFormPosVel.GeodeticPos.X, platFormPosVel.GeodeticPos.Y, platFormPosVel.altitude ) );
}
//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);
}
}
public void writePhotoCenterRec(int missionNumber, int currentFlightLine, int offset, int currentPhotocenter, PosVel platFormPosVel)
{
String photoCenterName = "";
if (missionNumber >= 0)
photoCenterName = missionNumber.ToString("D3") + "_" + currentFlightLine.ToString("D2") + "_" + (offset + currentPhotocenter).ToString("D3");
else
photoCenterName = currentFlightLine.ToString("D2") + "_" + (offset + currentPhotocenter).ToString("D3");
FlyKmlFile.WriteLine(String.Format("<Placemark> <name>" + photoCenterName +
" </name> <styleUrl>#whiteDot</styleUrl> <Point> <coordinates>{0:####.000000},{1:###.000000},{2:#####.00}</coordinates> </Point> </Placemark>",
platFormPosVel.GeodeticPos.X, platFormPosVel.GeodeticPos.Y, platFormPosVel.altitude));
}
private PosVel getGPSState()
{
//placeholder for the realtime when we have the hardware connected
PosVel pv = new PosVel();
return pv;
}
//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 realTimeAction()
{
/////////////////////////////////////////////////////////////////
//this controls all the real-time action
//mostly generic for both polygon and linear feature coverage
/////////////////////////////////////////////////////////////////
if (hardwareAttached)
{
try
{
//get the posvel message from the GPS receiver attached to the mbed
//posvel is interpolated from the most recent prior 1PPS message
posVel_ = navIF_.getPosVel();
}
catch (Exception ex)
{
logFile.WriteLine("Exception in navIF_.getPosVel(): " + ex.Message);
}
//logFile.WriteLine(" posvel numSats = " + posVel_.numSV.ToString() );
}
//write the position kml file
if (missionTimerTicks % kmlPositionThinningFactor == 0)
{
try
{
kmlPositionWriter.writePositionRec(platFormPosVel);
}
catch
{
logFile.WriteLine("Exception in kmlPositionWriter.writePositionRec ");
}
}
//count the number of cycles through the real-time action
missionTimerTicks++;
//
if (!hardwareAttached || posVel_.solutionComputed)
{
labelPilotMessage.Visible = false;
//compute platform/FL geometry and triggerRequested event
try
{
//platform geometry computed and camera triggers are commanded here
if(coverageType == COVERAGE_TYPE.polygon) polygonMissionGeomtry();
}
catch
{
logFile.WriteLine(" exception in prepMissionDisplay() ");
return;
}
//prepare the various portions of the bitmap graphics display
try
{
if (coverageType == COVERAGE_TYPE.linearFeature)
{
prepLinearFeatureBitmapForPaint();
}
else if (coverageType == COVERAGE_TYPE.polygon)
{
try
{
//test to see if we have moved off the ZI map or have moved back on to the ZI map
bool ZImapUsedForLastTime = UseZImapForPolygonMission;
if (platformWithinMissionMap()) UseZImapForPolygonMission = true;
else UseZImapForPolygonMission = false;
if (ZImapUsedForLastTime != UseZImapForPolygonMission)
{
//we have changed the mission map
firstGPSPositionAvailable = false; //causes the crumb trail to be reset to a constant location
currentFlightLineChanged = true; //causes the semi-infinite current-line to be redrawn
preparePolygonMissionDisplayfixedBackground(); //static map portion of the display
}
//prepare the portions of the mission display that vary rapidly
prepPolygonBitmapForPaint();
}
catch
{
MessageBox.Show(" excepton in prepPolygonBitmapForPaint");
}
}
}
catch
{
logFile.WriteLine(" exception in prepBitmapForPaint() higher up ");
return;
}
//repaint the screen ...
this.Refresh(); //calls the Paint event
//doevents in the btnOK real-time loop
//try
//{
// Application.DoEvents(); //acts on any pressed buttons
//}
//catch
//{
// MessageBox.Show(" cant do events");
//}
//prepare the info for the steering bar
try
{
if (coverageType == COVERAGE_TYPE.polygon)
{
//sign flip based on 5/14/2013 road test
signedError = -Convert.ToInt32(FLGeometry.PerpendicularDistanceToFL * Math.Sign(FLGeometry.FightLineTravelDirection));
iXTR = Convert.ToInt32(FLGeometry.headingRelativeToFL);
iTGO = Convert.ToInt32(TGO);
labelALT.Text = "ALT= " + (ps.msnSum[missionNumber].flightAltMSLft - platFormPosVel.altitude / 0.3048).ToString("F0");
}
else if (coverageType == COVERAGE_TYPE.linearFeature)
{
if (FPGeometry.velMag > 0.10)
signedError = Convert.ToInt32(FPGeometry.LOSRate * (LFSum.plannedRabbitDistanceAhead / FPGeometry.velMag) );
iXTR = Convert.ToInt32(FPGeometry.headingToPath * Rad2Deg);
iTGO = Convert.ToInt32(TGO);
//get the alongTrack altitude command from the input datafile
labelALT.Text = "ALT= " + (FPGeometry.commandedAltitude - platFormPosVel.altitude / 0.3048).ToString("F0");
}
labelTGO.Text = "TGO= " + iTGO.ToString("D3");
labelXTR.Text = "XTR= " + iXTR.ToString("D2");
labelVEL.Text = "VEL= " + (speed*100.0/51.4).ToString("F0");
}
catch
{
signedError = 0;
iTGO = 0;
iXTR = 0;
logFile.WriteLine("Exception in preparing steering bar information");
}
////////////////////////////////////////////////////////////////////////
//show information to the pilot --- why is this done every time ???
////////////////////////////////////////////////////////////////////////
//Originally had the information display disappear after a set time
labelElapsedTime.Visible = true;
labelSatsLocked.Visible = true;
labelNumImages.Visible = true;
labelElapsedTime.Text = "Elapsed Time= " + (elapsedTime.ElapsedMilliseconds / 1000.0).ToString("F0");
PowerStatus power = SystemInformation.PowerStatus;
if (navIF_ != null)
{
labelSatsLocked.Text = "Sats= " + posVel_.numSV.ToString() + " Batt: " + (power.BatteryLifePercent * 100).ToString("F0") + "%";
}
else labelSatsLocked.Text = "Sats= 0" + " Batt: " + (power.BatteryLifePercent * 100).ToString("F0") + "%";
labelNumImages.Text = "Images= " + totalImagesCommanded.ToString() +
"/" + totalImagesTriggerReceived.ToString() + "/" + totalImagesLoggedByCamera.ToString() + "/" + totalImagesThisMission.ToString();
}
else //this cause the mission activities to wait for sats to be locked and the GPS time to be converged
{
if (hardwareAttached)
{
labelPilotMessage.Visible = true;
labelPilotMessage.Text = "waiting sats ... " + posVel_.numSV + " locked";
}
labelElapsedTime.Text = "Elapsed Time= " + (elapsedTime.ElapsedMilliseconds / 1000.0).ToString("F0");
labelSatsLocked.Visible = false;
labelNumImages.Visible = false;
}
}
//constructor for the polygon coverage form
public Mission(String _FlightPlanFolder, String _MissionDataFolder, String MissionDateStringNameIn, int _missionNumber,
ProjectSummary _ps, bool[] _priorFlownFLs, LogFile _logFile,
NavInterfaceMBed navIF_In, SDKHandler cameraIn, bool _simulatedMission, bool _hardwareAttached, StreamWriter _reflyFile, Image _projectImage)
{
InitializeComponent();
coverageType = COVERAGE_TYPE.polygon; //we use a separate constructor for the linearFeature mission
this.FormBorderStyle = System.Windows.Forms.FormBorderStyle.None;
//set the mission image -- mapWidth & mapHeight = 640 X 480 based on the Google Earth map download limits
this.Width = (int)(mapScaleFactor * mapWidth); //mapscaleFactor scales the map to fit a screen size of 1024 X 768
this.Height = (int)(mapScaleFactor * mapHeight);
//this.Width = 640; //pixel height of the form
//this.Height = 480; //pixel width of the form
posVel_ = new PosVel();
//retrieve local variables from the arguments
missionNumber = _missionNumber;
ps = _ps;
MissionDataFolder = _MissionDataFolder;
FlightPlanFolder = _FlightPlanFolder;
priorFlownFLs = _priorFlownFLs; //this contains the completed flight lines so they arent reflown
reflyFile = _reflyFile; //write the completed flight line indices to this file
projectImage = _projectImage; //contains the complete project image in case the display runs off the smaller maps
navIF_ = navIF_In;
camera = cameraIn;
logFile = _logFile;
MissionDateStringName = MissionDateStringNameIn;
//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;
timeFromTrigger = new Stopwatch();
elapsedTime = new Stopwatch();
getPosVelTimer = new Stopwatch();
timePastEndfFlightline = new Stopwatch();
////ib is used internally to the GeoToPix procedures
////we will need to reset the ib & PixMultipliers if we have to use the Project map (plane exits mission map)
//ib = ps.msnSum[missionNumber].MissionImage; //placeholder for the Mission 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
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];
//shows the "waiting sats" message
labelPilotMessage.Visible = false;
//get the max flight line length for this mission -- should come from the missionPlan
//used to establish a region before and after the flightlines where flightlinecapture is allowed
for (int i = 0; i < ps.msnSum[missionNumber].FlightLinesCurrentPlan.Count; i++)
{
if (ps.msnSum[missionNumber].FlightLinesCurrentPlan[i].FLLengthMeters > maxFlightLineLength)
maxFlightLineLength = ps.msnSum[missionNumber].FlightLinesCurrentPlan[i].FLLengthMeters;
}
}
public int getPlatformToFLGeometry(PosVel platform)
{
///////////////////////////////////////////////////////////////////////////////////////////////////////
//must be computed for each HUD command cycle ....
//do these computations in UTM rectilinear coordinates
//this path layout does not change over the course of a Project
//however, the Waldo_FCS will allow flight lines to be flown from the start-to-end or end-to-start
//the platform dynamics will be used to define the flightlline flight direction
///////////////////////////////////////////////////////////////////////////////////////////////////////
velMag = Math.Sqrt(platform.velN * platform.velN + platform.velE * platform.velE);
heading = Math.Atan2(platform.velE, platform.velN);
distanceFromStartAlongPath = 0.0;
int currentVertex = 0;
LOSRate = polyMath.LOSRatetoPointAlongPath(
platform.UTMPos,
new PointD(platform.velE, platform.velN),
LFSum.plannedRabbitDistanceAhead / 2.0, ref distanceFromStartAlongPath, ref velocityAlongPath, ref headingToPath, ref currentVertex);
//compute the commanded altitude -- just use the value at the last vertex
//TODO: interpolate this
commandedAltitude =
LFSum.paths[pathNumber].commandedAltAlongPath[currentVertex];
Console.WriteLine("currentVertex = " + currentVertex.ToString() + " commandedAlt=" + commandedAltitude.ToString() );
return currentVertex;
}
public PosVel getPosVel()
{
/////////////////////////////////////////////////////////////////////////
//return the best estimate of the pos vel at this time
//this is called from the Waldo_FCS foreground loop to get
//the latest (current) values of the aircraft position and velocity
//position and velocity are valid at the 1PPS mark and must be interpolated
//to the current time from there
////////////////////////////////////////////////////////////////////////
PosVel posvel = new PosVel(); //form this current value of posvel here and return.
int numSV = 0, solSV = 0; bool solutionComputed = false;
try
{
comStatusMessageLock.TryEnterReadLock(5);
{
numSV = commStatusMessage.numSatsTracked;
solSV = commStatusMessage.numSatsInSolution;
if (commStatusMessage.solutionStatus == "SOL_COMPUTED") solutionComputed = true;
}
comStatusMessageLock.ExitReadLock();
}
catch (Exception ex)
{
logFile.WriteLine("exception in comStatusMessageLock: " + ex.Message);
}
posvel.numSV = numSV;
posvel.solSV = solSV;
if (solutionComputed) posvel.solutionComputed = true;
else
{
//no need to compute remainder of posvel if we do not have a converged position solution from GPS
posvel.solutionComputed = false;
return posvel;
}
//keep track of time since the last GPS 1PPS when we had a good position & velocity message
//timeFrom1PPS stopwatch restarted at each 1PPS
//"oneAsRequired" also is incremented at 1PPS and set to zero when a new pos/el dataset is received
//oneAsRequired accounts for fact that: last valid nav occurring slightly AFTER the 1PPS
double timeFromLastValidNav = timeFrom1PPS.ElapsedMilliseconds / 1000.0 + oneAsRequired;
//get a local value of the latitude and longitude at PPS
double latitudePPS = 0.0, longitudePPS = 0.0, altitudePPS = 0.0, velE = 0.0, velN = 0.0, velD = 0.0;
double UTMEastingPPS=0.0, UTMNorthingPPS = 0.0;
//we will block here for max 5msecs if the posvelAt1PPS is being filled in the mbed comm thread
try
{
posvelLock.TryEnterReadLock(5); //try to get the posveldata from the mbed comm thread
{
latitudePPS = posvelAt1PPS.GeodeticPos.Y;
longitudePPS = posvelAt1PPS.GeodeticPos.X;
altitudePPS = posvelAt1PPS.altitude;
velN = posvelAt1PPS.velN;
velE = posvelAt1PPS.velE;
velD = posvelAt1PPS.velD;
UTMEastingPPS = posvelAt1PPS.UTMPos.X;
UTMNorthingPPS = posvelAt1PPS.UTMPos.Y;
}
posvelLock.ExitReadLock();
}
catch (Exception ex)
{
logFile.WriteLine("Exception in posvelLock: " + ex.Message);
}
try
{
//extrapolate forward from last valid PPS posvel dataset from the GPS receiver
posvel.UTMPos.X = UTMEastingPPS + timeFromLastValidNav * velE;
posvel.UTMPos.Y = UTMNorthingPPS + timeFromLastValidNav * velN;
posvel.altitude = altitudePPS - timeFromLastValidNav * velD;
//transfer back to GeoDetic after the extrapolation
utm.UTMtoLL(posvel.UTMPos.Y, posvel.UTMPos.X, UTMZone, ref posvel.GeodeticPos.Y, ref posvel.GeodeticPos.X);
//logFile.WriteLine("lat lon + " + posvel.GeodeticPos.Y.ToString() + " " + posvel.GeodeticPos.X.ToString() );
posvel.velN = velN;
posvel.velE = velE;
posvel.velD = velD;
}
catch (Exception ex)
{
logFile.WriteLine("Exception in posvelLock: " + ex.Message);
}
return posvel;
}
public NavInterfaceMBed(LogFile _logFile, SettingsManager Settings)
{
logFile = _logFile; //file where we write the mbed navigation data and status message
//Thread reader/writer lock to prevent clobbering the posvel variable while it is being accessed
posvelLock = new ReaderWriterLockSlim();
comStatusMessageLock = new ReaderWriterLockSlim();
triggerTimeReceievdFromMbed = false; //set to true when we receive a 1PPS status message
//reset to false in the calling program when the status message is processed
//used to store the CRC results fpr the GPS messages received at the PC
GPS_PC_CRC = new GPSMessageCRC_atPC();
trigger = new Trigger();
computeCRC = new NovatelCRC(); //class to compute the Novatel CRC value (see their manual)
comStats = new CommStats(); //accumulated comm stats
commStatusMessage = new CommStatusMessage(); //per sec comm status message
navIFMutex_ = new Mutex(); //not sure we need this
////////////////////////////////////////////////////////////////////////////////////////////
//wait here in a loop unitl we have attached the USB cable to access the mbed device
////////////////////////////////////////////////////////////////////////////////////////////
initializeMbedSerialPort();
if (!serialInit_)
{
logFile.WriteLine("mbed serial port not found");
throw new Exception("no serial port found");
}
/////////////////////////////////////////////////////////////////////////////////////
//At this stage we have found the mbed port and have successfully opened the port
/////////////////////////////////////////////////////////////////////////////////////
logFile.WriteLine("Successfully opened the mbed device");
utm = new UTM2Geodetic();
posvelAt1PPS = new PosVel();
timeFrom1PPS = new Stopwatch();
//set up the communications interface thread
Thread mbedCommunication = new Thread(mbedCommunicationWorker);
mbedCommunication.IsBackground = false;
//start the communication and begin retrieving mbed messages
mbedCommunication.Start();
logFile.WriteLine("Completed starting the mbed communication thread");
if (mbedCommunication.IsAlive)
{
logFile.WriteLine("mbedCommunication thread is operating ");
}
else
{
logFile.WriteLine("mbed communication htread os not operating ");
MessageBox.Show("mbed comminication thread did not start ");
}
}
private void PPSTimer_Tick(object sender, EventArgs e)
{
elapsedSeconds++;
if (hardwareAttached) // need the GPS data to show the actual position on the missionSelection Form
{
//DANGEROUS: if this is called while the Mission Form is active -- big trouble!!
//the PPSTimer is stopped at the time the Mission form is displayed.
//request and read the POSVEL message from mbed -- see the procedure below
//getPosVel();
//superimpose the aircraft location on the background image
//draw the real-time location of the platform -- the POSVEL message was requested i nthe PPSTimer;
//The mbed datalink was established at the very beginning od the ProjectSelection form
//GeoToPix uses the scalong from the Project Map
posVel_ = navIF_.getPosVel();
lblGPSStatus.Text = elapsedSeconds.ToString() + " GPS SATS: Tracked= " + posVel_.numSV.ToString() + " In Solution= " + posVel_.solSV.ToString();
logFile.WriteLine(posVel_.GeodeticPos.X.ToString() + " " + posVel_.GeodeticPos.Y.ToString());
Point pt = new Point();
if (posVel_.solutionComputed)
{
pt = GeoToPix(new PointD(posVel_.GeodeticPos.X, posVel_.GeodeticPos.Y));
}
//create a graphics object from the project map with polygon overlays.
Graphics g = Graphics.FromImage(bmWithPos);
//draw circle centered over the geodetic aircraft location
g.DrawEllipse(new Pen(Color.Black, 2), pt.X - 4, pt.Y - 4, 8, 8);
g.Dispose();
//repaint the MissionSelection map
this.Refresh();
}
}
//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
}
public void getPlatformToFLGeometry(PosVel platform)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//must be computed for each steering bar command cycle ....
//do these computations in UTM rectilinear coordinates
//each flight line has a geodetic start and end as defined back in the original mission planning
//this initial layout does not change over the course of a Project
//however, the geoscanne will allow flight lines to be flown from the start-to-end or end-to-start
//the platform dynamics will be used to define the flightlline flight direction
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//the GPS messages from the receiver might be bad
//bool dataIsAcceptable = false; //use this to vet the quality of the data from this routine
//compute dot product of the vector from FLstart to the platform position, divided by the line length
//this will be negative if the point is outside start and > 1.0 if the point is beyond end
//becomes the projection of the platform position vector (relative to the Flightline start) onto the flightline
percentFromStart = ( (platform.UTMPos.Y - FLstartUTM.Y) * (FLendUTM.Y - FLstartUTM.Y) +
(platform.UTMPos.X - FLstartUTM.X) * (FLendUTM.X - FLstartUTM.X) ) / FLlengthSq;
//compute the components of the vector: "intersection on the line minus the platform position"
//point on the flightline that is closest to the platform position
double POCAY = FLstartUTM.Y + percentFromStart * (FLendUTM.Y - FLstartUTM.Y);
double POCAX = FLstartUTM.X + percentFromStart * (FLendUTM.X - FLstartUTM.X);
//error vector from the platform to the closest point on the flightline -- magnitude is the error
double dY = POCAY - platform.UTMPos.Y;
double dX = POCAX - platform.UTMPos.X;
//cross product of vector from start-to-Platform and start-to-end.
//sign of this gives the sign of the error relative to the flight line;
double crp = ( (platform.UTMPos.X-FLstartUTM.X)*(FLendUTM.Y-FLstartUTM.Y) - (platform.UTMPos.Y-FLstartUTM.Y)*(FLendUTM.X-FLstartUTM.X) ) / FLlengthSq;
//distance from the flght line start to the intersection
distanceFromStartAlongFL = percentFromStart * FLlengthMeters;
//magnitude of the line from platform to the flightline.
//sign is positive if the platform is to the right of the flightline (start-to-end)
PerpendicularDistanceToFL = Math.Sqrt(dY * dY + dX * dX) * Math.Sign(crp);
//prevent divide-by-zero
if (Math.Abs(platform.velE) < 0.01 && Math.Abs(platform.velN) < 0.01)
{
platform.velE = 0.01;
platform.velN = 0.01;
}
velMag = Math.Sqrt(platform.velE * platform.velE + platform.velN * platform.velN);
if (velMag < 0.10) velMag = 0.10;
PointD velocityUnit = new PointD(platform.velE / velMag, platform.velN / velMag);
//velocity along the flight line
velocityAlongFlightLine = start2EndFlightLineUnit.X * platform.velE + start2EndFlightLineUnit.Y * platform.velN;
//Form velocity-crossed-UnitVector vector to get the sine if the angle
double vcu = velocityUnit.X * start2EndFlightLineUnit.Y - velocityUnit.Y * start2EndFlightLineUnit.X;
//form the dot product to get the cosine of the angle
double vdu = velocityUnit.X * start2EndFlightLineUnit.X + velocityUnit.Y * start2EndFlightLineUnit.Y;
//if vdu > zero we are heading from the start end to the end end.
//else we are headed from the end to the start
//the sign of this result will be "+" if the veocity vector is aimed to the right of the start-to-end unit vector
headingRelativeToFL = Math.Asin(vcu) * Rad2Deg; //this will be limited +/- 90 deg
FightLineTravelDirection = Math.Sign(vdu); //+1 if along start-to-end and -1 if along end-to-start direction
//////////////////////////////////////////////
//do this only for the simulation
//////////////////////////////////////////////
//put a target point 500m ahead of the closect point on the flight line
//we will use a pursuit guidance law to chase this point
PointD pointOnFlightlineAheadOfPlatform = new PointD(0.0,0.0);
pointOnFlightlineAheadOfPlatform.X = POCAX + FightLineTravelDirection*start2EndFlightLineUnit.X * 500.0;
pointOnFlightlineAheadOfPlatform.Y = POCAY + FightLineTravelDirection*start2EndFlightLineUnit.Y * 500.0;
//compute the Line-Of-Sight rate between the platform and the target point
//the gammaDot will be commanded to -3*LOSRate
double delX = pointOnFlightlineAheadOfPlatform.X - platform.UTMPos.X;
double delY = pointOnFlightlineAheadOfPlatform.Y - platform.UTMPos.Y;
double LOSAngle = Math.Atan2( delX,delY);
LOSRate = -Math.Cos(LOSAngle) * Math.Cos(LOSAngle) * (platform.velE / delY - delX / (delY * delY) * platform.velN);
}
