From 36a52359826a2ea647252642c184f001f9d3f6c0 Mon Sep 17 00:00:00 2001 From: Morten Nielsen Date: Sun, 26 Jul 2020 15:29:44 -0700 Subject: [PATCH] early gnss lib work --- src/NmeaParser.Gnss/GnssMessageAggregator.cs | 120 ++++++++++++++++ src/NmeaParser.Gnss/NmeaParser.Gnss.csproj | 52 +++++++ src/NmeaParser.Gnss/Ntrip/Carrier.cs | 23 +++ src/NmeaParser.Gnss/Ntrip/Caster.cs | 59 ++++++++ src/NmeaParser.Gnss/Ntrip/Client.cs | 140 +++++++++++++++++++ src/NmeaParser.Gnss/Ntrip/NtripSource.cs | 23 +++ src/NmeaParser.Gnss/Ntrip/NtripStream.cs | 46 ++++++ src/NmeaParser.sln | 20 +++ src/NmeaParser/MessageAggregator.cs | 57 ++++++++ 9 files changed, 540 insertions(+) create mode 100644 src/NmeaParser.Gnss/GnssMessageAggregator.cs create mode 100644 src/NmeaParser.Gnss/NmeaParser.Gnss.csproj create mode 100644 src/NmeaParser.Gnss/Ntrip/Carrier.cs create mode 100644 src/NmeaParser.Gnss/Ntrip/Caster.cs create mode 100644 src/NmeaParser.Gnss/Ntrip/Client.cs create mode 100644 src/NmeaParser.Gnss/Ntrip/NtripSource.cs create mode 100644 src/NmeaParser.Gnss/Ntrip/NtripStream.cs create mode 100644 src/NmeaParser/MessageAggregator.cs diff --git a/src/NmeaParser.Gnss/GnssMessageAggregator.cs b/src/NmeaParser.Gnss/GnssMessageAggregator.cs new file mode 100644 index 0000000..8825fae --- /dev/null +++ b/src/NmeaParser.Gnss/GnssMessageAggregator.cs @@ -0,0 +1,120 @@ +// ******************************************************************************* +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// ****************************************************************************** + +using NmeaParser.Messages; +using System; +using System.Linq; + +namespace NmeaParser.Gnss +{ + public class GnssMessageProcessor : MessageAggregator + { + + public GnssMessageProcessor(NmeaDevice device) : base(device) + { + } + + protected override void OnMessagesReceived(NmeaMessage[] messages) + { + base.OnMessagesReceived(messages); + // We prefer GNSS messages, so put thos first + var info = new PositionInformation(); + messages = messages.OrderBy(rmc => rmc.TalkerId == Talker.GlobalNavigationSatelliteSystem ? -100 : (int)rmc.TalkerId).ToArray(); + info.Longitude = double.NaN; + info.Latitude = double.NaN; + var RmcMessages = messages.OfType().Where(rmc => rmc.Active); + if (RmcMessages.Any()) + { + var rmc = RmcMessages.First(); + info.Latitude = rmc.Latitude; + info.Longitude = rmc.Longitude; + info.Speed = rmc.Speed; + info.TimeOfFix = rmc.FixTime; + } + var GsaMessages = messages.OfType().Where(gsa => gsa.Fix == Gsa.FixType.Fix3D); + if (!GsaMessages.Any()) + GsaMessages = messages.OfType().Where(gsa => gsa.Fix == Gsa.FixType.Fix2D); + if (GsaMessages.Any()) + { + var gsa = GsaMessages.First(); + info.Hdop = gsa.Hdop; + info.Pdop = gsa.Pdop; + info.Vdop = gsa.Vdop; + info.SatelliteIds = gsa.SatelliteIDs; + } + var GsvMessages = messages.OfType(); + if (GsvMessages.Any()) + { + Gsv gsv = GsvMessages.First(); + info.Satellites = gsv.SVs.ToArray(); + } + var GstMessages = messages.OfType(); + if (GstMessages.Any()) + { + Gst gst = GstMessages.First(); + info.HorizontalAccuracy = (gst.SemiMajorError + gst.SemiMinorError) / 2; + info.VerticalAccuracy = gst.SigmaHeightError; + if (!info.TimeOfFix.HasValue) + { + info.TimeOfFix = DateTime.UtcNow.Date.Add(gst.FixTime); + } + } + var GgaMessages = messages.OfType().Where(g => g.Quality != Gga.FixQuality.Invalid); + if (GgaMessages.Any()) + { + Gga gga = GgaMessages.First(); + info.Latitude = gga.Latitude; + info.Longitude = gga.Longitude; + info.Altitude = gga.Altitude; + info.GeoidalSeparation = gga.GeoidalSeparation; + if (!info.TimeOfFix.HasValue) + { + info.TimeOfFix = DateTime.UtcNow.Date.Add(gga.FixTime); + } + } + var GnsMessages = messages.OfType(); + if (GnsMessages.Any()) + { + Gns gns = GnsMessages.First(); + if (!info.TimeOfFix.HasValue) + { + info.TimeOfFix = DateTime.UtcNow.Date.Add(gns.FixTime); + } + info.GeoidalSeparation = gns.GeoidalSeparation; + info.Latitude = gns.Latitude; + info.Longitude = gns.Longitude; + } + LocationUpdated?.Invoke(this, info); + } + + public event EventHandler LocationUpdated; + } + + public struct PositionInformation + { + public double? Longitude { get; internal set; } + public double? Latitude { get; internal set; } + public double? Altitude { get; internal set; } + public double? GeoidalSeparation { get; internal set; } + public double? Speed { get; internal set; } + public double? Pdop { get; internal set; } + public double? Hdop { get; internal set; } + public double? Vdop { get; internal set; } + public double? HorizontalAccuracy { get; internal set; } + public double? VerticalAccuracy { get; internal set; } + public DateTimeOffset? TimeOfFix { get; internal set; } + public int[] SatelliteIds { get; internal set; } + public SatelliteVehicle[] Satellites { get; internal set; } + } +} \ No newline at end of file diff --git a/src/NmeaParser.Gnss/NmeaParser.Gnss.csproj b/src/NmeaParser.Gnss/NmeaParser.Gnss.csproj new file mode 100644 index 0000000..31f6254 --- /dev/null +++ b/src/NmeaParser.Gnss/NmeaParser.Gnss.csproj @@ -0,0 +1,52 @@ + + + + netstandard1.4 + true + + Debug;Release + AnyCPU + Morten Nielsen + Morten Nielsen + Global Navigation System library based on a NMEA input stream. + nmea winrt wpf uwp xamarin gps serialport bluetooth gnss galileo GLONASS + SharpGIS.NmeaParser.Gnss + 2.1 + NMEA Parser - GNSS + Apache-2.0 + https://dotmorten.github.io/NmeaParser/ + https://github.com/dotMorten/NmeaParser + en-US + Copyright © Morten Nielsen 2015-2020 + $(MSBuildThisFileDirectory)..\..\artifacts\NmeaParser\$(Configuration) + ..\..\artifacts\NuGet\$(Configuration)\ + + true + true + $(AllowedOutputExtensionsInPackageBuildOutputFolder);.pdb + 8.0 + true + true + enable + logo.png + + 2.1.0.0 + + + + $(DefineConstants);NETSTANDARD + + + + + + + + + + + + + + + diff --git a/src/NmeaParser.Gnss/Ntrip/Carrier.cs b/src/NmeaParser.Gnss/Ntrip/Carrier.cs new file mode 100644 index 0000000..aed0979 --- /dev/null +++ b/src/NmeaParser.Gnss/Ntrip/Carrier.cs @@ -0,0 +1,23 @@ +// ******************************************************************************* +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// ****************************************************************************** + +namespace NmeaParser.Gnss.Ntrip +{ + public enum Carrier : int + { + No = 0, + L1 = 1, + L1L2 = 2 + } +} diff --git a/src/NmeaParser.Gnss/Ntrip/Caster.cs b/src/NmeaParser.Gnss/Ntrip/Caster.cs new file mode 100644 index 0000000..639f1f5 --- /dev/null +++ b/src/NmeaParser.Gnss/Ntrip/Caster.cs @@ -0,0 +1,59 @@ +// ******************************************************************************* +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// ****************************************************************************** + +using System.Globalization; +using System.Net; + +namespace NmeaParser.Gnss.Ntrip +{ + public class Caster : NtripSource + { + internal Caster (string[] d) + { + var a = d[1].Split(':'); + Address = IPAddress.Parse(a[0]); + Port = int.Parse(a[1]); + Identifier = d[3]; + Operator = d[4]; + SupportsNmea = d[5] == "1"; + CountryCode = d[6]; + Latitude = double.Parse(d[7], CultureInfo.InvariantCulture); + Longitude = double.Parse(d[8], CultureInfo.InvariantCulture); + FallbackAddress = IPAddress.Parse(d[9]); + } + + public Caster(IPAddress address, int port, string identifier, string _operator, bool supportsNmea, string countryCode, double latitude, double longitude, IPAddress fallbackkAddress) + { + Address = address; + Port = port; + Identifier = identifier; + Operator = _operator; + SupportsNmea = supportsNmea; + CountryCode = countryCode; + Latitude = latitude; + Longitude = longitude; + FallbackAddress = fallbackkAddress; + } + + public IPAddress Address { get; } + public int Port { get; } + public string Identifier { get; } + public string Operator { get; } + public bool SupportsNmea { get; } + public string CountryCode { get; } + public double Latitude { get; } + public double Longitude { get; } + public IPAddress FallbackAddress { get; } + } +} diff --git a/src/NmeaParser.Gnss/Ntrip/Client.cs b/src/NmeaParser.Gnss/Ntrip/Client.cs new file mode 100644 index 0000000..b30b16a --- /dev/null +++ b/src/NmeaParser.Gnss/Ntrip/Client.cs @@ -0,0 +1,140 @@ +// ******************************************************************************* +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// ****************************************************************************** + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Net.Sockets; +using System.Threading.Tasks; + +namespace NmeaParser.Gnss.Ntrip +{ + public class Client : IDisposable + { + private readonly string _host; + private readonly int _port; + private string? _auth; + private Socket? sckt; + private bool connected; + private Task? runningTask; + + public Client(string host, int port) + { + _host = host; + _port = port; + } + + public Client(string host, int port, string username, string password) : this(host, port) + { + _auth = Convert.ToBase64String(System.Text.Encoding.UTF8.GetBytes(username + ":" + password)); + } + + public IEnumerable GetSourceTable() + { + string data = ""; + byte[] buffer = new byte[1024]; + using (var sck = Request("")) + { + int count; + while ((count = sck.Receive(buffer)) > 0) + { + data += System.Text.Encoding.UTF8.GetString(buffer, 0, count); + } + } + var lines = data.Split('\n'); + List sources = new List(); + foreach (var item in lines) + { + var d = item.Split(';'); + if (d.Length == 0) continue; + if (d[0] == "ENDSOURCETABLE") + break; + if (d[0] == "CAS") + { + sources.Add(new Caster(d)); + } + else if (d[0] == "STR") + { + sources.Add(new NtripStream(d)); + } + } + return sources; + } + + private Socket Request(string path) + { + var sckt = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp); + sckt.Blocking = true; + sckt.Connect(_host, _port); + + string msg = $"GET /{path} HTTP/1.1\r\n"; + msg += "User-Agent: NTRIP ntripclient\r\n"; + if (_auth != null) + { + msg += "Authorization: Basic " + _auth + "\r\n"; + } + msg += "Accept: */*\r\nConnection: close\r\n"; + msg += "\r\n"; + + byte[] data = System.Text.Encoding.ASCII.GetBytes(msg); + sckt.Send(data); + return sckt; + } + + public void Connect(string strName) + { + if (sckt != null) throw new Exception("Connection already open"); + sckt = Request(strName); + connected = true; + runningTask = Task.Run(ReceiveThread); + } + + private async Task ReceiveThread() + { + byte[] buffer = new byte[65536]; + + while (connected && sckt != null) + { + int count = sckt.Receive(buffer); + if (count > 0) + { + DataReceived?.Invoke(this, buffer.Take(count).ToArray()); + } + await Task.Delay(10); + } + sckt.Shutdown(SocketShutdown.Both); + sckt.Dispose(); + sckt = null; + } + + public Task CloseAsync() + { + if (runningTask != null) + { + connected = false; + var t = runningTask; + runningTask = null; + return t; + } + return Task.CompletedTask; + } + + public void Dispose() + { + _ = CloseAsync(); + } + + public event EventHandler? DataReceived; + } +} diff --git a/src/NmeaParser.Gnss/Ntrip/NtripSource.cs b/src/NmeaParser.Gnss/Ntrip/NtripSource.cs new file mode 100644 index 0000000..8cc71da --- /dev/null +++ b/src/NmeaParser.Gnss/Ntrip/NtripSource.cs @@ -0,0 +1,23 @@ +// ******************************************************************************* +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// ****************************************************************************** + +namespace NmeaParser.Gnss.Ntrip +{ + public class NtripSource + { + protected NtripSource() + { + } + } +} diff --git a/src/NmeaParser.Gnss/Ntrip/NtripStream.cs b/src/NmeaParser.Gnss/Ntrip/NtripStream.cs new file mode 100644 index 0000000..b713ee7 --- /dev/null +++ b/src/NmeaParser.Gnss/Ntrip/NtripStream.cs @@ -0,0 +1,46 @@ +// ******************************************************************************* +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// ****************************************************************************** + +using System.Globalization; + +namespace NmeaParser.Gnss.Ntrip +{ + public class NtripStream : NtripSource + { + internal NtripStream(string[] d) + { + Mountpoint = d[1]; + Identifier = d[2]; + Format = d[3]; + FormatDetails = d[4]; + Carrier = (Carrier)int.Parse(d[5]); + Network = d[7]; + CountryCode = d[8]; + Latitude = double.Parse(d[9], CultureInfo.InvariantCulture); + Longitude = double.Parse(d[10], CultureInfo.InvariantCulture); + SupportsNmea = d[11] == "1"; + } + + public string Mountpoint { get; } + public string Identifier { get; } + public string Format { get; } + public string FormatDetails { get; } + public Carrier Carrier { get; } + public string Network { get; } + public string CountryCode { get; } + public double Latitude { get; } + public double Longitude { get; } + public bool SupportsNmea { get; } + } +} diff --git a/src/NmeaParser.sln b/src/NmeaParser.sln index deb258b..6fd94d0 100644 --- a/src/NmeaParser.sln +++ b/src/NmeaParser.sln @@ -37,8 +37,11 @@ Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "NmeaParser.Tests.Net", "Uni EndProject Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "SampleApp.NetCore", "SampleApp.WinDesktop\SampleApp.NetCore.csproj", "{6F97C607-42A0-458B-B9E9-CF5AF53CBB1E}" EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "NmeaParser.Gnss", "NmeaParser.Gnss\NmeaParser.Gnss.csproj", "{3ABEC519-B792-41D0-8462-1512B05276D6}" +EndProject Global GlobalSection(SharedMSBuildProjectFiles) = preSolution + UnitTests\NmeaParser.Tests\NmeaParser.Tests.projitems*{73efb2ef-de40-46c4-9685-745a9815c0d2}*SharedItemsImports = 5 UnitTests\NmeaParser.Tests\NmeaParser.Tests.projitems*{92cad93b-6c3b-45a0-a723-be046de50fec}*SharedItemsImports = 4 UnitTests\NmeaParser.Tests\NmeaParser.Tests.projitems*{979ae182-eb59-4181-9d45-3fd6e4817f11}*SharedItemsImports = 13 EndGlobalSection @@ -171,6 +174,22 @@ Global {6F97C607-42A0-458B-B9E9-CF5AF53CBB1E}.Release|x64.Build.0 = Release|Any CPU {6F97C607-42A0-458B-B9E9-CF5AF53CBB1E}.Release|x86.ActiveCfg = Release|Any CPU {6F97C607-42A0-458B-B9E9-CF5AF53CBB1E}.Release|x86.Build.0 = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|Any CPU.Build.0 = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|ARM.ActiveCfg = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|ARM.Build.0 = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|x64.ActiveCfg = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|x64.Build.0 = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|x86.ActiveCfg = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Debug|x86.Build.0 = Debug|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|Any CPU.ActiveCfg = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|Any CPU.Build.0 = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|ARM.ActiveCfg = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|ARM.Build.0 = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|x64.ActiveCfg = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|x64.Build.0 = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|x86.ActiveCfg = Release|Any CPU + {3ABEC519-B792-41D0-8462-1512B05276D6}.Release|x86.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE @@ -187,6 +206,7 @@ Global {48540D33-4349-42D2-9D49-144A7049565A} = {456E7573-3324-43CB-8BA0-8D9C300EEB50} {73EFB2EF-DE40-46C4-9685-745A9815C0D2} = {28B8E327-C504-4E08-B2CE-09D1CBB8B904} {6F97C607-42A0-458B-B9E9-CF5AF53CBB1E} = {7ABA337E-6748-484E-A0F4-E1715E1C95F1} + {3ABEC519-B792-41D0-8462-1512B05276D6} = {1701F3BA-A09C-4706-A612-24FD9340FC18} EndGlobalSection GlobalSection(ExtensibilityGlobals) = postSolution SolutionGuid = {03788B53-C0BF-485B-AA19-A9EAB0E9AF7B} diff --git a/src/NmeaParser/MessageAggregator.cs b/src/NmeaParser/MessageAggregator.cs new file mode 100644 index 0000000..c37e6f7 --- /dev/null +++ b/src/NmeaParser/MessageAggregator.cs @@ -0,0 +1,57 @@ +using NmeaParser.Messages; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace NmeaParser +{ + /// + /// Aggregates a group of messages, and raises an event once all messages has completed transmission + /// + public class MessageAggregator + { + private readonly Dictionary _messages = new Dictionary(); + + /// + /// Initializes a new instance of the class. + /// + /// + public MessageAggregator(NmeaDevice device) + { + if (device == null) + throw new ArgumentNullException(nameof(device)); + Device = device; + Device.MessageReceived += Device_MessageReceived; + } + + private void Device_MessageReceived(object sender, NmeaMessageReceivedEventArgs e) + { + if (_messages.ContainsKey(e.Message.MessageType)) + { + var messages = _messages.Values.ToArray(); + _messages.Clear(); + OnMessagesReceived(messages); + MessagesReceived?.Invoke(this, messages); + } + _messages.Add(e.Message.MessageType, e.Message); + } + + /// + /// Called when a group of messages have been received. + /// + protected virtual void OnMessagesReceived(NmeaMessage[] messages) + { + } + + /// + /// Raised when a group of messages have been received. + /// + public event EventHandler MessagesReceived; + + /// + /// The device that's being listened to. + /// + public NmeaDevice Device { get; } + } +}