Merge branch 'main' into feat/new-workflows

asv-soft · Oct 16, 2024 · e59b2a1 · e59b2a1
2 parents eb7f125 + 6faefc1
commit e59b2a1
Show file tree

Hide file tree

Showing 10 changed files with 223 additions and 12 deletions.
diff --git a/src/Asv.Gnss.Tests/NmeaTests.cs b/src/Asv.Gnss.Tests/NmeaTests.cs
@@ -60,7 +60,7 @@ public void Serialize_GGA_message_from_byteArray()
             }
 
             var targetMsg = GetNmeaMessages(msg);
-            Assert.Equal("$GPGGA,125319,5508.7020098,N,06124.3378698,E,7,08,2.4,259.000,M,-12.794,M,,0000*68\r\n", targetMsg);
+            Assert.Equal("$GPGGA,125319,5508.7020098,N,06124.3378698,E,7,08,2.4,259.000,M,-12.794,M,,*68\r\n", targetMsg);
         }
 
 
@@ -280,9 +280,9 @@ public void Serialize_RMC_message_from_byteArray()
             {
                 0x24, 0x47, 0x50, 0x52, 0x4D, 0x43, 0x2C, 0x31, 0x32, 0x33, 0x35, 0x31, 0x39, 0x2C, 0x41, 0x2C, 0x34,
                 0x38, 0x30, 0x37, 0x2E, 0x30, 0x33, 0x38, 0x2C, 0x4E, 0x2C, 0x30, 0x31, 0x31, 0x33, 0x31, 0x2E, 0x30,
-                0x30, 0x30, 0x2C, 0x45, 0x2C, 0x30, 0x32, 0x32, 0x2E, 0x34, 0x2C, 0x30, 0x38, 0x34, 0x2E, 0x34, 0x2C,
-                0x32, 0x33, 0x30, 0x33, 0x39, 0x34, 0x2C, 0x30, 0x30, 0x33, 0x2E, 0x31, 0x2C, 0x57, 0x2A, 0x36, 0x41,
-                0x0D, 0x0A
+                0x30, 0x2C, 0x45, 0x2C, 0x30, 0x32, 0x32, 0x2E, 0x34, 0x2C, 0x30, 0x38, 0x34, 0x2E, 0x34, 0x2C, 0x32,
+                0x33, 0x30, 0x33, 0x39, 0x34, 0x2C, 0x30, 0x30, 0x33, 0x2E, 0x31, 0x2C, 0x57, 0x2C, 0x41, 0x2A, 0x33,
+                0x37, 0x0D, 0x0A
             };
             Nmea0183MessageRMC msg = null;
             var parser = new Nmea0183Parser().RegisterDefaultMessages();
@@ -293,7 +293,7 @@ public void Serialize_RMC_message_from_byteArray()
             }
 
             var targetMsg = GetNmeaMessages(msg);
-            Assert.Equal("$GPRMC,123519,A,4807.038,N,01131.00,E,022.4,084.4,230394,003.1,W*5A\r\n", targetMsg);
+            Assert.Equal("$GPRMC,123519,A,4807.038,N,01131.00,E,022.4,084.4,230394,003.1,W,A*37\r\n", targetMsg);
         }
     }
 }
diff --git a/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGloObservations.cs b/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGloObservations.cs
@@ -81,7 +81,25 @@ protected override int InternalGetContentByteSize()
 
         public override void Randomize(Random random)
         {
-            throw new NotImplementedException();
+            Tod = new DateTime(2014, 08, 20, 15, 0, 0, DateTimeKind.Utc);
+            var length = (random.Next() % 6) + 4;
+            var randomPrn = new int[length];
+            var index = 0;
+            while (index < length)
+            {
+                var prn = random.Next() % 24 + 1;
+                if (randomPrn.Any(_ => _ == prn)) continue;
+                randomPrn[index] = prn;
+                index++;
+            }
+            Observations = new AsvGloObservation[length];
+
+            for (var i = 0; i < length; i++)
+            {
+                var obs = new AsvGloObservation();
+                obs.Randomize(random, randomPrn[i]);
+                Observations[i] = obs;
+            }
         }
     }
 
@@ -233,6 +251,15 @@ public int GetByteSize()
         /// </summary>
         public double L1CNR { get; set; }
 
+        public void Randomize(Random random, int prn)
+        {
+            Prn = prn;
+            SatelliteId = AsvHelper.satno(NavigationSystemEnum.SYS_GLO, Prn);
+            SatelliteCode = AsvHelper.Sat2Code(SatelliteId, Prn);
+            L1Code = AsvHelper.CODE_L1C;
+            Frequency = 1602000000 + (random.Next() % 16 - 7) * 562500;
+            L1LockTime = 937;
+        }
     }
 }
 
