/// <summary>
/// Distance in meters between two Earth Coordinates.
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public double DistanceFromStart(EarthCoordinates start)
{
// Distance between start and end (Latitude and Altitude) in meters
double x = Pythagorean(start._polarAxisMeters - this._polarAxisMeters, start._equatorialPlaneMeters - this._equatorialPlaneMeters);
// Distance between start and end Longitude in meters.
double y = 2.0 * Math.PI * ((((start._polarAxisMeters + this._polarAxisMeters) / 2.0)) / 360.0) * (start.Longitude - this.Longitude);
// Distance between start and end
return(Pythagorean(x, y));
}
/// <summary>
/// Returns three dimensional cartesian coordinates for this EarthCoordinate
/// with vertical orientation to the center of the earth
/// and horizontal orientation to the polar coordinates
/// <remarks>
/// X is meters -East/+West from the start
/// Y is meters -South/+North from the start
/// Z is meters -Below/+Above the start
/// </remarks>
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public Point3 OffsetFromStart(EarthCoordinates start)
{
// Distance between start and end Longitude in meters.
double y = 2.0 * Math.PI * ((((start._polarAxisMeters + this._polarAxisMeters) / 2.0)) / 360.0) * (start.Longitude - this.Longitude);
// Difference in altitude
double z = (this.AltitudeMeters - start.AltitudeMeters);
// To calculate the difference in Latitude,
// find the Longitude and Altitude midpoints
double midLongitude = (start.Longitude + this.Longitude) / 2.0;
double midAltitude = (start.AltitudeMeters + this.AltitudeMeters) / 2.0;
EarthCoordinates lat1;
if ((Math.Abs(midAltitude) > 20.0) || (Math.Abs(midLongitude) > 0.0001))
{
// The points are spread apart,
// so plot two new points at the starting and ending Latitude
// and using the midpoints of Longitude and Altitude.
lat1 = new EarthCoordinates(start.Latitude, midLongitude, midAltitude, start.DateTime, start.HorizontalDilutionOfPrecision, start.VerticalDilutionOfPrecision);
}
else
{
// The points are near the same Longitude and Altitude,
// so calculate Latitude using the start Longitude and Altitude.
lat1 = start;
midLongitude = start.Longitude;
midAltitude = start.AltitudeMeters;
}
EarthCoordinates lat2 = new EarthCoordinates(this.Latitude, midLongitude, midAltitude, this.DateTime, this.HorizontalDilutionOfPrecision, this.VerticalDilutionOfPrecision);
// Finally, measure the distance between these points.
double x = lat2.DistanceFromStart(lat1);
return(new Point3(x, y, z));
}
/// <summary>
/// Handle Gps GPGSA data packet
/// </summary>
/// <param name="fields"></param>
private void GsaHandler(string[] fields)
{
try
{
if (_state.GpGsa == null)
_state.GpGsa = new GpGsa();
_state.GpGsa.IsValid = false;
if (fields.Length != 18)
{
MessageHandler(string.Format(CultureInfo.InvariantCulture, "Invalid Number of parameters in GPGSA ({0}/{1})", fields.Length, 18));
return;
}
string Mode1 = fields[1]; // M-Manual A-Automatic switch between 2D and 3d
int Mode2 = int.Parse(fields[2], CultureInfo.InvariantCulture); // 1-Fix not available, 2-2D, 3-3D
string Status;
switch (Mode2)
{
case 2:
Status = "2D Fix";
break;
case 3:
Status = "3D Fix";
break;
default:
Status = "Fix not available";
break;
}
int satelliteId;
for(int i = 0; i < 12; i++)
{
if (int.TryParse(fields[i + 3], NumberStyles.Integer, CultureInfo.InvariantCulture, out satelliteId))
_state.GpGsa._satelliteUsed[i] = satelliteId;
else
_state.GpGsa._satelliteUsed[i] = 0;
}
double SphericalDilutionOfPrecision = -1.0d;
double.TryParse(fields[15], out SphericalDilutionOfPrecision);
double HorizontalDilutionOfPrecision = -1.0d;
double.TryParse(fields[16], out HorizontalDilutionOfPrecision);
double VerticalDilutionOfPrecision = -1.0d;
double.TryParse(fields[17], out VerticalDilutionOfPrecision);
_state.GpGsa.LastUpdate = DateTime.Now;
_state.GpGsa.Status = Status;
_state.GpGsa.AutoManual = Mode1;
_state.GpGsa.Mode = (GsaMode)Mode2;
_state.GpGsa.SphericalDilutionOfPrecision = SphericalDilutionOfPrecision;
_state.GpGsa.HorizontalDilutionOfPrecision = HorizontalDilutionOfPrecision;
_state.GpGsa.VerticalDilutionOfPrecision = VerticalDilutionOfPrecision;
_state.GpGsa.IsValid = (_state.GpGsa.Mode != GsaMode.NoFix);
if (_state.MicrosoftGpsConfig.CaptureHistory
&& _state.GpGsa != null && _state.GpGsa.IsValid // GSA: Precision data.
&& (_state.GpGll != null && _state.GpGll.IsValid || _state.GpRmc != null && _state.GpRmc.IsValid)
&& _state.GpGga != null && _state.GpGga.IsValid) // GGA: Altitude and backup position.
{
double Latitude, Longitude;
DateTime LastUpdate;
if (_state.GpGll != null && _state.GpGll.IsValid)
{
// GLL: Primary Position.
Latitude = _state.GpGll.Latitude;
Longitude = _state.GpGll.Longitude;
LastUpdate = _state.GpGll.LastUpdate;
}
else
{
// RMC: Backup course, speed, and position.
Latitude = _state.GpRmc.Latitude;
Longitude = _state.GpRmc.Longitude;
LastUpdate = _state.GpRmc.LastUpdate;
}
EarthCoordinates ec = new EarthCoordinates(Latitude, Longitude, _state.GpGga.AltitudeMeters, LastUpdate, _state.GpGsa.HorizontalDilutionOfPrecision, _state.GpGsa.VerticalDilutionOfPrecision);
_state.History.Add(ec);
if (_state.History.Count % 100 == 0)
SaveState(_state);
}
SendNotification(_subMgrPort, new GpGsaNotification(_state.GpGsa), Tag_GpGsa, _state.GpGsa.IsValid.ToString());
}
catch (Exception ex)
{
Debug.WriteLine("Exception in GsaHandler(): " + ex);
_gpsDataPort.Post(ex);
}
}
/// <summary>
/// Generate a bitmap showing an overhead view of the gps waypoints
/// </summary>
/// <param name="width"></param>
/// <returns></returns>
private Stream GenerateTop(int width)
{
MemoryStream memory = null;
int height = width * 3 / 4;
using (Bitmap bmp = new Bitmap(width, height))
{
using (Graphics g = Graphics.FromImage(bmp))
{
g.Clear(Color.LightGray);
g.DrawRectangle(Pens.White, new Rectangle(-1, -1, 3, 3));
if (_state.History == null || _state.History.Count < 1)
{
g.DrawString("No Data - check 'Capture History' box to see the track", new Font(FontFamily.GenericSansSerif, 16, GraphicsUnit.Pixel), Brushes.Red, new Point(10, 10));
}
else // plot a simple map from the Gps waypoints
{
double minLongitude = 9999.0, minLatitude = 9999.0, minAltitude = 9999999.0;
double maxLongitude = -9999.0, maxLatitude = -9999.0, maxAltitude = -9999999.0;
foreach (EarthCoordinates ec in _state.History)
{
minLongitude = Math.Min(minLongitude, ec.Longitude);
minLatitude = Math.Min(minLatitude, ec.Latitude);
minAltitude = Math.Min(minAltitude, ec.AltitudeMeters);
maxLongitude = Math.Max(maxLongitude, ec.Longitude);
maxLatitude = Math.Max(maxLatitude, ec.Latitude);
maxAltitude = Math.Max(maxAltitude, ec.AltitudeMeters);
}
if (minLongitude < 0)
{
double hold = minLongitude;
minLongitude = maxLongitude;
maxLongitude = hold;
}
EarthCoordinates start = new EarthCoordinates(minLatitude, minLongitude, minAltitude);
EarthCoordinates end = new EarthCoordinates(maxLatitude, maxLongitude, maxAltitude);
Point3 box = end.OffsetFromStart(start);
double scale = Math.Max(box.X / (double)width, box.Y / (double)height);
Point lastPoint = Point.Empty;
Point currentPoint;
foreach (EarthCoordinates ec in _state.History)
{
box = ec.OffsetFromStart(start);
currentPoint = new Point(width - (int)(box.Y / scale) - 10, height - (int)(box.X / scale) - 10);
// Draw the point.
int dop = (int)Math.Truncate(ec.HorizontalDilutionOfPrecision);
Rectangle r = new Rectangle(currentPoint.X - dop, currentPoint.Y - dop, dop * 2, dop * 2);
g.FillEllipse(Brushes.Azure, r);
g.DrawEllipse(Pens.Yellow, r);
if (lastPoint == Point.Empty)
g.DrawString("Start", new Font(FontFamily.GenericSansSerif, 16, GraphicsUnit.Pixel), Brushes.Green, currentPoint);
else
{
g.DrawLine(Pens.Red, lastPoint, currentPoint);
}
lastPoint = currentPoint;
}
}
}
memory = new MemoryStream();
bmp.Save(memory, ImageFormat.Jpeg);
memory.Position = 0;
}
return memory;
}
/// <summary>
/// Returns three dimensional cartesian coordinates for this EarthCoordinate
/// with vertical orientation to the center of the earth
/// and horizontal orientation to the polar coordinates
/// <remarks>
/// X is meters -East/+West from the start
/// Y is meters -South/+North from the start
/// Z is meters -Below/+Above the start
/// </remarks>
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public Point3 OffsetFromStart(EarthCoordinates start)
{
// Distance between start and end Longitude in meters.
double y = 2.0 * Math.PI * ((((start._polarAxisMeters + this._polarAxisMeters) / 2.0)) / 360.0) * (start.Longitude - this.Longitude);
// Difference in altitude
double z = (this.AltitudeMeters - start.AltitudeMeters);
// To calculate the difference in Latitude,
// find the Longitude and Altitude midpoints
double midLongitude = (start.Longitude + this.Longitude) / 2.0;
double midAltitude = (start.AltitudeMeters + this.AltitudeMeters) / 2.0;
EarthCoordinates lat1;
if ((Math.Abs(midAltitude) > 20.0) || (Math.Abs(midLongitude) > 0.0001))
{
// The points are spread apart,
// so plot two new points at the starting and ending Latitude
// and using the midpoints of Longitude and Altitude.
lat1 = new EarthCoordinates(start.Latitude, midLongitude, midAltitude, start.DateTime, start.HorizontalDilutionOfPrecision, start.VerticalDilutionOfPrecision);
}
else
{
// The points are near the same Longitude and Altitude,
// so calculate Latitude using the start Longitude and Altitude.
lat1 = start;
midLongitude = start.Longitude;
midAltitude = start.AltitudeMeters;
}
EarthCoordinates lat2 = new EarthCoordinates(this.Latitude, midLongitude, midAltitude, this.DateTime, this.HorizontalDilutionOfPrecision, this.VerticalDilutionOfPrecision);
// Finally, measure the distance between these points.
double x = lat2.DistanceFromStart(lat1);
return new Point3(x, y, z);
}
/// <summary>
/// Distance in meters between two Earth Coordinates.
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public double DistanceFromStart(EarthCoordinates start)
{
// Distance between start and end (Latitude and Altitude) in meters
double x = Pythagorean(start._polarAxisMeters - this._polarAxisMeters, start._equatorialPlaneMeters - this._equatorialPlaneMeters);
// Distance between start and end Longitude in meters.
double y = 2.0 * Math.PI * ((((start._polarAxisMeters + this._polarAxisMeters) / 2.0)) / 360.0) * (start.Longitude - this.Longitude);
// Distance between start and end
return Pythagorean(x,y);
}
