diff --git a/src/Asv.Gnss.Test/NmeaTests.cs b/src/Asv.Gnss.Test/NmeaTests.cs index 1479bdf..e0556cd 100644 --- a/src/Asv.Gnss.Test/NmeaTests.cs +++ b/src/Asv.Gnss.Test/NmeaTests.cs @@ -1,5 +1,6 @@ using System; using System.Globalization; +using System.Linq; using System.Reactive.Linq; using System.Text; using Xunit; @@ -71,6 +72,75 @@ public void Parsing_GSV_message_from_string() } + [Fact] + public void Parsing_multi_constellation_GSV_message_from_string() + { + var source = new[] + { + "$GLGSV,3,1,10,81,63,034,51,82,53,272,51,80,52,292,,79,38,200,49*6A\r\n", + "$GLGSV,3,2,10,65,24,046,42,66,16,105,47,73,15,334,46,88,14,062,46*61\r\n", + "$GLGSV,3,3,10,83,11,253,,72,07,001,44*68\r\n", + "$GPGSV,4,1,16,02,80,085,53,11,76,091,,12,61,180,51,25,58,284,52*79\r\n", + "$GPGSV,4,2,16,20,42,142,50,06,33,056,47,29,28,280,47,05,19,166,47*7F\r\n", + "$GPGSV,4,3,16,31,15,321,46,19,08,093,46,04,04,025,,09,03,055,*78\r\n", + "$GPGSV,4,4,16,44,32,184,48,51,31,171,48,48,31,194,47,46,30,199,48*7E\r\n", + "$GAGSV,3,1,09,34,72,231,53,30,65,251,53,36,51,059,51,02,36,170,49*62\r\n", + "$GAGSV,3,2,09,27,25,314,47,15,19,236,47,04,08,037,46,09,04,085,*65\r\n", + "$GAGSV,3,3,09,11,03,057,*50\r\n", + "$GQGSV,1,1,01,02,08,309,37*4D\r\n", + "$BDGSV,5,1,18,34,85,015,53,11,67,274,51,12,55,069,49,43,39,265,50*61\r\n", + "$BDGSV,5,2,18,23,37,289,50,25,36,225,48,44,31,078,49,22,25,064,46*69\r\n", + "$BDGSV,5,3,18,21,20,119,45,16,11,320,42,06,10,325,40,09,10,340,38*6D\r\n", + "$BDGSV,5,4,18,39,08,310,40,37,07,331,45,59,06,288,,19,05,017,*64\r\n", + "$BDGSV,5,5,18,31,03,333,,24,02,179,*68\r\n", + "$GIGSV,1,1,00,,,,*60\r\n" + }; + var array = source.Select(_ => Encoding.ASCII.GetBytes(_)).SelectMany(__ => __).ToArray(); + var msgs = new Nmea0183MessageGSV[17]; + var index = 0; + var parser = new Nmea0183Parser().RegisterDefaultMessages(); + parser.OnMessage.Cast().Subscribe(_ => + { + if (index >= 17) return; + msgs[index++] = _; + }); + foreach (var p in array) + { + parser.Read(p); + } + + var targetConstellation = new (NmeaNavigationSystemEnum Sys, int?[] SatPrn)[17]; + targetConstellation[0] = (Sys: NmeaNavigationSystemEnum.SYS_GLO, SatPrn: new int?[] { 17, 18, 16, 15 }); + targetConstellation[1] = (Sys: NmeaNavigationSystemEnum.SYS_GLO, SatPrn: new int?[] { 1, 2, 9, 24 }); + targetConstellation[2] = (Sys: NmeaNavigationSystemEnum.SYS_GLO, SatPrn: new int?[] { 19, 8 }); + targetConstellation[3] = (Sys: NmeaNavigationSystemEnum.SYS_GPS, SatPrn: new int?[] { 02, 11, 12, 25 }); + targetConstellation[4] = (Sys: NmeaNavigationSystemEnum.SYS_GPS, SatPrn: new int?[] { 20, 06, 29, 05 }); + targetConstellation[5] = (Sys: NmeaNavigationSystemEnum.SYS_GPS, SatPrn: new int?[] { 31, 19, 04, 09 }); + targetConstellation[6] = (Sys: NmeaNavigationSystemEnum.SYS_SBS, SatPrn: new int?[] { 131, 138, 135, 133 }); + targetConstellation[7] = (Sys: NmeaNavigationSystemEnum.SYS_GAL, SatPrn: new int?[] { 34, 30, 36, 02 }); + targetConstellation[8] = (Sys: NmeaNavigationSystemEnum.SYS_GAL, SatPrn: new int?[] { 27, 15, 04, 09 }); + targetConstellation[9] = (Sys: NmeaNavigationSystemEnum.SYS_GAL, SatPrn: new int?[] { 11 }); + targetConstellation[10] = (Sys: NmeaNavigationSystemEnum.SYS_QZS, SatPrn: new int?[] { 2 }); + targetConstellation[11] = (Sys: NmeaNavigationSystemEnum.SYS_CMP, SatPrn: new int?[] { 34, 11, 12, null }); + targetConstellation[12] = (Sys: NmeaNavigationSystemEnum.SYS_CMP, SatPrn: new int?[] { 23, 25, null, 22 }); + targetConstellation[13] = (Sys: NmeaNavigationSystemEnum.SYS_CMP, SatPrn: new int?[] { 21, 16, 06, 09 }); + targetConstellation[14] = (Sys: NmeaNavigationSystemEnum.SYS_CMP, SatPrn: new int?[] { null, null, null, 19 }); + targetConstellation[15] = (Sys: NmeaNavigationSystemEnum.SYS_CMP, SatPrn: new int?[] { 31, 24 }); + targetConstellation[16] = (Sys: NmeaNavigationSystemEnum.SYS_IRN, SatPrn: Array.Empty()); + + for (var i = 0; i < 17; i++) + { + Assert.NotNull(msgs[i]); + Assert.Equal(targetConstellation[i].SatPrn.Length, msgs[i].Satellites.Length); + for (var j = 0; j < msgs[i].Satellites.Length; j++) + { + Assert.Equal(targetConstellation[i].SatPrn[j], msgs[i].Satellites[j].ExtPRN); + if (msgs[i].Satellites[j].ExtPRN != null) + Assert.Equal(targetConstellation[i].Sys, msgs[i].Satellites[j].ExtNavSys); + } + } + } + [Fact] public void Parsing_GST_message_from_string() { diff --git a/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageGSV.cs b/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageGSV.cs index ed93541..2fe6b7c 100644 --- a/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageGSV.cs +++ b/src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageGSV.cs @@ -1,4 +1,6 @@ -namespace Asv.Gnss +using System.Collections.Generic; + +namespace Asv.Gnss { /// /// GSV Satellites in view @@ -35,34 +37,35 @@ protected override void InternalDeserializeFromStringArray(string[] items) if (!string.IsNullOrEmpty(items[2])) MessageNumber = int.Parse(items[2]); if (!string.IsNullOrEmpty(items[3])) SatellitesInView = int.Parse(items[3]); - Satellites = new Satellite[(items.Length - 4)/4]; - var index = 0; - for (var i = 4; i < 4 + Satellites.Length * 4; i += 4) + var length = (items.Length - 4) / 4; + var satellites = new List(); + for (var i = 4; i < 4 + length * 4; i += 4) { - var number = 0; + int number; var elevationDeg = 0; var azimuthDeg = 0; var snrdB = 0; if (!string.IsNullOrEmpty(items[i])) number = int.Parse(items[i]); + else continue; if (!string.IsNullOrEmpty(items[i + 1])) elevationDeg = int.Parse(items[i + 1]); if (!string.IsNullOrEmpty(items[i + 2])) azimuthDeg = int.Parse(items[i + 2]); if (!string.IsNullOrEmpty(items[i + 3])) snrdB = int.Parse(items[i + 3]); - Satellites[index] = new Satellite + var sat = new Satellite { Number = number, ElevationDeg = elevationDeg, AzimuthDeg = azimuthDeg, SnrdB = snrdB }; - if (Nmea0183Helper.GetPrnFromNmeaSatId(number, out var prn, out var nav)) + if (Nmea0183Helper.GetPrnFromNmeaSatId(SourceId, number, out var prn, out var nav)) { - Satellites[index].ExtPRN = prn; - Satellites[index].ExtNavSys = nav; + sat.ExtPRN = prn; + sat.ExtNavSys = nav; } - - index++; + satellites.Add(sat); } + Satellites = satellites.ToArray(); } /// Gets or sets the total number of messages. diff --git a/src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs b/src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs index f75ec25..044f7a3 100644 --- a/src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs +++ b/src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs @@ -49,56 +49,158 @@ public enum NmeaNavigationSystemEnum public static class Nmea0183Helper { + public static bool GetPrnFromNmeaSatId(int NMEASatId, out int PRN, out NmeaNavigationSystemEnum nav) { nav = NmeaNavigationSystemEnum.SYS_NONE; PRN = -1; if (NMEASatId <= 0) return false; - if (NMEASatId <= 32) - { - nav = NmeaNavigationSystemEnum.SYS_GPS; - PRN = NMEASatId; - return true; - } - - if (NMEASatId <= 54) - { - nav = NmeaNavigationSystemEnum.SYS_SBS; - PRN = NMEASatId + 87; - return true; - } - - if (NMEASatId <= 64) - { - return false; - } - - if (NMEASatId <= 96) + + switch (NMEASatId) { - nav = NmeaNavigationSystemEnum.SYS_GLO; - PRN = NMEASatId - 64; - return true; + case <= 32: + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_GPS; + return true; + case <= 54: + PRN = NMEASatId + 87; + nav = NmeaNavigationSystemEnum.SYS_SBS; + return true; + case >= 65 and <= 96: + PRN = NMEASatId - 64; + nav = NmeaNavigationSystemEnum.SYS_GLO; + return true; + case >= 193 and <= 199: + PRN = NMEASatId - 192; + nav = NmeaNavigationSystemEnum.SYS_QZS; + return true; + case >= 201 and <= 235: + PRN = NMEASatId - 200; + nav = NmeaNavigationSystemEnum.SYS_CMP; + return true; + case >= 301 and <= 336: + PRN = NMEASatId - 300; + nav = NmeaNavigationSystemEnum.SYS_GAL; + return true; + case >= 401 and <= 414: + PRN = NMEASatId - 400; + nav = NmeaNavigationSystemEnum.SYS_IRN; + return true; } - - // TODO: + + // 1 - 32 GPS // 33 - 54 Various SBAS systems (EGNOS, WAAS, SDCM, GAGAN, MSAS) // 55 - 64 not used (might be assigned to further SBAS systems) // 65 - 88 GLONASS - // 89 - 96 GLONASS (future extensions?) + // 89 - 96 GLONASS (future extensions) // 97 - 119 not used // 120 - 151 not used (SBAS PRNs occupy this range) // 152 - 158 Various SBAS systems (EGNOS, WAAS, SDCM, GAGAN, MSAS) // 159 - 172 not used // 173 - 182 IMES - // 193 - 197 QZSS - // 196 - 200 QZSS (future extensions?) + // 193 - 199 QZSS // 201 - 235 BeiDou (u-blox, not NMEA) // 301 - 336 GALILEO - // 401 - 437 BeiDou (NMEA) + // 401 - 414 IRNSS + // 415 - 437 IRNSS (future extensions) return false; } + public static bool GetPrnFromNmeaSatId(string talkerId, int NMEASatId, out int PRN, + out NmeaNavigationSystemEnum nav) + { + if (NMEASatId is >= 120 and <= 158) + { + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_SBS; + return true; + } + + switch (talkerId) + { + case "GN": + return GetPrnFromNmeaSatId(NMEASatId, out PRN, out nav); + case "GP": + switch (NMEASatId) + { + case <= 32: + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_GPS; + return true; + case <= 64: + PRN = NMEASatId + 87; + nav = NmeaNavigationSystemEnum.SYS_SBS; + return true; + } + break; + case "GL": + if (NMEASatId is >= 65 and <= 96) + { + PRN = NMEASatId - 64; + nav = NmeaNavigationSystemEnum.SYS_GLO; + return true; + } + break; + case "GQ": + switch (NMEASatId) + { + case >= 1 and <= 7: + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_QZS; + return true; + case >= 193 and <= 199: + PRN = NMEASatId - 192; + nav = NmeaNavigationSystemEnum.SYS_QZS; + return true; + } + break; + case "GB": + case "BD": + switch (NMEASatId) + { + case >= 1 and <= 35: + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_CMP; + return true; + case >= 201 and <= 235: + PRN = NMEASatId - 200; + nav = NmeaNavigationSystemEnum.SYS_CMP; + return true; + } + break; + case "GA": + switch (NMEASatId) + { + case >= 1 and <= 36: + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_GAL; + return true; + case >= 301 and <= 336: + PRN = NMEASatId - 300; + nav = NmeaNavigationSystemEnum.SYS_GAL; + return true; + } + break; + case "GI": + switch (NMEASatId) + { + case >= 1 and <= 14: + PRN = NMEASatId; + nav = NmeaNavigationSystemEnum.SYS_IRN; + return true; + case >= 401 and <= 414: + PRN = NMEASatId - 400; + nav = NmeaNavigationSystemEnum.SYS_IRN; + return true; + } + break; + } + + PRN = -1; + nav = NmeaNavigationSystemEnum.SYS_NONE; + return false; + } public static string TryFindSourceTitleById(string value) { @@ -106,9 +208,13 @@ public static string TryFindSourceTitleById(string value) { case "AG": return "Autopilot - General"; + case "AP": return "Autopilot - Magnetic"; + case "BD": + return "BeiDou Navigation Satellite System"; + case "CD": return "Communications – Digital Selective Calling (DSC)"; @@ -139,9 +245,27 @@ public static string TryFindSourceTitleById(string value) case "ER": return "Engine Room Monitoring Systems"; + case "GA": + return "Galileo Navigation Satellite System"; + + case "GB": + return "BeiDou Navigation Satellite System"; + + case "GI": + return "Indian Regional Navigation Satellite System (IRNSS)"; + + case "GL": + return "GLONASS Navigation Satellite System"; + + case "GN": + return "Global Navigation"; + case "GP": return "Global Positioning System (GPS)"; - + + case "GQ": + return "Quasi-Zenith Satellite System (QZSS)"; + case "HC": return "Heading – Magnetic Compass"; diff --git a/src/Asv.Gnss/Parsers/NMEA/Nmea0183MessageBase.cs b/src/Asv.Gnss/Parsers/NMEA/Nmea0183MessageBase.cs index 724e4c5..b1d77c9 100644 --- a/src/Asv.Gnss/Parsers/NMEA/Nmea0183MessageBase.cs +++ b/src/Asv.Gnss/Parsers/NMEA/Nmea0183MessageBase.cs @@ -65,11 +65,11 @@ public string SourceId public override void Deserialize(ref ReadOnlySpan buffer) { if (buffer.Length < 5) throw new Exception("Too small string for NMEA"); - var message = buffer.GetString(Encoding.ASCII); + var message = buffer.GetString(Encoding.ASCII).Trim(); SourceId = message.StartsWith('P') ? "P" : message[..2]; - var items = message.Trim().Split(','); + var items = message.Split(','); InternalDeserializeFromStringArray(items); - buffer = buffer.Slice(buffer.Length); + buffer = buffer[buffer.Length..]; } ///