diff --git a/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGloRawCa.cs b/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGloRawCa.cs
@@ -98,7 +98,22 @@ protected override int InternalGetContentByteSize()
 
         public override void Randomize(Random random)
         {
-            throw new NotImplementedException();
+            EpochTime = new DateTime(2014, 08, 20, 15, 0, 0, DateTimeKind.Utc);
+            Prn = random.Next() % 24 + 1;
+            SatelliteId = AsvHelper.satno(NavigationSystemEnum.SYS_GLO, Prn);
+            L1Code = AsvHelper.CODE_L1C;
+            SignalType = GnssSignalTypeEnum.L1CA;
+            RindexSignalCode = "1C";
+            RinexSatCode = $"R{Prn}";
+            Frequency = 1602000000 + (random.Next() % 16 - 7) * 562500;
+            NAVBitsU32 = new uint[15][];
+            GloWords = new GlonassWordBase[15];
+            for (var i = 0; i < 15; i++)
+            {
+                NAVBitsU32[i] = new uint[NavBitsU32Length];
+                NAVBitsU32[i][0] = (uint)(i + 1) << 27;
+                GloWords[i] = GlonassWordFactory.Create(NAVBitsU32[i]);
+            }
         }
 
         /// <summary>

diff --git a/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGpsObservations.cs b/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGpsObservations.cs
@@ -75,7 +75,25 @@ protected override int InternalGetContentByteSize()
 
         public override void Randomize(Random random)
         {
-            throw new NotImplementedException();
+            Tow = new DateTime(2014, 08, 20, 15, 0, 0, DateTimeKind.Utc);
+            var length = (random.Next() % 6) + 4;
+            var randomPrn = new int[length];
+            var index = 0;
+            while (index < length)
+            {
+                var prn = random.Next() % 32 + 1;
+                if (randomPrn.Any(_ => _ == prn)) continue;
+                randomPrn[index] = prn;
+                index++;
+            }
+            Observations = new AsvGpsObservation[length];
+
+            for (var i = 0; i < length; i++)
+            {
+                var obs = new AsvGpsObservation();
+                obs.Randomize(random, randomPrn[i]);
+                Observations[i] = obs;
+            }
         }
     }
 
@@ -155,6 +173,15 @@ public int GetByteSize()
             return 13;
         }
 
+        public void Randomize(Random random, int prn)
+        {
+            Prn = prn;
+            SatelliteId = AsvHelper.satno(NavigationSystemEnum.SYS_GPS, Prn);
+            SatelliteCode = AsvHelper.Sat2Code(SatelliteId, Prn);
+            L1Code = AsvHelper.CODE_L1C;
+            L1LockTime = 937;
+        }
+
         /// <summary>
         /// 
         /// </summary>

diff --git a/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGpsRawCa.cs b/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessageGpsRawCa.cs
@@ -21,6 +21,10 @@ protected override void InternalContentDeserialize(ref ReadOnlySpan<byte> buffer
             CrcPassed = AsvHelper.GetBitU(buffer, ref bitIndex, 1) != 0;
             var code1 = AsvHelper.GetBitU(buffer, ref bitIndex, 1); bitIndex += 4;
 
+            SignalType = GnssSignalTypeEnum.L1CA;
+            RindexSignalCode = "1C";
+            RinexSatCode = $"G{Prn}";
+
             SatelliteId = AsvHelper.satno(NavigationSystemEnum.SYS_GPS, Prn);
             var byteIndex = bitIndex / 8;
             buffer = buffer.Slice(byteIndex, buffer.Length - byteIndex);
@@ -65,7 +69,17 @@ protected override int InternalGetContentByteSize()
 
         public override void Randomize(Random random)
         {
-            throw new NotImplementedException();
+            UtcTime = new DateTime(2014, 08, 20, 15, 0, 0, DateTimeKind.Utc);
+            Prn = random.Next() % 32 + 1;
+            SatelliteId = AsvHelper.satno(NavigationSystemEnum.SYS_GPS, Prn);
+            L1Code = AsvHelper.CODE_L1C;
+            SignalType = GnssSignalTypeEnum.L1CA;
+            RindexSignalCode = "1C";
+            RinexSatCode = $"G{Prn}";
+            NAVBitsU32 = new uint[NavBitsU32Length];
+            NAVBitsU32[0] = (uint)GpsRawHelper.GpsSubframePreamble << 22;
+            NAVBitsU32[1] = (uint)(random.Next() % 5 + 1) << 8;
+            GpsSubFrame = GpsSubFrameFactory.Create(NAVBitsU32);
         }
 
         /// <summary>

diff --git a/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessagePvtGeo.cs b/src/Asv.Gnss/Parsers/Asv/Msg/AsvMessagePvtGeo.cs
@@ -50,6 +50,7 @@ protected override void InternalContentDeserialize(ref ReadOnlySpan<byte> buffer
             MeanCorrAge = AsvHelper.GetBitU(buffer, ref bitIndex, 16) * 0.01;
             HAccuracy = AsvHelper.GetBitU(buffer, ref bitIndex, 16) * 0.01;
             VAccuracy = AsvHelper.GetBitU(buffer, ref bitIndex, 16) * 0.01;
+            buffer = buffer[(bitIndex / 8)..];
         }
 
 
@@ -78,6 +79,7 @@ protected override void InternalContentSerialize(ref Span<byte> buffer)
             AsvHelper.SetBitU(buffer, (uint)Math.Round(MeanCorrAge / 0.01), ref bitIndex, 16);
             AsvHelper.SetBitU(buffer, (uint)Math.Round(HAccuracy / 0.01), ref bitIndex, 16);
             AsvHelper.SetBitU(buffer, (uint)Math.Round(VAccuracy / 0.01), ref bitIndex, 16);
+            buffer = buffer[(bitIndex / 8)..];
         }
 
         protected override int InternalGetContentByteSize()
