/// <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); } }