/// <summary>
/// Creates a route from an ordered set of points
/// </summary>
/// <param name="name">Name of the new route</param>
/// <param name="points">Points of the route (ordered)</param>
public Route(string name, params GeographicPosition[] points)
: this(name)
{
for (var index = 0; index < points.Length; index++)
{
var pt = points[index];
var routePt = new RoutePoint(name, index, points.Length, "WP" + index, pt, null, null);
_routePoints.Add(routePt);
}
CalculateMetaData();
// Would be weird if that fails now
CheckPointsUnique(_routePoints);
}
/// <summary>
/// Returns the current route
/// </summary>
/// <param name="routeList">The list of points along the route</param>
/// <returns>The state of the route received</returns>
public AutopilotErrorState TryGetCurrentRoute(out List <RoutePoint> routeList)
{
routeList = new List <RoutePoint>();
List <RoutePart>?segments = FindLatestCompleteRoute(out string routeName);
if (segments == null)
{
return(AutopilotErrorState.NoRoute);
}
List <string> wpNames = new List <string>();
foreach (var segment in segments)
{
wpNames.AddRange(segment.WaypointNames);
}
// We've seen RTE messages, but no waypoints yet
if (wpNames.Count == 0)
{
return(AutopilotErrorState.WaypointsWithoutPosition);
}
if (wpNames.GroupBy(x => x).Any(g => g.Count() > 1))
{
return(AutopilotErrorState.RouteWithDuplicateWaypoints);
}
for (var index = 0; index < wpNames.Count; index++)
{
var name = wpNames[index];
GeographicPosition?position = null;
if (_wayPoints.TryGetValue(name, out var pt))
{
position = pt.Position;
}
else
{
// Incomplete route - need to wait for all wpt messages
return(AutopilotErrorState.WaypointsWithoutPosition);
}
RoutePoint rpt = new RoutePoint(routeName, index, wpNames.Count, name, position, null, null);
routeList.Add(rpt);
}
return(AutopilotErrorState.RoutePresent);
}
/// <summary>
/// Adds the given point to the end of the route
/// </summary>
/// <param name="pt">Point to insert</param>
/// <exception cref="ArgumentException">Duplicate waypoint name, undefined position, etc.</exception>
public void AddPoint(RoutePoint pt)
{
if (pt == null || pt.Position == null || string.IsNullOrWhiteSpace(pt.WaypointName))
{
throw new ArgumentException("Inserted point must have a position and a valid name");
}
var newRoute = _routePoints.ToList();
newRoute.Add(pt);
// throws if a duplicate was inserted
CheckPointsUnique(newRoute);
_routePoints = newRoute;
CalculateMetaData();
}
/// <summary>
/// Sets the next point on the route (i.e. to skip a missed waypoint)
/// </summary>
/// <param name="pt">The next point</param>
/// <returns>True on success, false otherwise. This returns false if the given position is not part of the route.</returns>
public bool SetNextPoint(RoutePoint pt)
{
return(SetNextPoint(pt.Position));
}
/// <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);
}
}
