//=========================================================================
//=========================================================================
/// <summary>
/// Raises our gps data received event in a thread safe way
/// </summary>
public void RaiseGpsDataReceivedEventArgs()
{
//---- raise the event
if (this.GpsDataRecieved != null)
{
this.GpsDataRecieved(this, new GpsDataReceivedEventArgs(GpsReading.Parse(this._nmeaSentenceBuffer)));
}
}
public static void Main()
{
var oldReading = new GpsReading(null, DateTime.Now.ToUniversalTime(), 46.03695, -118.67283, 90f, 5f, 0);
Console.WriteLine("Before: " + oldReading);
var reading = GetNewLatLong(oldReading, 45d, 1000d);
Console.WriteLine("After: " + reading);
}
//=========================================================================
//=========================================================================
#region -= event handlers =-
//=========================================================================
protected void _robot_GpsDataRecieved(object sender, GpsRobot.GpsDataReceivedEventArgs e)
{
//---- save our gps reading
this._gpsReading = e.GpsReading;
//---- use lamda to define a delegate to update the UI
Action updateUI = () => { this.UpdateUI(); };
//---- invoke the updateui on the main thread
Dispatcher.BeginInvoke(updateUI, new object[] { });
}
/// <summary>
/// Given a current GPS reading, a heading, and a distance traveled, compute
/// a new GPS reading.
/// </summary>
/// <remarks>
/// Taken from http://www.movable-type.co.uk/scripts/latlong.html
/// </remarks>
/// <param name="oldReading">Old GPS reading</param>
/// <param name="headingInDegrees">Heading in degrees - -360 - 360</param>
/// <param name="distanceInMeters">Distance traveled in meters</param>
private static GpsReading GetNewLatLong(GpsReading oldReading, double headingInDegrees, double distanceInMeters)
{
const double EarthRadiusMeters = 6371000d;
double angularDist = distanceInMeters / EarthRadiusMeters;
double newHeadingDeg = oldReading.Heading + headingInDegrees;
if (newHeadingDeg >= 360d)
{
newHeadingDeg -= 360d;
}
if (newHeadingDeg < 0d)
{
newHeadingDeg += 360d;
}
//* Convert to radians
double newHeading = newHeadingDeg * Math.PI / 180d;
double lat = oldReading.Latitude * Math.PI / 180d;
double lon = oldReading.Longitude * Math.PI / 180d;
double lat2 =
Math.Asin(Math.Sin(lat) * Math.Cos(angularDist)
+ Math.Cos(lat) * Math.Sin(angularDist) * Math.Cos(newHeading));
double lon2 = lon
+ Math.Atan2(Math.Sin(newHeading) * Math.Sin(angularDist) * Math.Cos(lat),
Math.Cos(angularDist) - Math.Sin(lat) * Math.Sin(lat2));
// normalise to -180...+180
lon2 = (lon2 + 3 * Math.PI) % (2 * Math.PI) - Math.PI;
//* Convert back to degrees
lat2 *= 180 / Math.PI;
lon2 *= 180 / Math.PI;
GpsReading reading = new GpsReading(null, DateTime.Now.ToUniversalTime(),
lat2, lon2, (float)newHeadingDeg, (float)oldReading.Speed, oldReading.Odometer);
return(reading);
}
protected virtual GpsInfo ParseGpsInfo(string[] commands)
{
var reading = GpsReading.Parse(commands);
if (reading.FixedGpsData == null)
{
return(null);
}
var gpsInfo = new GpsInfo();
if (reading.FixedGpsData.Elevation != null)
{
gpsInfo.AltitudeMeters = Convert.ToDouble(reading.FixedGpsData.Elevation.Value);
}
if (reading.FixedGpsData.Position != null)
{
gpsInfo.LatitudeDegrees = Convert.ToDouble(reading.FixedGpsData.Position.Latitude.Degrees);
gpsInfo.LongitudeDegrees = Convert.ToDouble(reading.FixedGpsData.Position.Longitude.Degrees);
}
//采用gps本身提供的日期，20160509
gpsInfo.UtcTime = reading.Summary.UtcDateTime;
//gpsInfo.UtcTime = reading.FixedGpsData.UtcTime;
gpsInfo.LocalTime = gpsInfo.UtcTime.ToLocalTime();
gpsInfo.FixedSatelliteCount = reading.FixedGpsData.NumberOfSatelitesInUse;
gpsInfo.TrackedSatelliteCount = gpsInfo.FixedSatelliteCount;
gpsInfo.FixQuality = reading.FixedGpsData.Quality;
if (reading.GroundVector != null && reading.GroundVector.GroundSpeedInKmh != 0)
{
gpsInfo.SpeedInKmh = reading.GroundVector.GroundSpeedInKmh;
}
if (reading.Summary != null && reading.Summary.GroundSpeed != 0)
{
var summary = reading.Summary;
gpsInfo.SpeedInKmh = Convert.ToDouble(summary.GroundSpeed);
gpsInfo.AngleDegrees = Convert.ToDouble(summary.Heading);
}
if (reading.Heading != null)
{
//20160111，使用高精度版本
tempVersion = true;
gpsInfo.AngleDegrees = reading.Heading.BearingDegrees;
//高精度和普通科三gps角度会差180度，如：高精度3°，普通184°
//gpsInfo.AngleDegrees = gpsInfo.AngleDegrees + 180;
//if (gpsInfo.AngleDegrees > 360)
// gpsInfo.AngleDegrees = gpsInfo.AngleDegrees - 360;
gpsInfo.FixedSatelliteCount = reading.Heading.SatellitesInSolution;
gpsInfo.TrackedSatelliteCount = reading.Heading.SatellitesInTrack;
//记录上一次角度
lastAngleDegrees = gpsInfo.AngleDegrees;
}
if (tempVersion)
{
//20160111，使用高精度版本,(有时Heading为空的情况)
gpsInfo.HighPrecisionVersion = true;
if (reading.Heading == null)
{
gpsInfo.AngleDegrees = lastAngleDegrees;
}
}
return(gpsInfo);
}
public GpsDataReceivedEventArgs(GpsReading gpsReading)
: base()
{
this._gpsReading = gpsReading;
}
public async Task EntersGeofence()
{
this.gpsDelegate.CurrentStates.Should().BeEmpty();
await this.gpsDelegate.OnReading(GpsReading.Create(1, 1));
}
public GpsDataReceivedEventArgs(GpsReading gpsReading)
: base()
{
this._gpsReading = gpsReading;
}
//=========================================================================
protected void _robot_GpsDataRecieved(object sender, GpsRobot.GpsDataReceivedEventArgs e)
{
//---- save our gps reading
this._gpsReading = e.GpsReading;
//---- use lamda to define a delegate to update the UI
Action updateUI = () => { this.UpdateUI(); };
//---- invoke the updateui on the main thread
Dispatcher.BeginInvoke(updateUI, new object[] { });
}