@@ -87,7 +89,13 @@ protected override int InternalGetContentByteSize()
 
         public override void Randomize(Random random)
         {
-            throw new NotImplementedException();
+            Tow = GpsRawHelper.Utc2Gps(new DateTime(2014, 08, 20, 15, 0, 0, DateTimeKind.Utc));
+            PosType = AsvPosTypeEnum.DifferentialPvt;
+            TimeSystem = AsvTimeSystemEnum.Gps;
+            Datum = AsvDatumEnum.WGS84;
+            Latitude = random.NextDouble() * 180.0 - 90.0;
+            Longitude = random.NextDouble() * 360.0 - 180.0;
+            Height = random.NextDouble() * 2000.0;
         }
 
         /// <summary>

diff --git a/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageRMC.cs b/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageRMC.cs
@@ -38,8 +38,15 @@ protected override void InternalDeserializeFromStringArray(string[] items)
 
             Longitude = Nmea0183Helper.ParseLongitude(items[5]);
             if (string.Equals(items[6], "W", StringComparison.InvariantCultureIgnoreCase)) Longitude *= -1;
-            SpeedOverGroundKnots = Nmea0183Helper.ParseDouble(items[7]);
-            TrackMadeGoodDegreesTrue = Nmea0183Helper.ParseDouble(items[8]);
+            SpeedOverGroundKnots =
+                double.TryParse(items[7], NumberStyles.Any, CultureInfo.InvariantCulture, out var speedKnots)
+                    ? speedKnots
+                    : double.NaN;
+            TrackMadeGoodDegreesTrue =
+                double.TryParse(items[8], NumberStyles.Any, CultureInfo.InvariantCulture, out var trackMade)
+                    ? trackMade
+                    : double.NaN;
+
             Date = Nmea0183Helper.ParseDate(items[9]);
             if (double.TryParse(items[10], NumberStyles.Any, CultureInfo.InvariantCulture, out var magneticVariation))
             {

diff --git a/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageVTG.cs b/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageVTG.cs
@@ -0,0 +1,104 @@
+using System;
+using System.Globalization;
+using System.Text;
+using Asv.IO;
+
+namespace Asv.Gnss
+{
+    /// 1) Time (UTC)
+    /// 2) Track made good, degrees true
+    /// 4) Track made good, degrees magnetic
+    /// 6) Speed over ground, knots
+    /// 8) Speed over ground, kph
+    /// 10) Positioning system mode
+    public class Nmea0183MessageVTG : Nmea0183MessageBase
+    {
+        private const byte TrueTrack = 0x54; // 'T'
+        private const byte MagneticTrack = 0x4D; // 'M'
+        private const byte KnotsUnit = 0x4E; // 'N'
+        private const byte KphUnit = 0x4B; // 'K'
+
+        /// <summary>
+        /// Represents the GNSS message ID.
+        /// </summary>
+        public const string GnssMessageId = "VTG";
+
+        /// Gets the message ID associated with this message.
+        /// @return The message ID as a string.
+        /// /
+        public override string MessageId => GnssMessageId;
+        protected override void InternalDeserializeFromStringArray(string[] items)
+        {
+            TimeUtc = Nmea0183Helper.ParseTime(items[1]);
+            TrackMadeGoodDegreesTrue =
+                double.TryParse(items[2], NumberStyles.Any, CultureInfo.InvariantCulture, out var trueTrack)
+                    ? trueTrack
+                    : double.NaN;
+            TrackMadeGoodDegreesMagnetic =
+                double.TryParse(items[4], NumberStyles.Any, CultureInfo.InvariantCulture, out var magneticTrack)
+                    ? magneticTrack
+                    : double.NaN;
+            SpeedOverGroundKnots =
+                double.TryParse(items[6], NumberStyles.Any, CultureInfo.InvariantCulture, out var speedKnots)
+                    ? speedKnots
+                    : double.NaN;
+            SpeedOverGroundKph =
+                double.TryParse(items[8], NumberStyles.Any, CultureInfo.InvariantCulture, out var speedKph)
+                    ? speedKph
+                    : double.NaN;
+            if (items.Length > 10)
+            {
+                PositioningSystemMode = items[10];
+            }
+        }
+
+        protected override void InternalSerialize(ref Span<byte> buffer, Encoding encoding)
+        {
+            Nmea0183Helper.SerializeTime(TimeUtc).CopyTo(ref buffer, encoding);
+            InsertSeparator(ref buffer);
+            var trueTrack = double.IsNaN(TrackMadeGoodDegreesTrue)
+                ? string.Empty
+                : Math.Round(TrackMadeGoodDegreesTrue, 2).ToString("000.00", CultureInfo.InvariantCulture);
+            trueTrack.CopyTo(ref buffer, encoding);
+            InsertSeparator(ref buffer);
+            InsertByte(ref buffer, TrueTrack);
+            InsertSeparator(ref buffer);
+
+            var magneticTrack = double.IsNaN(TrackMadeGoodDegreesMagnetic)
+                ? string.Empty
+                : Math.Round(TrackMadeGoodDegreesMagnetic, 2).ToString("000.00", CultureInfo.InvariantCulture);
+            magneticTrack.CopyTo(ref buffer, encoding);
+            InsertSeparator(ref buffer);
+            InsertByte(ref buffer, MagneticTrack);
+            InsertSeparator(ref buffer);
+
+            var speedKnots = double.IsNaN(SpeedOverGroundKnots)
+                ? string.Empty
+                : Math.Round(SpeedOverGroundKnots, 2).ToString("000.00", CultureInfo.InvariantCulture);
+            speedKnots.CopyTo(ref buffer, encoding);
+            InsertSeparator(ref buffer);
+            InsertByte(ref buffer, KnotsUnit);
+            InsertSeparator(ref buffer);
+
+            var speedKph = double.IsNaN(SpeedOverGroundKph)
+                ? string.Empty
+                : Math.Round(SpeedOverGroundKph, 2).ToString("000.00", CultureInfo.InvariantCulture);
+            speedKph.CopyTo(ref buffer, encoding);
+            InsertSeparator(ref buffer);
+            InsertByte(ref buffer, KphUnit);
+            InsertSeparator(ref buffer);
+
+            if (string.IsNullOrWhiteSpace(PositioningSystemMode)) return;
+
+            InsertSeparator(ref buffer);
+            PositioningSystemMode.CopyTo(ref buffer, encoding);
+        }
+
+        public DateTime? TimeUtc { get; set; }
+        public double TrackMadeGoodDegreesTrue { get; set; }
+        public double TrackMadeGoodDegreesMagnetic { get; set; }
+        public double SpeedOverGroundKnots { get; set; }
+        public double SpeedOverGroundKph { get; set; }
+        public string PositioningSystemMode { get; set; } = "A";
+    }
+}
diff --git a/src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs b/src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs
@@ -647,6 +647,12 @@ public static string SetNavigationalStatus(this NmeaNavigationalStatusEnum statu
             };
         }
 
+        public static double KnotsToKph(double knots)
+        {
+            if (double.IsNaN(knots)) return double.NaN;
+            return knots * 1.852;
+        }
+
     }
 
     public enum NmeaGpsQuality
@@ -690,4 +696,6 @@ public enum NmeaPositionModeEnum
         RtkFloat, //F
         RtkFixed //R
     }
+
+
 }
diff --git a/src/Asv.Gnss/Parsers/NMEA/Nmea0183ParserFactory.cs b/src/Asv.Gnss/Parsers/NMEA/Nmea0183ParserFactory.cs
@@ -19,6 +19,7 @@ public static IEnumerable<Func<Nmea0183MessageBase>> DefaultMessages
                 yield return () => new Nmea0183MessageGSV();
                 yield return () => new Nmea0183MessageGBS();
                 yield return () => new Nmea0183MessageRMC();
+                yield return () => new Nmea0183MessageVTG();
             }
         }