Ejemplo n.º 1
0
		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;
		}
Ejemplo n.º 2
0
        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;
		}
Ejemplo n.º 9
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;
		}