public GPSFix(GPSFix fix)
{
posChanged = fix.posChanged;
ecef_x = fix.ecef_x;
ecef_y = fix.ecef_y;
ecef_z = fix.ecef_z;
m_LatitudeRadians = fix.m_LatitudeRadians;
m_LongitudeRadians = fix.m_LongitudeRadians;
m_AltitudeAboveMSL = fix.m_AltitudeAboveMSL;
m_MSLAltitudeAboveWGS84 = fix.m_MSLAltitudeAboveWGS84;
m_HeadingRadians = fix.m_HeadingRadians;
m_HDOP = fix.m_HDOP;
m_VDOP = fix.m_VDOP;
m_EPH = fix.m_EPH;
m_EPV = fix.m_EPV;
FixType = fix.FixType;
IsDifferential = fix.IsDifferential;
UTCFixTime = fix.UTCFixTime;
SpeedEast = fix.SpeedEast;
SpeedNorth = fix.SpeedNorth;
SpeedUp = fix.SpeedUp;
}
public GPSFix(GPSFix fix)
{
posChanged = fix.posChanged;
ecef_x = fix.ecef_x;
ecef_y = fix.ecef_y;
ecef_z = fix.ecef_z;
m_LatitudeRadians = fix.m_LatitudeRadians;
m_LongitudeRadians = fix.m_LongitudeRadians;
m_AltitudeAboveMSL = fix.m_AltitudeAboveMSL;
m_MSLAltitudeAboveWGS84 = fix.m_MSLAltitudeAboveWGS84;
m_HeadingRadians = fix.m_HeadingRadians;
m_HDOP = fix.m_HDOP;
m_VDOP = fix.m_VDOP;
m_EPH = fix.m_EPH;
m_EPV = fix.m_EPV;
FixType = fix.FixType;
IsDifferential = fix.IsDifferential;
UTCFixTime = fix.UTCFixTime;
SpeedEast = fix.SpeedEast;
SpeedNorth = fix.SpeedNorth;
SpeedUp = fix.SpeedUp;
}
// <summary>
// Calculates the speed and heading of the specified current fix based on the distance and bearing to the
// specified origin point, and the time taken to travel from the origin point to here. Elevation is not
// taken into account in the geodesic distance calculation (it is assumed that elevation does not
// significantly affect geodesic distance calculations, which may or may not be true), but elevation
// differens are taken into account when calculating vertical speed. This probably needs looking at
// in the future.
// </summary>
private void SetSpeedAndHeading(GPSFix current, GPSFix origin, out double distance)
{
// Check that we are dealing with valid fixes, and that the time difference between this fix and
// the origin fix is positive.
if ((origin.FixType == GPSFixTypes.Invalid) || (current.FixType == GPSFixTypes.Invalid))
throw new Exception("Invalid current or origin fix for speed calculation.");
if (current.UTCFixTime.CompareTo(origin.UTCFixTime) <= 0)
throw new Exception("Negative or zero time difference from origin to current fix for speed calculation.");
// Calculate the distance and bearing from the current point to the origin point.
double bearing;
GeoidUtils.GetDistanceAndBearing(current.LatitudeRadians, current.LongitudeRadians,
origin.LatitudeRadians, origin.LongitudeRadians, out distance, out bearing);
// Rotate the bearing 180 degress to get the heading.
current.HeadingRadians = bearing >= 0 ? bearing - Math.PI : bearing + Math.PI;
// Calculate the north and east components of the speed based on the distance, time difference
// and heading.
double seconds = current.UTCFixTime.Subtract(origin.UTCFixTime).TotalSeconds;
double groundSpeed = distance / seconds;
current.SpeedNorth = groundSpeed * Math.Cos(current.HeadingRadians);
current.SpeedEast = groundSpeed * Math.Sin(current.HeadingRadians);
// Calculate the vertical speed based on the elevation and time differences, but only if
// both fixes are 3D.
if ((current.FixType == GPSFixTypes.Fix3D) && (origin.FixType == GPSFixTypes.Fix3D))
current.SpeedUp = (current.AltitudeAboveMSL - origin.AltitudeAboveMSL) / seconds;
else
current.SpeedUp = 0;
}
private void PublishFix(GPSFix fix, NMEASentenceType fixSource)
{
if (fixSource == fixTriggerSentence)
{
if (haveGGA && haveRMC)
{
// Output a combined fix if the partial fix is valid.
if ((partialFix != null) && (partialFix.UTCFixTime == fix.UTCFixTime) &&
(fixSource != partialFixSentenceType))
{
GPSFix fixRMC = fixSource == NMEASentenceType.RMC ? fix : partialFix;
GPSFix fixGGA = fixSource == NMEASentenceType.GGA ? fix : partialFix;
m_MostRecentGPSFix = fixGGA;
m_MostRecentGPSFix.HeadingRadians = fixRMC.HeadingRadians;
m_MostRecentGPSFix.SpeedEast = fixRMC.SpeedEast;
m_MostRecentGPSFix.SpeedNorth = fixRMC.SpeedNorth;
// Still need to calculate vertical speed. Sigh.
CalculateSpeedAndHeading(m_MostRecentGPSFix, true);
}
partialFix = null;
partialFixSentenceType = NMEASentenceType.Unknown;
}
else if (fixSource == NMEASentenceType.GGA)
{
// Don't have speed information, so calculate it.
m_MostRecentGPSFix = fix;
CalculateSpeedAndHeading(m_MostRecentGPSFix, false);
}
else if (fixSource == NMEASentenceType.RMC)
{
// Publish the fix as is.
m_MostRecentGPSFix = fix;
}
}
else
{
// Save the fix until we hit the trigger sentence.
partialFix = fix;
partialFixSentenceType = fixSource;
}
}
private void CalculateSpeedAndHeading(GPSFix fix, bool verticalOnly)
{
// Only calculate speed between valid fixes, and only save valid fixes for next time. This
// allows the occassinal invalid fix in between valid fixes without disrupting the speed
// calculations for the valid fixes (the last valid fix will hang around for a while).
if (fix.FixType != GPSFixTypes.Invalid)
{
double saveAltitude = fix.AltitudeAboveMSL;
if (savedFix != null)
{
double seconds = fix.UTCFixTime.Subtract(savedFix.UTCFixTime).TotalSeconds;
if ((seconds > 0) && (seconds <= savedFixValiditySeconds))
{
// Only calculate speed and heading if the total position change,
// which may occur across multiple fixes, is greater than a
// predefined threshold. We obtain the total position change by
// not updating the position in the saved fix until the threshold
// has been reached, at which point we adjust the saved fix to
// the new position. Since we report speeds of 0 until the
// threshold has been reached, we always calculate the speed from
// the saved position from the time of the last fix, not the
// original time of the saved fix, so that a speed * time
// calculation will return the correct distance for this position
// change.
if (verticalOnly)
{
// Calculate the vertical speed based on the elevation and time differences, but only if
// both fixes are 3D.
if ((fix.FixType == GPSFixTypes.Fix3D) && (savedFix.FixType == GPSFixTypes.Fix3D))
{
if (Math.Abs(fix.AltitudeAboveMSL - savedFix.AltitudeAboveMSL) <=
movementThreshold)
{
// Change in altitude is insignificant, so set vertical speed to zero and ensure that
// the altitude of the saved fix is preserved below.
fix.SpeedUp = 0;
saveAltitude = savedFix.AltitudeAboveMSL;
}
else
{
fix.SpeedUp = (fix.AltitudeAboveMSL - savedFix.AltitudeAboveMSL) / seconds;
}
}
else
{
fix.SpeedUp = 0;
}
// Always update the saved fix.
savedFix = null;
}
else
{
double distance;
SetSpeedAndHeading(fix, savedFix, out distance);
if (Math.Abs(fix.AltitudeAboveMSL - savedFix.AltitudeAboveMSL) <=
movementThreshold)
{
// Change in altitude is insignificant, so set vertical speed to zero and ensure that
// the altitude of the saved fix is preserved below.
fix.SpeedUp = 0;
saveAltitude = savedFix.AltitudeAboveMSL;
}
if (distance > movementThreshold)
{
// We have moved enough for a reliable speed calculation, so save the current fix
// for next time.
savedFix = null;
}
else
{
// Haven't moved enough to exclude the effects of position error, so set all
// speeds to zero and update the time of the saved fix so that we can test against
// the same saved position next time around.
fix.SpeedEast = 0;
fix.SpeedNorth = 0;
fix.HeadingRadians = savedFix.HeadingRadians;
savedFix.UTCFixTime = fix.UTCFixTime;
}
}
}
else
{
// Too long in between fixes, so invalidate the saved fix.
savedFix = null;
}
}
// Only save the current fix if we haven't updated the existing saved fix.
if (savedFix == null)
savedFix = new GPSFix(fix);
savedFix.AltitudeAboveMSL = saveAltitude;
}
}
private void ProcessRMC(string[] parts)
{
// Make sure we have the right number of parts.
if (parts.Length < 6)
{
CallOnLogEvent("Invalid RMC sentence.");
return;
}
// Create a new fix object.
GPSFix fix = new GPSFix();
// Assume a 2D fix type unless the fix is invalid.
fix.FixType = parts[2].Equals("A") ? GPSFixTypes.Fix2D : GPSFixTypes.Invalid;
// Extract the UTC time, if present.
fix.UTCFixTime = ParseNMEATimestamp(parts[1]);
// Extract the latitude, converting the minutes portion to a fraction of a degree.
if (parts[3].Equals(String.Empty))
{
// Invalidate the fix.
fix.FixType = GPSFixTypes.Invalid;
}
else
{
double temp = 0;
if (! ParseDouble(parts[3], ref temp))
throw new Exception("Invalid latitude: " + parts[3]);
temp /= 100;
double lat = (int)temp;
lat = (lat + (temp - lat) / 0.6) * (parts[4].Equals("N") ? 1 : -1);
fix.LatitudeDegrees = lat;
}
// Extract the longitude.
if (parts[5].Equals(String.Empty))
{
// Invalidate the fix.
fix.FixType = GPSFixTypes.Invalid;
}
else
{
double temp = 0;
if (! ParseDouble(parts[5], ref temp))
throw new Exception("Invalid longitude: " + parts[5]);
temp /= 100;
double lon = (int)temp;
lon = (lon + (temp - lon) / 0.6) * (parts[6].Equals("E") ? 1 : -1);
fix.LongitudeDegrees = lon;
}
// Extract the speed and heading. If parsing fails, the values will be left as 0.
double speed = 0, track = 0;
ParseDouble(parts[7], ref speed);
ParseDouble(parts[8], ref track);
speed *= 1852.0 / 3600;
fix.HeadingDegrees = track;
fix.SpeedNorth = speed * Math.Cos(fix.HeadingRadians);
fix.SpeedEast = speed * Math.Sin(fix.HeadingRadians);
// Fake an EPE.
fix.EPH = 5;
// Publish the fix.
PublishFix(fix, NMEASentenceType.RMC);
}
private void ProcessGGA(string[] parts)
{
// Make sure we have the right number of parts.
if (parts.Length != 15)
{
CallOnLogEvent("Invalid GGA sentence.");
return;
}
// Create a new fix object.
GPSFix fix = new GPSFix();
// Determine the fix type from this sentence. We can't reliably uses the
// presence of an altitude to distinguish between 2D and 3D fixes,
// so assume that all fixes are 2D in the absence of GSA information.
fix.FixType = (parts[6].Equals("1") || parts[6].Equals("2")) ?
GPSFixTypes.Fix2D : GPSFixTypes.Invalid;
// Extract the HDOP, leaving it set to zero if this fails.
double hdop = 0;
if (! parts[8].Equals(String.Empty))
ParseDouble(parts[8], ref hdop);
fix.HDOP = hdop;
// Determine if we have a differential fix.
fix.IsDifferential = parts[6].Equals("2");
// Extract the UTC time, if present.
fix.UTCFixTime = ParseNMEATimestamp(parts[1]);
// Extract the latitude, converting the minutes portion to a fraction of a degree.
if (parts[2].Equals(String.Empty))
{
// Invalidate the fix.
fix.FixType = GPSFixTypes.Invalid;
}
else
{
double temp = 0;
if (! ParseDouble(parts[2], ref temp))
throw new Exception("Invalid latitude: " + parts[2]);
temp /= 100;
double lat = (int)temp;
lat = (lat + (temp - lat) / 0.6) * (parts[3].Equals("N") ? 1 : -1);
fix.LatitudeDegrees = lat;
}
// Extract the longitude.
if (parts[4].Equals(String.Empty))
{
// Invalidate the fix.
fix.FixType = GPSFixTypes.Invalid;
}
else
{
double temp = 0;
if (! ParseDouble(parts[4], ref temp))
throw new Exception("Invalid longitude: " + parts[4]);
temp /= 100;
double lon = (int)temp;
lon = (lon + (temp - lon) / 0.6) * (parts[5].Equals("E") ? 1 : -1);
fix.LongitudeDegrees = lon;
}
// Check if we have an altitude value.
if (! parts[9].Equals(String.Empty))
{
// Parse the altitude.
double alt = 0;
if (! ParseDouble(parts[9], ref alt))
throw new Exception("Invalid altitude: " + parts[9]);
switch (parts[10])
{
case "M":
break;
case "f":
alt *= 0.3048;
break;
default:
throw new Exception("Invalid unit of measurement: " + parts[10]);
}
fix.AltitudeAboveMSL = alt;
// Extract the MSL height above WGS84. If not present, implies previous altitude
// is relative to WGS84 and not MSL (for Garmin receivers, anyway).
double mslAltitude = 0;
if (parts[11].Equals(String.Empty))
{
// TODO: calculate correct MSLAltitudeAboveWGS84
mslAltitude = 0;
// Convert from Altitude above WGS84 to Altitude above MSL;
fix.AltitudeAboveMSL -= mslAltitude;
}
else
{
if (! ParseDouble(parts[11], ref mslAltitude))
throw new Exception("Invalid MSL altitude: " + parts[11]);
switch (parts[12])
{
case "M":
break;
case "f":
mslAltitude *= 0.3048;
break;
default:
throw new Exception("Invalid unit of measurement: " + parts[12]);
}
}
fix.MSLAltitudeAboveWGS84 = mslAltitude;
}
// Update the fix based on the contents of the last GSA sentence, if any.
if ((gsaData != null) && (DateTime.Now.Subtract(gsaData.timeReceived).TotalSeconds < gsaDataValiditySeconds))
{
// Bump the fix type up to 3D if the GSA fix was 3D.
if ((fix.FixType == GPSFixTypes.Fix2D) && (gsaData.fixType == GPSFixTypes.Fix3D))
fix.FixType = GPSFixTypes.Fix3D;
// Add the VDOP information.
fix.VDOP = gsaData.VDOP;
}
// Publish the fix.
PublishFix(fix, NMEASentenceType.GGA);
}
public void Reset()
{
readBuffer = "";
partialFix = null;
partialFixSentenceType = NMEASentenceType.Unknown;
haveGGA = false;
haveRMC = false;
lastGGATimestamp = new DateTime(0);
lastRMCTimestamp = new DateTime(0);
fixTriggerSentence = NMEASentenceType.Unknown;
savedFix = null;
m_MostRecentGPSFix = null;
gsaData = null;
partialSatelliteVehicles = null;
partialSatelliteVehicleIndex = 0;
m_MostRecentSatelliteVehicles = null;
lastGSVMessageNumber = 0;
}
public IGPSFix GetGPSFix()
{
GPSFix fix = new GPSFix();
GPSPVTDataType pvt;
ushort err = GPSGetPVT(out pvt);
if (err == 0)
{
// Convert the fix time.
DateTime utc = DateTime.UtcNow;
fix.UTCFixTime = new DateTime(utc.Year, utc.Month, utc.Day,
(int)(pvt.time.seconds / 3600),
(int)(pvt.time.seconds % 3600 / 60),
(int)(pvt.time.seconds % 3600 % 60),
(int)((double)pvt.time.fracSeconds / 4294967296 * 1000));
// Determine the fix type.
switch (pvt.status.fix)
{
case gpsFixInvalid:
case gpsFixUnusable:
fix.FixType = GPSFixTypes.Invalid;
break;
case gpsFix2D:
case gpsFix2DDiff:
fix.FixType = GPSFixTypes.Fix2D;
fix.IsDifferential = pvt.status.fix == gpsFix2DDiff;
break;
case gpsFix3D:
case gpsFix3DDiff:
fix.FixType = GPSFixTypes.Fix3D;
fix.IsDifferential = pvt.status.fix == gpsFix3DDiff;
break;
}
// Populate the rest of the fields.
fix.LatitudeRadians = pvt.position.lat * Math.PI / 2147483648;
fix.LongitudeRadians = pvt.position.lon * Math.PI / 2147483648;
fix.AltitudeAboveMSL = pvt.position.altMSL;
fix.MSLAltitudeAboveWGS84 = pvt.position.altWGS84 - pvt.position.altMSL;
fix.EPH = pvt.status.eph;
fix.EPV = pvt.status.epv;
fix.SpeedEast = pvt.velocity.east;
fix.SpeedNorth = pvt.velocity.north;
fix.SpeedUp = pvt.velocity.up;
fix.HeadingRadians = Math.Atan2(fix.SpeedEast, fix.SpeedNorth);
}
else
{
fix.FixType = GPSFixTypes.Invalid;
}
return fix;
}
