public void StructureArgumentGetsSameResultCalcCoords()
{
GreatCircle.CalcCoords(21.0, -120.12, 88.0, 1002, out double resLat, out double resLon);
var newPos = GreatCircle.CalcCoords(new GeographicPosition(21, -120.12, 250), Angle.FromDegrees(88.0), Length.FromMeters(1002));
Assert.Equal(resLat, newPos.Latitude);
Assert.Equal(resLon, newPos.Longitude);
}
private void MainSimulator()
{
while (!_terminate)
{
var newData = _activeData.Clone();
GeographicPosition newPosition = GreatCircle.CalcCoords(newData.Position, _activeData.Course,
_activeData.Speed * UpdateRate);
newData.Position = newPosition;
_activeData = newData;
SendNewData();
Thread.Sleep(UpdateRate);
}
}
public void TestCartesianToGC()
{
// start position of test
double latStart = 47.0;
double lonStart = 11.0;
// 1 arcsec is about 30m => move in 0.3m steps along all 8 axis away from the start position
double deltaInc = 0.01 / 3600.0;
GeographicPosition pStart = new GeographicPosition(latStart, lonStart, 0);
double distance = 0;
double direction = 0;
Length gcDist = Length.Zero;
Angle gcDir;
// iterate over the 8 axis (45° increments)
for (int signIdx = 0; signIdx < 8; signIdx++)
{
double dblXSign = 1.0;
if (signIdx >= 4 && signIdx <= 6)
{
dblXSign = -1.0;
}
else if (signIdx == 3 || signIdx == 7)
{
dblXSign = 0.0;
}
double dblYSign = 1.0;
if (signIdx >= 2 && signIdx <= 4)
{
dblYSign = -1.0;
}
else if (signIdx == 1 || signIdx == 5)
{
dblYSign = 0.0;
}
// iterate in 0.3m steps (up to about 150m distance)
for (int idx = 1; idx <= 400; idx++)
{
double delta = idx * deltaInc;
GeographicPosition pEnd = new GeographicPosition(latStart + dblYSign * delta, lonStart + dblXSign * delta, 0);
InternalDistDir(pStart, pEnd, ref distance, ref direction);
GreatCircle.DistAndDir(pStart, pEnd, out gcDist, out gcDir);
// compare the two calculation methods
Assert.True(Math.Abs(distance - gcDist.Meters) < 1.0, "position accuracy less than 1m");
Assert.True(GreatCircle.AngleDifferenceSignedDegrees(direction, gcDir.Degrees) < 1.0, "direction accuracy less than 1 deg");
// calculate the endpoint with the previously calculated offsets using great circle
double dblEndLat = 0;
double dblEndLon = 0;
GreatCircle.CalcCoords(pStart.Latitude, pStart.Longitude, gcDir.Degrees, gcDist.Meters, out dblEndLat, out dblEndLon);
Assert.True(Math.Abs(dblEndLat - pEnd.Latitude) < 1.0, "GC latitude accuracy less than 1m");
Assert.True(GreatCircle.AngleDifferenceSignedDegrees(dblEndLon, pEnd.Longitude) < 1.0, "GC longitude accuracy less than 1m");
// calculate the endpoint with the previously calculated offsets using the cartesic routine
GeographicPosition pCalcEnd = InternalExtrapolatePosition(pStart, distance, direction);
double dblDeltaX = Math.Abs(pCalcEnd.Longitude - pEnd.Longitude) * GreatCircle.MetersPerDegreeLongitude;
double dblDeltaY = Math.Abs(pCalcEnd.Latitude - pEnd.Latitude) * GreatCircle.MetersPerDegreeeLatitude;
Assert.True(dblDeltaY < 1.0, "XY latitude accuracy less than 1m");
Assert.True(dblDeltaX < 1.0, "XY longitude accuracy less than 1m");
}
}
}
private static bool TestDistAndDir(double startlat, double startlon, double endlat, double endlon)
{
double dist = 0;
double dir = 0;
GreatCircle.DistAndDir(startlat, startlon, endlat, endlon, out dist, out dir);
Angle dirNormalized = Angle.FromDegrees(dir).Normalize(true);
if (endlon >= 180)
{
endlon -= 360;
}
if (endlon < -180)
{
endlon += 360;
}
Assert.True(dist < 30000000, "Distance more than 1/2 times around the earth");
Assert.True(dirNormalized.Degrees >= 0 && dirNormalized.Degrees < 360, "Angle out of bounds");
double reslat = 0;
double reslon = 0;
GreatCircle.CalcCoords(startlat, startlon, dir, dist, out reslat, out reslon);
Angle lon = Angle.FromDegrees(reslon);
lon = lon.Normalize(false);
Assert.True(reslat >= -90 && reslat <= 90, "Invalid latitude");
Assert.True(lon.Degrees > -180, $"Invalid longitude A {reslon}");
Assert.True(lon.Degrees <= 180, $"Invalid longitude B {reslon}");
double accuracy = 1E-7;
if (dist > 100000)
{
accuracy = 1E-6;
}
if (dist > 1000000)
{
accuracy = 1E-6;
}
if (dist > 10000000)
{
accuracy = 1E-5;
}
if (Math.Abs(endlat - reslat) > accuracy)
{
Console.WriteLine("Error testing: " + startlat + "/" + startlon +
" to " + endlat + "/" + endlon + " Distance: " + dist +
" Difference in lat: " + (endlat - reslat));
return(false);
}
if (GreatCircle.AngleDifferenceSignedDegrees(endlon, lon.Degrees) > accuracy)
{
Console.WriteLine("Error testing: " + startlat + "/" + startlon +
" to " + endlat + "/" + endlon + " Distance: " + dist +
" Difference in lon: " + (endlon - reslon));
return(false);
}
return(true);
}
/// <summary>
/// Get the current position from the latest message containing any of the relevant data parts.
/// If <paramref name="extrapolate"></paramref> is true, the speed and direction are used to extrapolate the position (many older
/// GNSS receivers only deliver the position at 1Hz or less)
/// </summary>
/// <param name="position">Current position</param>
/// <param name="extrapolate">True to extrapolate the current position using speed and track</param>
/// <param name="track">Track (course over ground)</param>
/// <param name="sog">Speed over ground</param>
/// <param name="heading">Vessel Heading</param>
/// <returns>True if a valid position is returned</returns>
public bool TryGetCurrentPosition(out GeographicPosition?position, bool extrapolate, out Angle track, out Speed sog, out Angle?heading)
{
// Try to get any of the position messages
var gll = (PositionFastUpdate?)GetLastSentence(PositionFastUpdate.Id);
var gga = (GlobalPositioningSystemFixData?)GetLastSentence(GlobalPositioningSystemFixData.Id);
var rmc = (RecommendedMinimumNavigationInformation?)GetLastSentence(RecommendedMinimumNavigationInformation.Id);
var vtg = (TrackMadeGood?)GetLastSentence(TrackMadeGood.Id);
var hdt = (HeadingTrue?)GetLastSentence(HeadingTrue.Id);
TimeSpan age;
List <(GeographicPosition, TimeSpan)> orderablePositions = new List <(GeographicPosition, TimeSpan)>();
if (gll != null && gll.Position != null)
{
orderablePositions.Add((gll.Position, gll.Age));
}
if (gga != null && gga.Valid)
{
orderablePositions.Add((gga.Position, gga.Age));
}
if (rmc != null && rmc.Valid)
{
orderablePositions.Add((rmc.Position, rmc.Age));
}
if (orderablePositions.Count == 0)
{
// No valid positions received
position = null;
track = Angle.Zero;
sog = Speed.Zero;
heading = null;
return(false);
}
(position, age) = orderablePositions.OrderBy(x => x.Item2).Select(x => (x.Item1, x.Item2)).First();
if (gga != null && gga.EllipsoidAltitude.HasValue)
{
// If we had seen a gga message, use its height, regardless of which other message provided the position
position = new GeographicPosition(position.Latitude, position.Longitude, gga.EllipsoidAltitude.Value);
}
if (rmc != null)
{
sog = rmc.SpeedOverGround;
track = rmc.TrackMadeGoodInDegreesTrue;
}
else if (vtg != null)
{
sog = vtg.Speed;
track = vtg.CourseOverGroundTrue;
}
else
{
sog = Speed.Zero;
track = Angle.Zero;
heading = null;
return(false);
}
if (hdt != null)
{
heading = hdt.Angle;
}
else
{
heading = null;
}
if (extrapolate)
{
position = GreatCircle.CalcCoords(position, track, sog * age);
}
return(true);
}