Skip to content

Commit

Permalink
bugfix(nmea): fix the method that returns the satellite number and co…
Browse files Browse the repository at this point in the history 
…nstellation. Add unit test

Issue #17: #17
Asana: https://app.asana.com/0/1203851531040615/1208116466425061/f
  • Loading branch information
@malobanov
malobanov committed Sep 9, 2024
1 parent e23f3ed commit 9b5290f
Show file tree
Hide file tree
Showing 4 changed files with 241 additions and 44 deletions.
70 changes: 70 additions & 0 deletions src/Asv.Gnss.Test/NmeaTests.cs
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
using System;
using System.Globalization;
using System.Linq;
using System.Reactive.Linq;
using System.Text;
using Xunit;
Expand Down Expand Up @@ -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<Nmea0183MessageGSV>().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<int?>());

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()
{
Expand Down
25 changes: 14 additions & 11 deletions src/Asv.Gnss/Parsers/NMEA/Messages/Nmea0183MessageGSV.cs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
namespace Asv.Gnss
using System.Collections.Generic;

namespace Asv.Gnss
{
/// <summary>
/// GSV Satellites in view
Expand Down Expand Up @@ -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<Satellite>();
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.
Expand Down
184 changes: 154 additions & 30 deletions src/Asv.Gnss/Parsers/NMEA/Nmea0183Helper.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,66 +49,172 @@ 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)
{
switch (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)";

Expand Down Expand Up @@ -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";

Expand Down
6 changes: 3 additions & 3 deletions src/Asv.Gnss/Parsers/NMEA/Nmea0183MessageBase.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,11 +65,11 @@ public string SourceId
public override void Deserialize(ref ReadOnlySpan<byte> 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..];
}

/// <summary>
Expand Down

0 comments on commit 9b5290f

Please sign in to comment.