示例#1
0
        public void RmbEncode()
        {
            string msg = "A,22.200,L,Ostsee,Nordsee,6030.00000,N,02000.00000,E,53.996,270.0,19.4,V,D";

            NmeaSentence.OwnTalkerId = TalkerId.GlobalPositioningSystem;
            var rmb = new RecommendedMinimumNavToDestination(DateTimeOffset.UtcNow, Length.FromNauticalMiles(22.2), "Ostsee", "Nordsee", new GeographicPosition(60.5, 20.0, 0),
                                                             Length.FromKilometers(100), Angle.FromDegrees(-90), Speed.FromMetersPerSecond(10), false);

            Assert.True(rmb.Valid);
            Assert.Equal(msg, rmb.ToNmeaParameterList());
        }
示例#2
0
        public void RmbDecode()
        {
            string text    = "$GPRMB,A,0.02,R,R3,R4,4728.9218,N,00930.3359,E,0.026,222.0,2.4,V,D";
            var    decoded = TalkerSentence.FromSentenceString(text, out var error);

            Assert.Equal(NmeaError.None, error);
            Assert.NotNull(decoded);

            RecommendedMinimumNavToDestination nav = (RecommendedMinimumNavToDestination)decoded !.TryGetTypedValue(ref _lastPacketTime) !;

            Assert.True(nav.Valid);
            Assert.Equal(-0.02, nav.CrossTrackError.NauticalMiles, 2);
            Assert.Equal("R3", nav.PreviousWayPointName);
            Assert.Equal("R4", nav.NextWayPointName);
            Assert.Equal(47.48203, nav.NextWayPoint.Latitude, 6);
            Assert.Equal(9.505598, nav.NextWayPoint.Longitude, 6);
            Assert.Equal(0.026, nav.DistanceToWayPoint.GetValueOrDefault(Length.Zero).NauticalMiles, 3);
            Assert.Equal(222.0, nav.BearingToWayPoint.GetValueOrDefault(Angle.Zero).Degrees, 1);
            Assert.Equal(2.4, nav.ApproachSpeed.GetValueOrDefault(Speed.Zero).Knots, 1);
        }
示例#3
0
        /// <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);
            }
        }