From d8beb3010361e21c35f2c720ffb79ea8d0777e6e Mon Sep 17 00:00:00 2001 From: kimikage Date: Fri, 28 Jan 2011 17:58:25 +0900 Subject: [PATCH] =?utf8?q?NAV=E3=83=91=E3=82=B1=E3=83=83=E3=83=88=E3=81=AE?= =?utf8?q?=E6=95=B4=E7=90=86=E3=83=BB=E3=82=B9=E3=82=BF=E3=83=96=E4=BD=9C?= =?utf8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- Yubeshi/EcefVelocity.cs | 42 +++++++++---- Yubeshi/EnuVelocity.cs | 66 +++++++++++++++++++++ Yubeshi/GeodeticCoordinate.cs | 3 +- Yubeshi/Ubx/NavClock.cs | 36 ++++++----- Yubeshi/Ubx/NavDop.cs | 112 +++++++++++++++++++++++++++++++++++ Yubeshi/Ubx/NavPosEcef.cs | 24 ++++---- Yubeshi/Ubx/NavPosLlh.cs | 91 ++++++++++++++++++++++++++++ Yubeshi/Ubx/NavSbas.cs | 70 ++++++++++++++++++++++ Yubeshi/Ubx/NavSol.cs | 63 +++++++++++--------- Yubeshi/Ubx/NavStatus.cs | 1 - Yubeshi/Ubx/NavSvInfo.cs | 70 ++++++++++++++++++++++ Yubeshi/Ubx/NavTimeGps.cs | 83 ++++++++++++++++++++++++++ Yubeshi/Ubx/NavTimeUtc.cs | 84 ++++++++++++++++++++++++++ Yubeshi/Ubx/NavVelEcef.cs | 69 +++++++++++++++++++++ Yubeshi/Ubx/NavVelNed.cs | 89 ++++++++++++++++++++++++++++ Yubeshi/Ubx/Packet.cs | 2 + Yubeshi/Yubeshi.csproj | 9 +++ YubeshiTest/UbxTest/MyPacket.cs | 2 +- YubeshiTest/UbxTest/NavPacketTest.cs | 23 ++++++- YubeshiTest/UbxTest/SamplePackets.cs | 22 +++++-- 20 files changed, 884 insertions(+), 77 deletions(-) create mode 100755 Yubeshi/EnuVelocity.cs create mode 100755 Yubeshi/Ubx/NavDop.cs create mode 100755 Yubeshi/Ubx/NavPosLlh.cs create mode 100755 Yubeshi/Ubx/NavSbas.cs create mode 100755 Yubeshi/Ubx/NavSvInfo.cs create mode 100755 Yubeshi/Ubx/NavTimeGps.cs create mode 100755 Yubeshi/Ubx/NavTimeUtc.cs create mode 100755 Yubeshi/Ubx/NavVelEcef.cs create mode 100755 Yubeshi/Ubx/NavVelNed.cs diff --git a/Yubeshi/EcefVelocity.cs b/Yubeshi/EcefVelocity.cs index 49cbb00..5f3a7f8 100755 --- a/Yubeshi/EcefVelocity.cs +++ b/Yubeshi/EcefVelocity.cs @@ -10,33 +10,45 @@ namespace Yubeshi /// /// /// - /// ECEF X velocity in cm/s - /// ECEF Y velocity in cm/s - /// ECEF Z velocity in cm/s + /// ENU E velocity in m/s + /// ENU N velocity in m/s + /// ENU U velocity in m/s + /// Speed Accuracy Estimate in m/s + public EcefVelocity(double e, double n, double u, double accuracy) + { + E = e; + N = n; + U = u; + Accuracy = accuracy; + } + + /// + /// + /// + /// ENU E velocity in cm/s + /// ENU N velocity in cm/s + /// ENU U velocity in cm/s /// Speed Accuracy Estimate in cm/s - public EcefVelocity(int x, int y, int z, uint accuracy) + public EcefVelocity(int e, int n, int u, uint accuracy) + : this(e * 0.01, n * 0.01, u * 0.01, accuracy * 0.01) { - X = x * 0.01; - Y = y * 0.01; - Z = z * 0.01; - Accuracy = accuracy * 0.01; } #endregion #region properties - public double X + public double E { get; set; } - public double Y + public double N { get; set; } - public double Z + public double U { get; set; @@ -47,6 +59,14 @@ namespace Yubeshi get; set; } + + public double Speed + { + get + { + return Math.Sqrt(E * E + N * N + U * U); + } + } #endregion } } diff --git a/Yubeshi/EnuVelocity.cs b/Yubeshi/EnuVelocity.cs new file mode 100755 index 0000000..8f3330f --- /dev/null +++ b/Yubeshi/EnuVelocity.cs @@ -0,0 +1,66 @@ +/* + * 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 +{ + public class EnuVelocity + { + #region fields + + #endregion + + #region constructors + + public EnuVelocity(double e, double n, double u) + { + E = e; + N = n; + U = u; + Origin = null; + } + + public EnuVelocity(int e, int n, int u) + : this(e * 0.01, n * 0.01, u * 0.01) + { + } + + #endregion + + #region properties + + public double E + { + get; + set; + } + + public double N + { + get; + set; + } + + public double U + { + get; + set; + } + + public EcefCoordinate Origin + { + get; + private set; + } + + #endregion + + } +} diff --git a/Yubeshi/GeodeticCoordinate.cs b/Yubeshi/GeodeticCoordinate.cs index fdd0a1b..7eaabb0 100755 --- a/Yubeshi/GeodeticCoordinate.cs +++ b/Yubeshi/GeodeticCoordinate.cs @@ -27,7 +27,8 @@ namespace Yubeshi { } - public GeodeticCoordinate(double latitude, double longitude, double altitude) + public GeodeticCoordinate( + double latitude, double longitude, double altitude) { Latitude = latitude; Longitude = longitude; diff --git a/Yubeshi/Ubx/NavClock.cs b/Yubeshi/Ubx/NavClock.cs index f577050..333cc3d 100755 --- a/Yubeshi/Ubx/NavClock.cs +++ b/Yubeshi/Ubx/NavClock.cs @@ -19,12 +19,6 @@ namespace Yubeshi.Ubx : base(sentence, length) { ID = MessageID.NavClock; - - TimeOfWeek = 1e-3m * BitConverter.ToUInt32(sentence, 6 + 0); - ClockBias = 1e-9m * BitConverter.ToInt32(sentence, 6 + 4); - ClockDrift = 1e-9m * BitConverter.ToInt32(sentence, 6 + 8); - TimeAccuracy = 1e-9m * BitConverter.ToUInt32(sentence, 6 + 12); - FrequencyAccuracy = 1e-6m * BitConverter.ToUInt32(sentence, 6 + 16); } #endregion @@ -33,32 +27,42 @@ namespace Yubeshi.Ubx public decimal TimeOfWeek { - get; - private set; + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } } public decimal ClockBias { - get; - private set; + get + { + return BitConverter.ToInt32(Raw, 6 + 4) * 1e-9m; + } } public decimal ClockDrift { - get; - private set; + get + { + return BitConverter.ToInt32(Raw, 6 + 8) * 1e-9m; + } } public decimal TimeAccuracy { - get; - private set; + get + { + return BitConverter.ToUInt32(Raw, 6 + 12) * 1e-9m; + } } public decimal FrequencyAccuracy { - get; - private set; + get + { + return BitConverter.ToUInt32(Raw, 6 + 16) * 1e-6m; + } } #endregion diff --git a/Yubeshi/Ubx/NavDop.cs b/Yubeshi/Ubx/NavDop.cs new file mode 100755 index 0000000..ea65026 --- /dev/null +++ b/Yubeshi/Ubx/NavDop.cs @@ -0,0 +1,112 @@ +/* + * 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 NavDop : Packet + { + #region constructors + + public NavDop(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavDop; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + + public double Geometric + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 4) * 1e-2; + } + } + + public double Position + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 6) * 1e-2; + } + } + + public double Time + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 8) * 1e-2; + } + } + + public double Vertical + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 10) * 1e-2; + } + } + + public double Horizonal + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 12) * 1e-2; + } + } + + public double Noarthing + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 14) * 1e-2; + } + } + + public double Easting + { + get + { + return BitConverter.ToUInt16(Raw, 6 + 16) * 1e-2; + } + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + return TryParse(sentence, out packet, MessageID.NavDop, 18, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavDop(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavPosEcef.cs b/Yubeshi/Ubx/NavPosEcef.cs index 7111493..333c37a 100755 --- a/Yubeshi/Ubx/NavPosEcef.cs +++ b/Yubeshi/Ubx/NavPosEcef.cs @@ -19,14 +19,6 @@ namespace Yubeshi.Ubx : base(sentence, length) { ID = MessageID.NavPosEcef; - - uint tow = BitConverter.ToUInt32(sentence, 6 + 0); - TimeOfWeek = tow * 1e-3m; - int x = BitConverter.ToInt32(sentence, 6 + 4); - int y = BitConverter.ToInt32(sentence, 6 + 8); - int z = BitConverter.ToInt32(sentence, 6 + 12); - uint acc = BitConverter.ToUInt32(sentence, 6 + 16); - Position = new EcefCoordinate(x, y, z, acc); } #endregion @@ -35,14 +27,22 @@ namespace Yubeshi.Ubx public decimal TimeOfWeek { - get; - private set; + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } } public EcefCoordinate Position { - get; - private set; + get + { + int x = BitConverter.ToInt32(Raw, 6 + 4); + int y = BitConverter.ToInt32(Raw, 6 + 8); + int z = BitConverter.ToInt32(Raw, 6 + 12); + uint acc = BitConverter.ToUInt32(Raw, 6 + 16); + return new EcefCoordinate(x, y, z, acc); + } } #endregion diff --git a/Yubeshi/Ubx/NavPosLlh.cs b/Yubeshi/Ubx/NavPosLlh.cs new file mode 100755 index 0000000..ee25cc2 --- /dev/null +++ b/Yubeshi/Ubx/NavPosLlh.cs @@ -0,0 +1,91 @@ +/* + * 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 NavPosLlh : Packet + { + #region constructors + + public NavPosLlh(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavPosLlh; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + + public GeodeticCoordinate Position + { + get + { + Degree lon = BitConverter.ToInt32(Raw, 6 + 4) * 1e-7; + Degree lat = BitConverter.ToInt32(Raw, 6 + 8) * 1e-7; + double h = BitConverter.ToUInt32(Raw, 6 + 12); + return new GeodeticCoordinate(lon, lat, h); + } + } + + public double MslHeight + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 16); + } + } + + public double HorizonalAccuracy + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 20) * 1e-3; + } + } + + public double VerticalAccuracy + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 24) * 1e-3; + } + } + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + return TryParse(sentence, out packet, + MessageID.NavPosLlh, 28, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavPosLlh(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavSbas.cs b/Yubeshi/Ubx/NavSbas.cs new file mode 100755 index 0000000..6ac7690 --- /dev/null +++ b/Yubeshi/Ubx/NavSbas.cs @@ -0,0 +1,70 @@ +/* + * 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 NavSbas : Packet + { + #region type definitions + public struct SbasStatus + { + private byte[] Raw; + } + #endregion + + #region constructors + + public NavSbas(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavSbas; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + packet = null; + if (sentence.Length < 12) + { + return false; + } + int length = 12 + 12 * sentence[6 + 8]; + return TryParse(sentence, out packet, + MessageID.NavSbas, length, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavSbas(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavSol.cs b/Yubeshi/Ubx/NavSol.cs index 3f09b39..e61e805 100755 --- a/Yubeshi/Ubx/NavSol.cs +++ b/Yubeshi/Ubx/NavSol.cs @@ -19,23 +19,6 @@ namespace Yubeshi.Ubx : base(sentence, length) { ID = MessageID.NavSol; - - uint tow = BitConverter.ToUInt32(sentence, 6 + 0); - int ftow = BitConverter.ToInt32(sentence, 6 + 4); - TimeOfWeek = tow * 1e-3m + ftow * 1e-9m; - Week = BitConverter.ToInt16(sentence, 6 + 8); - PositionFixType = (GpsFixType)sentence[6 + 10]; - FixFlags = (FixStatusFlag)sentence[6 + 11]; - int x = BitConverter.ToInt32(sentence, 6 + 12); - int y = BitConverter.ToInt32(sentence, 6 + 16); - int z = BitConverter.ToInt32(sentence, 6 + 20); - uint acc = BitConverter.ToUInt32(sentence, 6 + 24); - Position = new EcefCoordinate(x, y, z, acc); - int vx = BitConverter.ToInt32(sentence, 6 + 28); - int vy = BitConverter.ToInt32(sentence, 6 + 32); - int vz = BitConverter.ToInt32(sentence, 6 + 36); - uint vAcc = BitConverter.ToUInt32(sentence, 6 + 40); - Velocity = new EcefVelocity(vx, vy, vz, vAcc); } #endregion @@ -44,38 +27,60 @@ namespace Yubeshi.Ubx public decimal TimeOfWeek { - get; - private set; + get + { + uint tow = BitConverter.ToUInt32(Raw, 6 + 0); + int ftow = BitConverter.ToInt32(Raw, 6 + 4); + return tow * 1e-3m + ftow * 1e-9m; + } } public int Week { - get; - private set; + get + { + return BitConverter.ToInt16(Raw, 6 + 8); + } } public GpsFixType PositionFixType { - get; - private set; + get + { + return (GpsFixType)Raw[6 + 10]; + } } public FixStatusFlag FixFlags { - get; - private set; + get + { + return (FixStatusFlag)Raw[6 + 11]; + } } public EcefCoordinate Position { - get; - private set; + get + { + int x = BitConverter.ToInt32(Raw, 6 + 12); + int y = BitConverter.ToInt32(Raw, 6 + 16); + int z = BitConverter.ToInt32(Raw, 6 + 20); + uint acc = BitConverter.ToUInt32(Raw, 6 + 24); + return new EcefCoordinate(x, y, z, acc); + } } public EcefVelocity Velocity { - get; - private set; + get + { + int vx = BitConverter.ToInt32(Raw, 6 + 28); + int vy = BitConverter.ToInt32(Raw, 6 + 32); + int vz = BitConverter.ToInt32(Raw, 6 + 36); + uint vAcc = BitConverter.ToUInt32(Raw, 6 + 40); + return new EcefVelocity(vx, vy, vz, vAcc); + } } public int SVsUsed diff --git a/Yubeshi/Ubx/NavStatus.cs b/Yubeshi/Ubx/NavStatus.cs index cbb30a5..d95e7aa 100755 --- a/Yubeshi/Ubx/NavStatus.cs +++ b/Yubeshi/Ubx/NavStatus.cs @@ -33,7 +33,6 @@ namespace Yubeshi.Ubx : base(sentence, length) { ID = MessageID.NavStatus; - } #endregion diff --git a/Yubeshi/Ubx/NavSvInfo.cs b/Yubeshi/Ubx/NavSvInfo.cs new file mode 100755 index 0000000..b0f0443 --- /dev/null +++ b/Yubeshi/Ubx/NavSvInfo.cs @@ -0,0 +1,70 @@ +/* + * 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 NavSvInfo : Packet + { + #region type definitions + public struct SvInfo + { + private byte[] Raw; + } + #endregion + + #region constructors + + public NavSvInfo(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavSvInfo; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + packet = null; + if (sentence.Length < 8) + { + return false; + } + int length = 8 + 12 * sentence[6 + 4]; + return TryParse(sentence, out packet, + MessageID.NavSvInfo, length, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavSvInfo(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavTimeGps.cs b/Yubeshi/Ubx/NavTimeGps.cs new file mode 100755 index 0000000..b3bfc19 --- /dev/null +++ b/Yubeshi/Ubx/NavTimeGps.cs @@ -0,0 +1,83 @@ +/* + * 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 NavTimeGps : Packet + { + #region constructors + + public NavTimeGps(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavTimeGps; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + uint tow = BitConverter.ToUInt32(Raw, 6 + 0); + int ftow = BitConverter.ToInt32(Raw, 6 + 4); + return tow * 1e-3m + ftow * 1e-9m; + } + } + + public int Week + { + get + { + return BitConverter.ToInt16(Raw, 6 + 8); + } + } + + public int LeapSecond + { + get + { + return (sbyte)(Raw[6 + 10]); + } + } + + public decimal Accuracy + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 12) * 1e-9m; + } + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + return TryParse(sentence, out packet, + MessageID.NavTimeGps, 16, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavTimeGps(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavTimeUtc.cs b/Yubeshi/Ubx/NavTimeUtc.cs new file mode 100755 index 0000000..8e02e30 --- /dev/null +++ b/Yubeshi/Ubx/NavTimeUtc.cs @@ -0,0 +1,84 @@ +/* + * 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 NavTimeUtc : Packet + { + #region constructors + + public NavTimeUtc(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavTimeUtc; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + uint tow = BitConverter.ToUInt32(Raw, 6 + 0); + int ftow = BitConverter.ToInt32(Raw, 6 + 4); + return tow * 1e-3m + ftow * 1e-9m; + } + + } + + public int Week + { + get + { + return BitConverter.ToInt16(Raw, 6 + 8); + } + } + + public int LeapSecond + { + get + { + return (sbyte)(Raw[6 + 10]); + } + } + + public decimal Accuracy + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 12) * 1e-9m; + } + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + return TryParse(sentence, out packet, + MessageID.NavTimeGps, 16, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavTimeUtc(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavVelEcef.cs b/Yubeshi/Ubx/NavVelEcef.cs new file mode 100755 index 0000000..ded8091 --- /dev/null +++ b/Yubeshi/Ubx/NavVelEcef.cs @@ -0,0 +1,69 @@ +/* + * 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 NavVelEcef : Packet + { + #region constructors + + public NavVelEcef(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavPosEcef; + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + + public EcefVelocity Velocity + { + get + { + int x = BitConverter.ToInt32(Raw, 6 + 4); + int y = BitConverter.ToInt32(Raw, 6 + 8); + int z = BitConverter.ToInt32(Raw, 6 + 12); + uint acc = BitConverter.ToUInt32(Raw, 6 + 16); + return new EcefVelocity(x, y, z, acc); + } + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + return TryParse(sentence, out packet, + MessageID.NavVelEcef, 20, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavVelEcef(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/NavVelNed.cs b/Yubeshi/Ubx/NavVelNed.cs new file mode 100755 index 0000000..cc8b093 --- /dev/null +++ b/Yubeshi/Ubx/NavVelNed.cs @@ -0,0 +1,89 @@ +/* + * 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 NavVelNed : Packet + { + #region constructors + + public NavVelNed(byte[] sentence, int length) + : base(sentence, length) + { + ID = MessageID.NavVelNed; + + } + + #endregion + + #region properties + + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + + public EnuVelocity Velocity + { + get + { + int n = BitConverter.ToInt32(Raw, 6 + 4); + int e = BitConverter.ToInt32(Raw, 6 + 8); + int d = -BitConverter.ToInt32(Raw, 6 + 12); + return new EnuVelocity(e, n, -d); + } + } + + public double Speed + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 16); + } + } + + public double GroundSpeed + { + get; + private set; + } + + public Degree Heading + { + get; + private set; + } + + #endregion + + #region public methods + + public static bool TryParse(byte[] sentence, out UnknownPacket packet) + { + return TryParse(sentence, out packet, + MessageID.NavVelNed, 36, Build); + } + + #endregion + + #region private methods + + private static Packet Build(byte[] sentence, int length) + { + return new NavVelNed(sentence, length); + } + + #endregion + } +} diff --git a/Yubeshi/Ubx/Packet.cs b/Yubeshi/Ubx/Packet.cs index f22f1ae..4418ccd 100755 --- a/Yubeshi/Ubx/Packet.cs +++ b/Yubeshi/Ubx/Packet.cs @@ -69,6 +69,8 @@ namespace Yubeshi.Ubx #endregion #region fields + public static readonly byte[] SyncCharacters = + new byte[] { 0xB5, 0x62 }; public const byte SyncCharacter1 = 0xB5; public const byte SyncCharacter2 = 0x62; #endregion diff --git a/Yubeshi/Yubeshi.csproj b/Yubeshi/Yubeshi.csproj index 7dae067..91c888d 100755 --- a/Yubeshi/Yubeshi.csproj +++ b/Yubeshi/Yubeshi.csproj @@ -63,7 +63,16 @@ + + + + + + + + + diff --git a/YubeshiTest/UbxTest/MyPacket.cs b/YubeshiTest/UbxTest/MyPacket.cs index 14a459f..f2c4ee4 100755 --- a/YubeshiTest/UbxTest/MyPacket.cs +++ b/YubeshiTest/UbxTest/MyPacket.cs @@ -13,7 +13,7 @@ using Yubeshi.Ubx; namespace YubeshiTest.UbxTest { - class MyPacket : Unknown + class MyPacket : Packet { #region constructors diff --git a/YubeshiTest/UbxTest/NavPacketTest.cs b/YubeshiTest/UbxTest/NavPacketTest.cs index 4b66701..333ab8e 100755 --- a/YubeshiTest/UbxTest/NavPacketTest.cs +++ b/YubeshiTest/UbxTest/NavPacketTest.cs @@ -18,19 +18,36 @@ namespace YubeshiTest.UbxTest class NavPacketTest { + private readonly decimal timeOfWeek = + new TimeSpan(1, 23, 45, 6, 789).Ticks * 1e-7m; + private readonly decimal timeOfWeekHD = + new TimeSpan(1, 23, 45, 6, 789).Ticks * 1e-7m + 012345e-9m; + [Test] public void NavClockTest() { UnknownPacket packet; Assert.IsTrue(NavClock.TryParse(P.NavClock, out packet)); NavClock p = packet as NavClock; + Assert.AreEqual(Packet.MessageID.NavClock, p.ID); + } + + [Test] + public void NavDopTest() + { + UnknownPacket packet; + Assert.IsTrue(NavDop.TryParse(P.NavDop, out packet)); + NavDop p = packet as NavDop; + Assert.AreEqual(Packet.MessageID.NavDop, p.ID); } [Test] public void NavPosEcefTest() { - UnknownPacket p; - Assert.IsTrue(NavPosEcef.TryParse(P.NavPosEcef, out p)); + UnknownPacket packet; + Assert.IsTrue(NavPosEcef.TryParse(P.NavPosEcef, out packet)); + NavPosEcef p = packet as NavPosEcef; + Assert.AreEqual(Packet.MessageID.NavPosEcef, p.ID); } [Test] @@ -39,6 +56,8 @@ namespace YubeshiTest.UbxTest UnknownPacket packet; Assert.IsTrue(NavSol.TryParse(P.NavSol, out packet)); NavSol p = packet as NavSol; + Assert.AreEqual(Packet.MessageID.NavSol, p.ID); + Assert.AreEqual(timeOfWeekHD, p.TimeOfWeek); Assert.AreEqual(-3961181.33, p.Position.X); Assert.AreEqual(3346187.67, p.Position.Y); Assert.AreEqual(3702487.20, p.Position.Z); diff --git a/YubeshiTest/UbxTest/SamplePackets.cs b/YubeshiTest/UbxTest/SamplePackets.cs index 9740fe8..ca45687 100755 --- a/YubeshiTest/UbxTest/SamplePackets.cs +++ b/YubeshiTest/UbxTest/SamplePackets.cs @@ -17,6 +17,7 @@ namespace YubeshiTest.UbxTest public static readonly byte[] UserDefined; public static readonly byte[] NavClock; + public static readonly byte[] NavDop; public static readonly byte[] NavPosEcef; public static readonly byte[] NavSol; @@ -39,7 +40,7 @@ namespace YubeshiTest.UbxTest NavClock = new byte[]{ 0xB5, 0x62, 0x01, 0x22, 0x14, 0x00, - 0x70, 0xAA, 0xB4, 0x12, + 0xE5, 0x16, 0x3F, 0x0A, // tow 0x3C, 0xDA, 0x06, 0x00, 0x48, 0x01, 0x00, 0x00, 0x4E, 0x00, 0x00, 0x00, @@ -47,9 +48,22 @@ namespace YubeshiTest.UbxTest 0xFF, 0xFF }; + NavDop = new byte[]{ + 0xB5, 0x62, 0x01, 0x04, 0x12, 0x00, + 0xE5, 0x16, 0x3F, 0x0A, // tow + 0x00, 0x00, // geometric + 0x00, 0x00, // position + 0x00, 0x00, // time + 0x00, 0x00, // vertical + 0x00, 0x00, // horizonal + 0x00, 0x00, // northing + 0x00, 0x00, // easting + 0xFF, 0xFF + }; + NavPosEcef = new byte[]{ 0xB5, 0x62, 0x01, 0x01, 0x14, 0x00, - 0x70, 0xAA, 0xF1, 0x13, // tow + 0xE5, 0x16, 0x3F, 0x0A, // tow 0x8B, 0xB7, 0x63, 0xE8, // x 0x8F, 0xE0, 0xF1, 0x13, // y 0x10, 0x8C, 0x11, 0x16, // z @@ -59,8 +73,8 @@ namespace YubeshiTest.UbxTest NavSol = new byte[]{ 0xB5, 0x62, 0x01, 0x06, 0x34, 0x00, - 0x70, 0xAA, 0xF1, 0x13, // tow - 0xC2, 0x0B, 0xFD, 0xFF, + 0xE5, 0x16, 0x3F, 0x0A, // tow + 0x39, 0x30, 0x00, 0x00, // ftow 0x4E, 0x06, 0x02, 0x5D, -- 2.11.0