From e1c13ba49267674b298c2e232eaf81269adc0d18 Mon Sep 17 00:00:00 2001 From: kimikage Date: Wed, 22 Dec 2010 17:32:38 +0900 Subject: [PATCH] =?utf8?q?Ubx=E3=83=91=E3=82=B1=E3=83=83=E3=83=88=E3=81=AE?= =?utf8?q?=E9=83=A8=E5=88=86=E7=9A=84=E5=AE=9F=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- Yubeshi/Coordinate.cs | 17 ++++++ Yubeshi/Nmea/GpDtm.cs | 36 +++++++++++- Yubeshi/Nmea/GpGbs.cs | 2 +- Yubeshi/Nmea/GpGga.cs | 40 +++++++++----- Yubeshi/Nmea/GpGll.cs | 49 ++++++++++++++++- Yubeshi/Nmea/GpGrs.cs | 62 ++++++++++++++++++++- Yubeshi/Nmea/GpGsa.cs | 55 ++++++++++++++++++- Yubeshi/Nmea/Packet.cs | 28 ++++++++++ Yubeshi/Nmea/Parser.cs | 47 +++++++++++++++- Yubeshi/Ubx/NavPosEcef.cs | 77 ++++++++++++++++++++++++++ Yubeshi/Ubx/Packet.cs | 113 +++++++++++++++++++++++++++++++++++++- Yubeshi/Yubeshi.csproj | 1 + YubeshiTest/Nmea/SamplePackets.cs | 9 ++- YubeshiTest/Ubx/NavPacketTest.cs | 29 ++++++++++ YubeshiTest/Ubx/SamplePackets.cs | 38 +++++++++++++ YubeshiTest/YubeshiTest.csproj | 4 ++ 16 files changed, 583 insertions(+), 24 deletions(-) create mode 100755 Yubeshi/Ubx/NavPosEcef.cs create mode 100755 YubeshiTest/Ubx/NavPacketTest.cs create mode 100755 YubeshiTest/Ubx/SamplePackets.cs diff --git a/Yubeshi/Coordinate.cs b/Yubeshi/Coordinate.cs index 7706363..255268d 100755 --- a/Yubeshi/Coordinate.cs +++ b/Yubeshi/Coordinate.cs @@ -22,19 +22,36 @@ namespace Yubeshi decimal lng = Math.Floor(lngRaw / 100m) + (lngRaw % 100m) / 60m; Latitude = (ns == "S") ? -lat : lat; Longitude = (ns == "W") ? -lng : lng; + Altitude = 0; } #region properties + + /// + /// in Degree + /// public decimal Latitude { get; set; } + /// + /// in Degree + /// public decimal Longitude { get; set; } + + /// + /// in metre + /// + public decimal Altitude + { + get; + set; + } #endregion } } diff --git a/Yubeshi/Nmea/GpDtm.cs b/Yubeshi/Nmea/GpDtm.cs index 0c345d0..95f06d3 100755 --- a/Yubeshi/Nmea/GpDtm.cs +++ b/Yubeshi/Nmea/GpDtm.cs @@ -11,10 +11,24 @@ using System.Text; namespace Yubeshi.Nmea { + /// + /// Datum Reference + /// public class GpDtm : Packet { - #region fields + #region type definitions + public enum Code + { + Unknown = 0, + Wgs84 = 84, + Wgs72 = 72, + UserDefined = 999, + } + #endregion + + #region fields + private static Dictionary codes; private static readonly byte[] header; private const int elementNum = 8; @@ -25,6 +39,10 @@ namespace Yubeshi.Nmea static GpDtm() { header = Encoding.ASCII.GetBytes("$GPDTM,"); + codes = new Dictionary(); + codes["W84"] = Code.Wgs84; + codes["W72"] = Code.Wgs72; + codes["999"] = Code.UserDefined; } public GpDtm() @@ -44,7 +62,23 @@ namespace Yubeshi.Nmea #endregion #region properties + public Code LocalDatumCode + { + get; + private set; + } + + public Coordinate Offset + { + get; + private set; + } + public Code ReferenceDetumCode + { + get; + private set; + } #endregion #region public method diff --git a/Yubeshi/Nmea/GpGbs.cs b/Yubeshi/Nmea/GpGbs.cs index c479ec3..d431f59 100755 --- a/Yubeshi/Nmea/GpGbs.cs +++ b/Yubeshi/Nmea/GpGbs.cs @@ -12,7 +12,7 @@ using System.Text; namespace Yubeshi.Nmea { /// - /// + /// GNSS Satellite Fault Detection /// public class GpGbs : Packet { diff --git a/Yubeshi/Nmea/GpGga.cs b/Yubeshi/Nmea/GpGga.cs index e8ac042..8505bbc 100755 --- a/Yubeshi/Nmea/GpGga.cs +++ b/Yubeshi/Nmea/GpGga.cs @@ -55,13 +55,15 @@ namespace Yubeshi.Nmea { Raw = new byte[elements.PacketLength]; Array.Copy(sentence, Raw, elements.PacketLength); - - TimeOfFix = ParseTime(elements.Values[0]); - Position = new Coordinate(elements.Values[1], elements.Values[2], - elements.Values[3], elements.Values[4]); - FixQuality = (FixQualityClass)Int32.Parse(elements.Values[5]); - TrackedSatellites = Int32.Parse(elements.Values[6]); - Dilution = Decimal.Parse(elements.Values[7]); + string[] v = elements.Values; + TimeOfFix = ParseTime(v[0]); + Position = new Coordinate(v[1], v[2], v[3], v[4]); + FixQuality = (FixQualityClass)Int32.Parse(v[5]); + TrackedSatellites = Int32.Parse(v[6]); + Dilution = Decimal.Parse(v[7]); + MslAltitude = GetLength(v[8], v[9]); + GeoidSeparation = GetLength(v[10], v[11]); + CheckSum = elements.CheckSum; } @@ -99,7 +101,23 @@ namespace Yubeshi.Nmea private set; } + public decimal MslAltitude + { + get; + private set; + } + public decimal GeoidSeparation + { + get; + private set; + } + + public int StationID + { + get; + private set; + } #endregion @@ -113,13 +131,5 @@ namespace Yubeshi.Nmea return new GpGga(sentence, elements); } - private TimeSpan ParseTime(string time) - { - string h = time.Substring(0, 2); - string m = time.Substring(2, 2); - string s = time.Substring(4, 2); - return new TimeSpan(Int32.Parse(h), Int32.Parse(m), Int32.Parse(s)); - } - } } \ No newline at end of file diff --git a/Yubeshi/Nmea/GpGll.cs b/Yubeshi/Nmea/GpGll.cs index c25672c..b24aa94 100755 --- a/Yubeshi/Nmea/GpGll.cs +++ b/Yubeshi/Nmea/GpGll.cs @@ -12,9 +12,56 @@ using System.Text; namespace Yubeshi.Nmea { /// - /// + /// Latitude and Longitude, with Time of Position Fix and Status /// public class GpGll : Packet { + #region fields + + private static readonly byte[] header; + private const int elementNum = 7; + + #endregion + + #region constructors + + static GpGll() + { + header = Encoding.ASCII.GetBytes("$GPGLL,"); + } + + public GpGll() + { + } + + public GpGll(byte[] sentence) + : this(sentence, GetElements(sentence, elementNum)) + { + } + + private GpGll(byte[] sentence, Elements elements) + { + + } + + #endregion + + #region public method + + public static bool TryParse(byte[] sentence, out Packet packet) + { + return TryParse(sentence, out packet, header, elementNum, Build); + } + + #endregion + + #region private method + + private static Packet Build(byte[] sentence, Elements elements) + { + return new GpGll(sentence, elements); + } + + #endregion } } \ No newline at end of file diff --git a/Yubeshi/Nmea/GpGrs.cs b/Yubeshi/Nmea/GpGrs.cs index cca3a2a..43ef94d 100755 --- a/Yubeshi/Nmea/GpGrs.cs +++ b/Yubeshi/Nmea/GpGrs.cs @@ -12,9 +12,69 @@ using System.Text; namespace Yubeshi.Nmea { /// - /// + /// GNSS Range Residuals /// public class GpGrs : Packet { + #region fields + + private static readonly byte[] header; + private const int elementNum = 14; + + #endregion + + #region constructors + + static GpGrs() + { + header = Encoding.ASCII.GetBytes("$GPGRS,"); + } + + public GpGrs() + { + } + + public GpGrs(byte[] sentence) + : this(sentence, GetElements(sentence, elementNum)) + { + } + + private GpGrs(byte[] sentence, Elements elements) + { + Raw = new byte[elements.PacketLength]; + Array.Copy(sentence, Raw, elements.PacketLength); + string[] v = elements.Values; + TimeOfFix = ParseTime(v[0]); + } + + #endregion + + #region properties + + public TimeSpan TimeOfFix + { + get; + private set; + } + + #endregion + + #region public method + + public static bool TryParse(byte[] sentence, out Packet packet) + { + return TryParse(sentence, out packet, header, elementNum, Build); + } + + #endregion + + #region private method + + private static Packet Build(byte[] sentence, Elements elements) + { + return new GpGrs(sentence, elements); + } + + #endregion } } \ No newline at end of file diff --git a/Yubeshi/Nmea/GpGsa.cs b/Yubeshi/Nmea/GpGsa.cs index 92cffcd..dd1bf39 100755 --- a/Yubeshi/Nmea/GpGsa.cs +++ b/Yubeshi/Nmea/GpGsa.cs @@ -12,9 +12,62 @@ using System.Text; namespace Yubeshi.Nmea { /// - /// + /// GNSS DOP and Active Satellites /// public class GpGsa : Packet { + #region fields + + private static readonly byte[] header; + private const int elementNum = 17; + + #endregion + + #region constructors + + static GpGsa() + { + header = Encoding.ASCII.GetBytes("$GPGSA,"); + } + + public GpGsa() + { + } + + public GpGsa(byte[] sentence) + : this(sentence, GetElements(sentence, elementNum)) + { + } + + private GpGsa(byte[] sentence, Elements elements) + { + Raw = new byte[elements.PacketLength]; + Array.Copy(sentence, Raw, elements.PacketLength); + string[] v = elements.Values; + } + + #endregion + + #region properties + + #endregion + + #region public method + + public static bool TryParse(byte[] sentence, out Packet packet) + { + return TryParse(sentence, out packet, header, elementNum, Build); + } + + #endregion + + #region private method + + private static Packet Build(byte[] sentence, Elements elements) + { + return new GpGsa(sentence, elements); + } + + #endregion } } \ No newline at end of file diff --git a/Yubeshi/Nmea/Packet.cs b/Yubeshi/Nmea/Packet.cs index fe9aadf..d6a07c1 100755 --- a/Yubeshi/Nmea/Packet.cs +++ b/Yubeshi/Nmea/Packet.cs @@ -75,6 +75,7 @@ namespace Yubeshi.Nmea } catch { + packet = null; return false; } return true; @@ -144,6 +145,33 @@ namespace Yubeshi.Nmea } return true; } + protected TimeSpan ParseTime(string time) + { + int h = Int32.Parse(time.Substring(0, 2)); + int m = Int32.Parse(time.Substring(2, 2)); + decimal ds = Decimal.Parse(time.Substring(4)); + int sec = (int)Math.Floor(ds); + int msec = (int)((ds - sec) * 1000m); + return new TimeSpan(0, h, m, sec, msec); + } + + protected static decimal GetLength(string value, string unit) + { + decimal v; + + if (!Decimal.TryParse(value, out v)) + { + return -Decimal.MaxValue;; + } + if (unit == "M") + { + return v; + } + else + { + return -Decimal.MaxValue; ; + } + } #endregion } } diff --git a/Yubeshi/Nmea/Parser.cs b/Yubeshi/Nmea/Parser.cs index 3a864f2..013f853 100755 --- a/Yubeshi/Nmea/Parser.cs +++ b/Yubeshi/Nmea/Parser.cs @@ -13,13 +13,56 @@ namespace Yubeshi.Nmea { public class Parser : Yubeshi.Parser { + #region type definitions + delegate bool TrialParser(byte[] sentence, out Packet packet); + #endregion + + #region fields + + private static Dictionary gpParsers; + private static Encoding ascii = Encoding.ASCII; + + #endregion + + #region constructors + static Parser() + { + gpParsers = new Dictionary(); + gpParsers["DTM"] = GpDtm.TryParse; + //gpParsers["GBS"] = GpGbs.TryParse; + gpParsers["GGA"] = GpGga.TryParse; + gpParsers["GLL"] = GpGll.TryParse; + gpParsers["GRS"] = GpGrs.TryParse; + gpParsers["GSA"] = GpGsa.TryParse; + } + #endregion + #region public method - public bool TryParse(byte[] sentence, out Packet packet) + public static bool TryParse(byte[] sentence, out Packet packet) { - packet = new GpGga(); + packet = null; + // $XXXX,*CCcl + if (sentence.Length < 11 || sentence[0] != '$') + { + return false; + } + if (sentence[1] == 'G') + { + if (sentence[2] == 'P') // GPS + { + string format = ascii.GetString(sentence, 3, 3); + if (gpParsers.ContainsKey(format)) + { + return gpParsers[format](sentence, out packet); + } + return false; + } + } + return false; } + #endregion } } diff --git a/Yubeshi/Ubx/NavPosEcef.cs b/Yubeshi/Ubx/NavPosEcef.cs new file mode 100755 index 0000000..d4823a6 --- /dev/null +++ b/Yubeshi/Ubx/NavPosEcef.cs @@ -0,0 +1,77 @@ +/* + * Yubeshi GPS Parser + * + * This software is distributed under a zlib-style license. + * See license.txt for more information. + */ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace Yubeshi.Ubx +{ + public class NavPosEcef : Packet + { + #region constructors + + public NavPosEcef(byte[] sentence, int length) + { + Raw = new byte[length + 8]; + Array.Copy(sentence, Raw, Raw.Length); + + uint tow = BitConverter.ToUInt32(sentence, 6 + 0); + X = 0.01m * BitConverter.ToInt32(sentence, 6 + 4); + Y = 0.01m * BitConverter.ToInt32(sentence, 6 + 8); + Z = 0.01m * BitConverter.ToInt32(sentence, 6 + 12); + } + + #endregion + + #region properties + + public TimeSpan TimeOfWeek + { + get; + private set; + } + + public decimal X + { + get; + private set; + } + + public decimal Y + { + get; + private set; + } + + public decimal Z + { + get; + private set; + } + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out Packet packet) + { + return TryParse(sentence, out packet, + MessageID.NavPosEcef, 20, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavPosEcef(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/Packet.cs b/Yubeshi/Ubx/Packet.cs index cd3ae51..6573e6d 100755 --- a/Yubeshi/Ubx/Packet.cs +++ b/Yubeshi/Ubx/Packet.cs @@ -11,7 +11,118 @@ using System.Text; namespace Yubeshi.Ubx { - public class Packet : UnknownPacket + public abstract class Packet : UnknownPacket { + #region type definitions + + protected delegate Packet PacketBuilder(byte[] sentence, int length); + + public enum MessageClass + { + /// + /// Navigation + /// + Nav = 0x01, + /// + /// Reciever Manager + /// + Rxm = 0x02, + Inf = 0x04, + Ack = 0x05, + Cfg = 0x06, + Mon = 0x0A, + Aid = 0x0B, + Tim = 0x0D, + } + + public enum MessageID + { + NavPosEcef = 0x0101, + NavPosLlh = 0x0102, + NavStatus = 0x0103, + NavDop = 0x0104, + NavSol = 0x0105, + NavVelEcef = 0x0111, + NavVelNed = 0x0112, + NavTimeGps = 0x0120, + NavTimeUtc = 0x0121, + NavClock = 0x0122, + NavSvInfo = 0x0130, + NavSbas = 0x0132, + } + + #endregion + + + #region constructors + + #endregion + + #region properties + + public MessageClass Class + { + get; + protected set; + } + + public MessageID ID + { + get; + protected set; + } + + public int CheckSum + { + get; + protected set; + } + + #endregion + + #region protected methods + + protected static bool TryParse(byte[] sentence, out Packet packet, + MessageID id, int fixedLength, PacketBuilder builder) + { + packet = null; + if (sentence.Length < 8) + { + return false; + } + if (sentence[0] != 0xB5 || sentence[1] != 0x62) + { + return false; + } + int messageId = sentence[2] << 8 | sentence[3]; + if ((int)id != messageId) + { + return false; + } + int length = sentence[5] << 8 | sentence[4]; + if (length > 0 && fixedLength != length) + { + return false; + } + if (sentence.Length < length + 8) + { + return false; + } + try + { + packet = builder(sentence, length); + } + catch + { + packet = null; + return false; + } + return true; + } + + + #endregion + + } } diff --git a/Yubeshi/Yubeshi.csproj b/Yubeshi/Yubeshi.csproj index 332fa7c..368e913 100755 --- a/Yubeshi/Yubeshi.csproj +++ b/Yubeshi/Yubeshi.csproj @@ -77,6 +77,7 @@ + diff --git a/YubeshiTest/Nmea/SamplePackets.cs b/YubeshiTest/Nmea/SamplePackets.cs index 4733902..2dc058e 100755 --- a/YubeshiTest/Nmea/SamplePackets.cs +++ b/YubeshiTest/Nmea/SamplePackets.cs @@ -1,4 +1,11 @@ -using System; +/* + * Yubeshi GPS Parser + * + * This software is distributed under a zlib-style license. + * See license.txt for more information. + */ + +using System; using System.Collections.Generic; using System.Text; diff --git a/YubeshiTest/Ubx/NavPacketTest.cs b/YubeshiTest/Ubx/NavPacketTest.cs new file mode 100755 index 0000000..3899fab --- /dev/null +++ b/YubeshiTest/Ubx/NavPacketTest.cs @@ -0,0 +1,29 @@ +/* + * Yubeshi GPS Parser + * + * This software is distributed under a zlib-style license. + * See license.txt for more information. + */ + +using System; +using System.Collections.Generic; +using System.Text; +using NUnit.Framework; +using Yubeshi.Ubx; +using P = YubeshiTest.Ubx.SamplePackets; + +namespace YubeshiTest.Ubx +{ + + class NavPacketTest + { + + [Test] + public void NavPosEcefTest() + { + Packet p; + Assert.AreEqual(true, NavPosEcef.TryParse(P.NavPosEcef, out p)); + } + + } +} diff --git a/YubeshiTest/Ubx/SamplePackets.cs b/YubeshiTest/Ubx/SamplePackets.cs new file mode 100755 index 0000000..4b5588d --- /dev/null +++ b/YubeshiTest/Ubx/SamplePackets.cs @@ -0,0 +1,38 @@ +/* + * Yubeshi GPS Parser + * + * This software is distributed under a zlib-style license. + * See license.txt for more information. + */ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace YubeshiTest.Ubx +{ + public class SamplePackets + { + public static readonly byte[] Dummy; + public static readonly byte[] NavPosEcef; + + + static SamplePackets() + { + Encoding ascii = Encoding.ASCII; + + Dummy = new byte[] { }; + NavPosEcef = new byte[]{ + 0xB5, 0x62, 0x01, 0x01, 0x14, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0xFF, 0xFF + }; + + + } + } +} diff --git a/YubeshiTest/YubeshiTest.csproj b/YubeshiTest/YubeshiTest.csproj index 58202f7..7a19777 100755 --- a/YubeshiTest/YubeshiTest.csproj +++ b/YubeshiTest/YubeshiTest.csproj @@ -59,8 +59,12 @@ + + + + -- 2.11.0