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);
}
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;
}
}
// <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;
}
}
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;
}
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);
}
/// <summary>
/// Based on index value which is the position in the nmea sentence, extract the value to the create type
/// and set the correct member value
/// </summary>
/// <param name="idx">index</param>
/// <param name="val">raw value</param>
protected override void SetIndexValue(int idx, ReadOnlySpan <char> val)
{
switch (idx)
{
case 0:
UTCTime = GetTimeSpanFromHHMMSS(val);
break;
case 1:
Latitude = GetDecimalDegrees(val);
break;
case 2:
NorthSouth = GetNorthSouth(val);
break;
case 3:
Longitude = GetDecimalDegrees(val);
break;
case 4:
EastWest = GetEastWest(val);
break;
case 5:
FixQuality = GetGPSFix(val);
break;
case 6:
SatsInView = GetInteger(val);
break;
case 7:
HDOP = GetDecimal(val);
break;
case 8:
Altitude = GetDecimal(val);
break;
case 9:
AltitudeUnits = GetUnits(val);
break;
case 10:
GeoidalSeperation = GetDecimal(val);
break;
case 11:
GeoidSeperationUnits = GetUnits(val);
break;
case 12:
AgeOfDGPSData = GetTimeSpanFromHHMMSS(val);
break;
case 13:
DGPStationID = GetString(val);
break;
}
}
private void OnTripStatus(object parm)
{
Debug.WriteLine(string.Format("{0} {1}", DateTime.Now.ToString("HH:mm:ss.fff"), "OnTripStatus"));
RoutePatternConfig configRP = null;
ServiceAlertConfig configSA = null;
VehicleConfig configV = null;
Interlocked.Exchange <RoutePatternConfig>(ref configRP, m_configRP);
Interlocked.Exchange <ServiceAlertConfig>(ref configSA, m_configSA);
Interlocked.Exchange <VehicleConfig>(ref configV, m_configV);
bool bContinue = true, bDispose = false;
Trip trip = null;
TripStatus ts = (TripStatus)parm;
Timeout timeoutFix = new Timeout(TimeSpan.FromMinutes(2)), timeoutOnRoute = new Timeout(TimeSpan.FromSeconds(500));
do
{
if (ts != null)
{
if (ts.NoTrip)
{
trip = null;
}
else if (!ts.OffRoute)
{
timeoutOnRoute.Reset();
if (trip == null || ts.RouteTag != trip.RouteTag || ts.RP < trip.RP || ts.TripNo != trip.TripNo || ts.ServiceStart != trip.ServiceStart)
{
trip = Trip.Create(this, ts.RouteTag, ts.RP, ts.TripNo, ts.ServiceStart, m_configRP, m_configSA, m_configV, m_fFerry);
}
else
{
trip.ProgressRP(ts.RP);
}
EnableSignalPriority(ts.SignalPriority);
}
else if (trip != null)
{
trip.OffRoute = true;
EnableSignalPriority(false);
}
}
if (trip != null)
{
GPSFix fix = m_Fix;
bool bEndTrip = false;
if (timeoutOnRoute.HasExpired)
{
bEndTrip = true;
}
else if (fix == null || !UTM.IsValidLL4Zone(configV.UTMZoneNo, configV.UTMZoneLetter, fix.Lat, fix.Long))
{
bEndTrip = timeoutFix.HasExpired;
}
else
{
timeoutFix.Reset();
//if( m_fFerry || !trip.OffRoute )
// bEndTrip = !trip.UpdatePosition( UTM.LL2UTM( configV.UTMZoneNo, fix.Lat, fix.Long ), fix.Speed, fix.SpeedWeighted );
}
if (bEndTrip)
{
trip = null;
}
else
{
Thread.Sleep(999);
}
}
lock ( m_qTS )
{
if (m_qTS.Count == 0)
{
ts = null;
}
else
{
ts = m_qTS.Dequeue();
if (ts == null)
{
bDispose = true;
}
}
if (bDispose || (trip == null && ts == null))
{
bContinue = m_bTSThreadPoolActive = false;
}
}
}while(bContinue);
if (!bDispose)
{
QueueRequest(new RequestEventArgs(RequestType.DisplayNextStop, null));
QueueRequest(new RequestEventArgs(RequestType.DisplayNextStopExtra, null));
if (configV.HeadSignDefault != null)
{
QueueRequest(new RequestEventArgs(RequestType.HeadSign, configV.HeadSignDefault));
}
EnableSignalPriority(false);
}
}
