public void RecordsDestination()
{
// Arrange
var orig = _landsEnd;
var dest = _JohnoGroats;
// Act
var route = new GreatCircle(orig, dest);
// Assert
Assert.That(
dest.Latitude,
Is.EqualTo(_JohnoGroats.Latitude).Within(0.0001),
"Recorded destination's latitude did not match supplied values");
Assert.That(
dest.Longitude,
Is.EqualTo(_JohnoGroats.Longitude).Within(0.0001),
"Recorded destination's longitude did not match supplied values");
Assert.That(
dest.Altitude,
Is.EqualTo(_JohnoGroats.Altitude).Within(0.0001),
"Recorded destination's altitude did not match supplied values");
Assert.That(
dest.WorldRadius,
Is.EqualTo(_JohnoGroats.WorldRadius).Within(0.0001),
"Recorded destination's radius did not match supplied values");
}
public void AviaticToRadiansTest()
{
Assert.Equal(0, GreatCircle.AviaticToRadians(90));
Assert.Equal(Math.PI / 2.0, GreatCircle.AviaticToRadians(0));
Assert.Equal(Math.PI, GreatCircle.AviaticToRadians(270));
Assert.Equal(Math.PI, GreatCircle.AviaticToRadians(-90));
Assert.Equal(Math.PI / 2.0 * 3, GreatCircle.AviaticToRadians(180));
}
public void RadiansToAviaticTest()
{
Assert.Equal(90, GreatCircle.RadiansToAviatic(0));
Assert.Equal(0, GreatCircle.RadiansToAviatic(Math.PI / 2.0));
Assert.Equal(270, GreatCircle.RadiansToAviatic(Math.PI));
Assert.Equal(270, GreatCircle.RadiansToAviatic(-Math.PI));
Assert.Equal(180, GreatCircle.RadiansToAviatic(-Math.PI / 2.0));
}
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);
}
public void CrossTrackError1()
{
GeographicPosition start = new GeographicPosition(0, 0, 0);
GeographicPosition end = new GeographicPosition(1, 0, 0);
GreatCircle.CrossTrackError(start, end, start, out var crossTrackError, out Length distance);
Assert.Equal(59.7053933897411, distance.NauticalMiles, 2); // 1 degree latitude = ~60 nautical miles
Assert.Equal(0, crossTrackError.Meters); // start on track -> deviation is 0
}
public void StructureArgumentGetsSameResultDistAndDir()
{
GreatCircle.DistAndDir(-35.84, 94,
35.87, -85, out var dist1, out var dir1);
GreatCircle.DistAndDir(new GeographicPosition(-35.84, 94, 100), new GeographicPosition(35.87, -85, 200), out var dist2, out var dir2);
Assert.Equal(dist1, dist2.Meters);
Assert.Equal(dir1 + 360.0, dir2.Degrees);
}
public void CrossTrackError2()
{
GeographicPosition start = new GeographicPosition(1, 0, 0);
GeographicPosition end = new GeographicPosition(2, 0, 0);
GeographicPosition current = new GeographicPosition(1.75, 0, 0);
GreatCircle.CrossTrackError(start, end, current, out var crossTrackError, out Length distance);
Assert.Equal(14.9264938243846, distance.NauticalMiles, 4); // 1 degree latitude = 60 nautical miles. A quarter of it is remaining
Assert.Equal(0, crossTrackError.Meters); // On track -> deviation is 0
}
public void CrossTrackError4()
{
GeographicPosition start = new GeographicPosition(1, 0, 0);
GeographicPosition end = new GeographicPosition(2, 0, 0);
GeographicPosition current = new GeographicPosition(1.75, -1.0 / 60.0, 0);
GreatCircle.CrossTrackError(start, end, current, out var crossTrackError, out Length distance);
Assert.Equal(14.9264938243846, distance.NauticalMiles, 4); // 1 degree latitude = 60 nautical miles. A third of it is remaining
Assert.Equal(-1.00, crossTrackError.NauticalMiles, 2); // One nautical mile off, to the left
}
private static void InternalDistDir(GeographicPosition startPosition,
GeographicPosition endPosition,
ref double distance, ref double direction)
{
double deltaLat = endPosition.Latitude - startPosition.Latitude;
double deltaLon = endPosition.Longitude - startPosition.Longitude;
double deltaX = deltaLon * GreatCircle.MetersPerDegreeLongitude * Math.Abs(Math.Cos(startPosition.Latitude * Math.PI / 180.0));
double deltaY = deltaLat * GreatCircle.MetersPerDegreeeLatitude;
distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
direction = GreatCircle.RadiansToAviatic(Math.Atan2(deltaY, deltaX));
}
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 CalculatesNewCoordinates_NoDeltaAlt()
{
// Arrange
var orig = _landsEnd;
var route = new GreatCircle(_landsEnd, _JohnoGroats);
// Act
var dest = orig.Travel(route);
// Assert
Assert.That(dest.Latitude, Is.EqualTo(_JohnoGroats.Latitude).Within(0.0001), "Miscalculation of final latitude");
Assert.That(dest.Longitude, Is.EqualTo(_JohnoGroats.Longitude).Within(0.0001), "Miscalculation of final longitude");
Assert.That(dest.Altitude, Is.EqualTo(_JohnoGroats.Altitude).Within(0.0001), "Miscalculation of final altitude");
}
public void CalculateVelocityTowardsTarget1()
{
GeographicPosition end = new GeographicPosition(2, 0, 0);
GeographicPosition current = new GeographicPosition(1, 0, 0);
Speed result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.Zero);
Assert.Equal(Speed.FromMetersPerSecond(10), result); // directly towards target
result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(180));
Assert.Equal(-Speed.FromMetersPerSecond(10), result); // directly away from target
result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(270));
Assert.Equal(Speed.Zero.MetersPerSecond, result.MetersPerSecond, 5); // perpendicular to target
}
public void RouteCalculationForward()
{
GeographicPosition start = new GeographicPosition(0, 1, 0);
var points = GreatCircle.CalculateRoute(start, 180, 1000, 100);
Assert.Equal(11, points.Count);
double previousLat = 1.1;
foreach (var pt in points)
{
Assert.True(previousLat > pt.Latitude);
previousLat = pt.Latitude;
Assert.Equal(1, pt.Longitude);
}
}
public void CalculateVelocityTowardsTarget2()
{
GeographicPosition end = new GeographicPosition(2, 0, 0);
GeographicPosition current = new GeographicPosition(1.75, 0.25, 0);
Speed result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.Zero);
// Will miss the target like this
Assert.True(Math.Abs((Speed.FromMetersPerSecond(10) * Math.Cos(45.0 / 180 * Math.PI)).MetersPerSecond - result.MetersPerSecond) < 0.05, result.ToString());
result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(45));
Assert.True(Math.Abs(result.MetersPerSecond) <= 0.04, result.ToString()); // about perpendicular to target
result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(280));
Assert.True(Math.Abs(8.21 - result.MetersPerSecond) < 0.01, result.ToString()); // perpendicular to target
}
public void RouteCalculationInverse()
{
GeographicPosition start = new GeographicPosition(0, 1, 0);
GeographicPosition end = new GeographicPosition(0, 2, 0);
var points = GreatCircle.CalculateRoute(start, end, 1000);
Assert.Equal(112, points.Count);
double previousLon = 0.9;
foreach (var pt in points)
{
Assert.True(previousLon < pt.Longitude);
previousLon = pt.Longitude;
Assert.Equal(0, pt.Latitude);
}
}
/// <summary>
/// Calculates distances and directions between the waypoints
/// </summary>
private void CalculateMetaData()
{
for (var index = 0; index < _routePoints.Count; index++)
{
var pt = _routePoints[index];
// First, make sure the total length and other properties are copied correctly everywhere
pt.RouteName = Name;
pt.IndexInRoute = index;
pt.TotalPointsInRoute = _routePoints.Count;
if (index != _routePoints.Count - 1)
{
GreatCircle.DistAndDir(pt.Position, _routePoints[index + 1].Position, out var distance, out var direction);
pt.DistanceToNextWaypoint = distance;
pt.BearingToNextWaypoint = direction;
}
}
}
public void CalculatesGreatCircle_Kerbin()
{
// Arrange
var orig = _KscFlagpole;
var dest = _VAB;
var expectedDistanceAtDestinationAltitude = 357.3329782527984d;
var expectedDistanceAtOriginAltitude = 357.33109503718202d;
var expectedDistanceAtSeaLeval = 357.29275513850934d;
var expectedDistanceWithAltitudeChange = 357.33203664499024d;
var expectedIntialBearing = 94.383387027330514d;
var expectedAltitudeChange = 3.1624748992035023d;
// Act
var route = new GreatCircle(orig, dest);
// Assert
Assert.That(
route.DeltaASL,
Is.EqualTo(expectedAltitudeChange).Within(0.0001),
"Miscalculated altitude difference");
Assert.That(
route.DistanceAtDestAlt,
Is.EqualTo(expectedDistanceAtDestinationAltitude).Within(0.0001),
"Miscalculated distance at destination's altitude");
Assert.That(
route.DistanceAtOrigAlt,
Is.EqualTo(expectedDistanceAtOriginAltitude).Within(0.0001),
"Miscalculated distance at origin's altitude");
Assert.That(
route.DistanceAtSeaLevel,
Is.EqualTo(expectedDistanceAtSeaLeval).Within(0.0001),
"Miscalculated distance at sea level");
Assert.That(
route.DistanceWithAltChange,
Is.EqualTo(expectedDistanceWithAltitudeChange).Within(0.0001),
"Miscalculated distance including altitude difference");
Assert.That(
route.ForwardAzimuth,
Is.EqualTo(expectedIntialBearing).Within(0.0001),
"Miscalulated initial bearing");
}
public void WGS84Test_Statics()
{
Assert.True(TestDistAndDir(0, 180, 0, -180));
Assert.True(TestDistAndDir(0, -179, 0, 179));
Assert.True(TestDistAndDir(45, 10, -55, -170));
Assert.True(TestDistAndDir(-35.84, 94, 35.87, -85));
double dist = 0;
double dir = 0;
GreatCircle.DistAndDir(47, 9, 47, 9, out dist, out dir);
Assert.Equal(0, dist);
double dist2 = 0;
double dir2 = 0;
GreatCircle.DistAndDir(-35.84, 94,
35.87, -85, out dist, out dir);
GreatCircle.DistAndDir(-35.840000001, 94,
35.87, -85, out dist2, out dir2);
Assert.True(dist2 > dist);
GreatCircle.DistAndDir(-35.84, 94.000001,
35.87, -85, out dist2, out dir2);
Assert.True(dist2 > dist);
}
public void CalculatesGreatCircle_NoDeltaAlt()
{
// Arrange
var orig = _landsEnd;
var dest = _JohnoGroats;
var expectedDistance = 968853.54671313858d;
var expectedIntialBearing = 9.1198181045040769d;
// Act
var route = new GreatCircle(orig, dest);
// Assert
Assert.That(
route.DeltaASL,
Is.EqualTo(0).Within(0.0001),
"Miscalculated altitude difference");
Assert.That(
route.DistanceAtDestAlt,
Is.EqualTo(expectedDistance).Within(0.0001),
"Miscalculated distance at destination's altitude");
Assert.That(
route.DistanceAtOrigAlt,
Is.EqualTo(expectedDistance).Within(0.0001),
"Miscalculated distance at origin's altitude");
Assert.That(
route.DistanceAtSeaLevel,
Is.EqualTo(expectedDistance).Within(0.0001),
"Miscalculated distance at sea level");
Assert.That(
route.DistanceWithAltChange,
Is.EqualTo(expectedDistance).Within(0.0001),
"Miscalculated distance including altitude difference");
Assert.That(
route.ForwardAzimuth,
Is.EqualTo(expectedIntialBearing).Within(0.0001),
"Miscalulated initial bearing");
}
private void textBox_TextChanged(object sender, TextChangedEventArgs e)
{
var latitude = Regex.Match(textBox.Text, @"(?<=Latitude\s=\s)[-\.\d]+").Value;
var longitude = Regex.Match(textBox.Text, @"(?<=Longitude\s=\s)[-\.\d]+").Value;
var altitude = Regex.Match(textBox.Text, @"(?<=Altitude\s=\s)[-\.\d]+").Value;
if ((latitude != string.Empty) && (longitude != string.Empty) && (altitude != string.Empty))
{
var coordinates =
new WorldCoordinates(
double.Parse(latitude),
double.Parse(longitude),
double.Parse(altitude),
600000);
var gc = new GreatCircle(_flagPole, coordinates);
var output = $" ForwardAzimuth = {gc.ForwardAzimuth}\n Distance = {gc.DistanceAtOrigAlt} \n DeltaAltitude = {gc.DeltaASL}";
Clipboard.SetText(output);
outputBlock1.Text = $"copied to clipboard:\n{output}";
}
else
{
outputBlock1.Text = "Could not extract Latitude, Longitude, and Altitude";
}
}
public void GreatCircle_Travel_to_WorldCoordinate()
{
// Arrange
var orig = _KscFlagpole;
var dest = _VAB;
// Act
var route = new GreatCircle(orig, dest);
var endpoint = orig.Travel(route);
// Assert
Assert.That(
endpoint.Latitude,
Is.EqualTo(dest.Latitude).Within(0.0001),
"Unacceptable variance in calculated endpoint latitude");
Assert.That(
endpoint.Longitude,
Is.EqualTo(dest.Longitude).Within(0.0001),
"Unacceptable variance in calculated endpoint longitude");
Assert.That(
endpoint.Altitude,
Is.EqualTo(dest.Altitude).Within(0.0001),
"Unacceptable variance in calculated endpoint altitude");
}
public void GreatCircleDeltas()
{
GeographicPosition p1 = new GeographicPosition(0, 1, 0);
GeographicPosition p2 = new GeographicPosition(0, 2, 0);
GeographicPosition p3 = new GeographicPosition(1, 1, 0);
Length distance = p1.DistanceTo(p2);
Angle direction;
Assert.Equal(111319.49079327357, distance.Meters, 10);
direction = p1.DirectionTo(p2);
Assert.Equal(90, direction.Degrees, 10);
distance = p1.DistanceTo(p3);
Assert.Equal(110574.38855780043, distance.Meters, 10);
p1 = new GeographicPosition(89.9999999, 0, 0);
p2 = new GeographicPosition(-89.999, 0, 0);
GreatCircle.DistAndDir(p1, p2, out distance, out direction);
Assert.Equal(20003.8197534766, distance.Kilometers, 10);
Assert.Equal(180, direction.Degrees, 5);
p1 = new GeographicPosition(0.00002, 90.99, 0);
p2 = new GeographicPosition(0.00001, -89.99, 0);
GreatCircle.DistAndDir(p1, p2, out distance, out direction);
Assert.Equal(19928.415241680563, distance.Kilometers, 10);
Assert.Equal(89.9954651234, direction.Degrees, 10);
p1 = new GeographicPosition(45, 9, 0);
p2 = new GeographicPosition(45, 10, 90);
GreatCircle.DistAndDir(p1, p2, out distance, out direction);
Assert.Equal(78.84633470979473, distance.Kilometers, 10);
Assert.Equal(89.6464421068, direction.Degrees, 10);
}
/// <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);
}
/// <summary>
/// Navigation loop.
/// </summary>
internal void CalculateNewStatus(int loops, DateTimeOffset now)
{
bool passedWp = false;
RecommendedMinimumNavToDestination?currentLeg = null;
if (_cache.TryGetLastSentence(RecommendedMinimumNavToDestination.Id, out RecommendedMinimumNavToDestination currentLeg1) &&
currentLeg1.Valid)
{
passedWp = currentLeg1.Arrived;
if (_selfNavMode)
{
// Reset navigation
_manualNextWaypoint = null;
_selfNavMode = false;
}
OperationState = AutopilotErrorState.OperatingAsSlave;
currentLeg = currentLeg1;
}
else
{
// So we have to test only one condition
currentLeg = null;
}
if (_activeDeviation == null || loops % 100 == 0)
{
if (!_cache.TryGetLastSentence(HeadingAndDeclination.Id, out HeadingAndDeclination deviation) ||
!deviation.Declination.HasValue)
{
if (!_cache.TryGetLastSentence(RecommendedMinimumNavigationInformation.Id,
out RecommendedMinimumNavigationInformation rmc) || !rmc.MagneticVariationInDegrees.HasValue)
{
if (loops % LogSkip == 0)
{
_logger.LogWarning("Autopilot: No magnetic variance");
}
return;
}
deviation = new HeadingAndDeclination(Angle.Zero, Angle.Zero, rmc.MagneticVariationInDegrees);
}
_activeDeviation = deviation;
}
if (_cache.TryGetCurrentPosition(out var position, out Angle track, out Speed sog, out Angle? heading) && position != null)
{
string previousWayPoint = string.Empty;
string nextWayPoint = string.Empty;
if (currentLeg != null)
{
previousWayPoint = currentLeg.PreviousWayPointName;
nextWayPoint = currentLeg.NextWayPointName;
}
List <RoutePoint>?currentRoute = null;
if (_activeRoute != null)
{
currentRoute = _activeRoute.Points;
}
RoutePoint?next;
// This returns RoutePresent if at least one valid waypoint is in the list
if (currentRoute == null && _cache.TryGetCurrentRoute(out currentRoute) != AutopilotErrorState.RoutePresent)
{
// No route. But if we have an RMB message, there could still be a current target (typically one that was
// directly selected with "Goto")
if (currentLeg == null)
{
OperationState = AutopilotErrorState.NoRoute;
return;
}
OperationState = AutopilotErrorState.DirectGoto;
next = new RoutePoint("Goto", 0, 1, currentLeg.NextWayPointName, currentLeg.NextWayPoint, null, null);
}
else if (currentLeg != null)
{
// Better to compare by position rather than name, because the names (unless using identifiers) may
// not be unique.
next = currentRoute.FirstOrDefault(x => x.Position.EqualPosition(currentLeg.NextWayPoint));
}
else
{
if (_manualNextWaypoint == null)
{
next = currentRoute.First();
_manualNextWaypoint = next;
}
else if (!HasPassedWaypoint(position, track, ref _manualNextWaypoint, currentRoute))
{
next = _manualNextWaypoint;
}
else
{
passedWp = true;
next = _manualNextWaypoint;
if (next == null) // reached end of route
{
currentRoute = null;
}
}
OperationState = AutopilotErrorState.OperatingAsMaster;
}
if (next != null && next.Position != null && (_knownNextWaypoint == null || next.Position.EqualPosition(_knownNextWaypoint.Position) == false))
{
// the next waypoint changed. Set the new origin (if previous is undefined)
// This means that either the user has selected a new route or we moved to the next leg.
_knownNextWaypoint = next;
_currentOrigin = null;
}
RoutePoint?previous = null;
if (currentRoute != null)
{
previous = currentRoute.Find(x => x.WaypointName == previousWayPoint);
}
if (previous == null && next != null)
{
if (_currentOrigin != null)
{
previous = _currentOrigin;
}
else
{
// Assume the current position is the origin
GreatCircle.DistAndDir(position, next.Position !, out Length distance, out Angle direction);
_currentOrigin = new RoutePoint("Goto", 1, 1, "Origin", position, direction,
distance);
previous = _currentOrigin;
}
}
else
{
// We don't need that any more. Reinit when previous is null again
_currentOrigin = null;
}
if (next == null)
{
// No position for next waypoint
OperationState = AutopilotErrorState.InvalidNextWaypoint;
NextWaypoint = null;
// Note: Possibly reached destination
return;
}
Length distanceToNext = Length.Zero;
Length distanceOnTrackToNext = Length.Zero;
Length crossTrackError = Length.Zero;
Length distancePreviousToNext = Length.Zero;
Angle bearingCurrentToDestination = Angle.Zero;
Angle bearingOriginToDestination = Angle.Zero;
GeographicPosition nextPosition = new GeographicPosition();
Speed approachSpeedToWayPoint = Speed.Zero;
if (next.Position != null)
{
nextPosition = next.Position;
GreatCircle.DistAndDir(position, next.Position, out distanceToNext, out bearingCurrentToDestination);
approachSpeedToWayPoint = GreatCircle.CalculateVelocityTowardsTarget(next.Position, position, sog, track);
// Either the last waypoint or "origin"
if (previous != null && previous.Position != null)
{
GreatCircle.DistAndDir(previous.Position, next.Position, out distancePreviousToNext, out bearingOriginToDestination);
GreatCircle.CrossTrackError(previous.Position, next.Position, position, out crossTrackError, out distanceOnTrackToNext);
}
}
NextWaypoint = next;
List <NmeaSentence> sentencesToSend = new List <NmeaSentence>();
RecommendedMinimumNavToDestination rmb = new RecommendedMinimumNavToDestination(now,
crossTrackError, previousWayPoint, nextWayPoint, nextPosition, distanceToNext, bearingCurrentToDestination,
approachSpeedToWayPoint, passedWp);
CrossTrackError xte = new CrossTrackError(crossTrackError);
Angle variation = _activeDeviation.Declination.GetValueOrDefault(Angle.Zero);
TrackMadeGood vtg = new TrackMadeGood(track, AngleExtensions.TrueToMagnetic(track, variation), sog);
BearingAndDistanceToWayPoint bwc = new BearingAndDistanceToWayPoint(now, nextWayPoint, nextPosition, distanceToNext,
bearingCurrentToDestination, AngleExtensions.TrueToMagnetic(bearingCurrentToDestination, variation));
BearingOriginToDestination bod = new BearingOriginToDestination(bearingOriginToDestination, AngleExtensions.TrueToMagnetic(
bearingOriginToDestination, variation), previousWayPoint, nextWayPoint);
sentencesToSend.AddRange(new NmeaSentence[] { rmb, xte, vtg, bwc, bod });
if (loops % 2 == 0)
{
// Only send these once a second
IEnumerable <RoutePart> rte;
IEnumerable <Waypoint> wpt;
if (currentRoute == null || currentRoute.Count == 0)
{
currentRoute = new List <RoutePoint>();
if (_currentOrigin != null)
{
currentRoute.Add(_currentOrigin);
}
if (next.Position != null)
{
currentRoute.Add(next);
}
}
// This should actually always contain at least two points now (origin and current target)
if (currentRoute.Count > 0)
{
CreateRouteMessages(currentRoute, out rte, out wpt);
sentencesToSend.AddRange(wpt);
sentencesToSend.AddRange(rte);
}
}
_output.SendSentences(sentencesToSend);
}
}
private bool HasPassedWaypoint(GeographicPosition position, Angle courseOverGround, ref RoutePoint?nextWaypoint, List <RoutePoint> currentRoute)
{
RoutePoint?previousWayPoint = null;
RoutePoint?wayPointAfterNext = null;
int idx = 0;
if (nextWaypoint != null)
{
idx = currentRoute.IndexOf(nextWaypoint);
}
else
{
// Can't have passed a null waypoint
return(false);
}
if (idx < 0)
{
// This is weird
return(false);
}
if (idx == 0)
{
previousWayPoint = _currentOrigin;
}
else
{
previousWayPoint = currentRoute[idx - 1];
}
if (idx < currentRoute.Count - 2)
{
wayPointAfterNext = currentRoute[idx + 1];
}
GreatCircle.DistAndDir(position, nextWaypoint.Position, out var distanceToNext, out var angleToNext);
if (distanceToNext < WaypointSwitchDistance)
{
_logger.LogInformation($"Reached waypoint {nextWaypoint.WaypointName}");
nextWaypoint = wayPointAfterNext;
return(true);
}
if (previousWayPoint != null && wayPointAfterNext != null)
{
GreatCircle.CrossTrackError(previousWayPoint.Position, nextWaypoint.Position, position, out var crossTrackCurrentLeg,
out _);
GreatCircle.CrossTrackError(nextWaypoint.Position, wayPointAfterNext.Position, position, out var crossTrackNextLeg,
out var distanceToAfterNext);
Angle delta = AngleExtensions.Difference(courseOverGround, angleToNext);
// We switch to the next leg if the cross track error to it is smaller than to the current leg.
// This condition is obvious for the side of the route with the smaller angle (that's the one in which the
// route bends at nextWaypoint), for the other side we need the additional condition that that waypoint
// is no longer ahead of us. This is the case if the direction to it and our current track are pointing in
// opposite directions
if (crossTrackCurrentLeg > crossTrackNextLeg && Math.Abs(delta.Normalize(false).Degrees) > 90)
{
_logger.LogInformation($"Reached waypoint {nextWaypoint.WaypointName}");
nextWaypoint = wayPointAfterNext;
return(true);
}
}
return(false);
}
/// <summary>
/// Creates a new instance of the <see cref="VoronoiCell"/> class with the given center and sides.
/// </summary>
/// <param name="center">The point used as center for Voronoi calculations. Assumed to be inside.</param>
/// <param name="firstPart">The <see cref="GreatCircle"/> that makes the first part of the Polygon.</param>
public VoronoiCell(SphereCoordinate center, GreatCircle firstPart)
{
Center = center;
FirstPart = firstPart;
}
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);
}
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");
}
}
}
