From d7bdae6dbdb5302593c916f5ea3d572cc74453aa Mon Sep 17 00:00:00 2001 From: kimikage Date: Thu, 24 Feb 2011 21:20:02 +0900 Subject: [PATCH] =?utf8?q?=E5=A2=83=E7=95=8C=E3=83=81=E3=82=A7=E3=83=83?= =?utf8?q?=E3=82=AF=E4=B8=8D=E5=85=B7=E5=90=88=E4=BF=AE=E6=AD=A3=EF=BC=8CU?= =?utf8?q?BX=E7=89=B9=E5=8C=96=E3=81=AE=E5=BA=A7=E6=A8=99=E3=82=B3?= =?utf8?q?=E3=83=B3=E3=82=B9=E3=83=88=E3=83=A9=E3=82=AF=E3=82=BF=E5=BB=83?= =?utf8?q?=E6=AD=A2=EF=BC=8C=E3=83=91=E3=82=B1=E3=83=83=E3=83=88=E5=AE=9F?= =?utf8?q?=E8=A3=85=E3=81=AE=E6=8B=A1=E5=85=85=E3=81=A8=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- Yubeshi/EcefCoordinate.cs | 12 --------- Yubeshi/EcefVelocity.cs | 11 -------- Yubeshi/EnuVelocity.cs | 5 ---- Yubeshi/Ubx/AidAlm.cs | 6 ++--- Yubeshi/Ubx/MonHw2.cs | 4 +-- Yubeshi/Ubx/NavDop.cs | 14 +++++----- Yubeshi/Ubx/NavPosEcef.cs | 4 +-- Yubeshi/Ubx/NavSbas.cs | 56 +++++++++++++++++++++++++++++++++++++- Yubeshi/Ubx/NavSol.cs | 6 ++--- Yubeshi/Ubx/NavStatus.cs | 68 ++++++++++++++++++++++++++++++++++++++++++----- Yubeshi/Ubx/NavSvInfo.cs | 6 ++--- Yubeshi/Ubx/NavTimeUtc.cs | 28 ++++++++++++++++--- Yubeshi/Ubx/NavVelEcef.cs | 4 +-- Yubeshi/Ubx/NavVelNed.cs | 32 +++++++++++++++++----- Yubeshi/Ubx/Parser.cs | 6 ++--- Yubeshi/Ubx/RxmAlm.cs | 6 ++--- Yubeshi/Ubx/RxmRaw.cs | 6 ++--- Yubeshi/Ubx/RxmSfrB.cs | 2 +- 18 files changed, 198 insertions(+), 78 deletions(-) diff --git a/Yubeshi/EcefCoordinate.cs b/Yubeshi/EcefCoordinate.cs index 6f6ed83..d53665f 100755 --- a/Yubeshi/EcefCoordinate.cs +++ b/Yubeshi/EcefCoordinate.cs @@ -35,18 +35,6 @@ namespace Yubeshi Z = z; Accuracy = accuracy; } - - /// - /// - /// - /// ECEF X coordinate in cm - /// ECEF Y coordinate in cm - /// ECEF Z coordinate in cm - /// 3D Position Accuracy Estimate in cm - public EcefCoordinate(int x, int y, int z, uint accuracy) - : this(x * 0.01, y * 0.01, z * 0.01, accuracy * 0.01) - { - } #endregion #region properties diff --git a/Yubeshi/EcefVelocity.cs b/Yubeshi/EcefVelocity.cs index 5f3a7f8..bcb810f 100755 --- a/Yubeshi/EcefVelocity.cs +++ b/Yubeshi/EcefVelocity.cs @@ -22,17 +22,6 @@ namespace Yubeshi 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 e, int n, int u, uint accuracy) - : this(e * 0.01, n * 0.01, u * 0.01, accuracy * 0.01) - { - } #endregion #region properties diff --git a/Yubeshi/EnuVelocity.cs b/Yubeshi/EnuVelocity.cs index 8f3330f..3cd33f9 100755 --- a/Yubeshi/EnuVelocity.cs +++ b/Yubeshi/EnuVelocity.cs @@ -27,11 +27,6 @@ namespace Yubeshi Origin = null; } - public EnuVelocity(int e, int n, int u) - : this(e * 0.01, n * 0.01, u * 0.01) - { - } - #endregion #region properties diff --git a/Yubeshi/Ubx/AidAlm.cs b/Yubeshi/Ubx/AidAlm.cs index 5efa063..458660f 100755 --- a/Yubeshi/Ubx/AidAlm.cs +++ b/Yubeshi/Ubx/AidAlm.cs @@ -20,14 +20,14 @@ namespace Yubeshi.Ubx { } - public AidAlm(uint svId) + public AidAlm(uint svID) : base( new byte[] { Packet.SyncCharacter1, Packet.SyncCharacter2, 0x0B, 0x30, // ID 0x01, 0x00, // length - (byte)svId, // svid + (byte)svID, // svid 0x00, 0x00 // checksum }) { @@ -53,7 +53,7 @@ namespace Yubeshi.Ubx #region properties - public uint SvId + public uint SvID { get { diff --git a/Yubeshi/Ubx/MonHw2.cs b/Yubeshi/Ubx/MonHw2.cs index 732563b..d3d419f 100755 --- a/Yubeshi/Ubx/MonHw2.cs +++ b/Yubeshi/Ubx/MonHw2.cs @@ -38,7 +38,7 @@ namespace Yubeshi.Ubx { get { - return (((int)Raw[6 + 0] << 24) >> 24) / 128.0; + return (sbyte)Raw[6 + 0] / 128.0; } } @@ -54,7 +54,7 @@ namespace Yubeshi.Ubx { get { - return (((int)Raw[6 + 2] << 24) >> 24) / 128.0; + return (sbyte)Raw[6 + 2] / 128.0; } } diff --git a/Yubeshi/Ubx/NavDop.cs b/Yubeshi/Ubx/NavDop.cs index 685c02b..e27911a 100755 --- a/Yubeshi/Ubx/NavDop.cs +++ b/Yubeshi/Ubx/NavDop.cs @@ -36,7 +36,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 4) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 4) * 0.01; } } @@ -44,7 +44,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 6) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 6) * 0.01; } } @@ -52,7 +52,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 8) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 8) * 0.01; } } @@ -60,7 +60,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 10) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 10) * 0.01; } } @@ -68,7 +68,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 12) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 12) * 0.01; } } @@ -76,7 +76,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 14) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 14) * 0.01; } } @@ -84,7 +84,7 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt16(Raw, 6 + 16) * 1e-2; + return BitConverter.ToUInt16(Raw, 6 + 16) * 0.01; } } diff --git a/Yubeshi/Ubx/NavPosEcef.cs b/Yubeshi/Ubx/NavPosEcef.cs index 3026f75..71b057e 100755 --- a/Yubeshi/Ubx/NavPosEcef.cs +++ b/Yubeshi/Ubx/NavPosEcef.cs @@ -39,8 +39,8 @@ namespace Yubeshi.Ubx 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); + double acc = BitConverter.ToUInt32(Raw, 6 + 16) * 0.01; + return new EcefCoordinate(x * 0.01, y * 0.01, z * 0.01, acc); } } diff --git a/Yubeshi/Ubx/NavSbas.cs b/Yubeshi/Ubx/NavSbas.cs index 3049f7e..7e65446 100755 --- a/Yubeshi/Ubx/NavSbas.cs +++ b/Yubeshi/Ubx/NavSbas.cs @@ -14,6 +14,23 @@ namespace Yubeshi.Ubx public class NavSbas : Packet { #region type definitions + + public enum SbasMode : byte + { + Disabled = 0, + IntegrityEnabled = 1, + TestModeEnabled = 3, + } + + public enum SbasSystem : sbyte + { + Unknown = -1, + Waas = 0, + Egnos = 1, + Msas = 2, + Gps = 16, + } + public struct SbasStatus { private byte[] Raw; @@ -39,6 +56,43 @@ namespace Yubeshi.Ubx } } + public int SbasID + { + get + { + return Raw[6 + 4]; + } + } + + public SbasMode Mode + { + get + { + return (SbasMode)Raw[6 + 5]; + } + } + + public SbasSystem System + { + get + { + return (SbasSystem)Raw[6 + 6]; + } + } + + public int NumberOfSvData + { + get + { + return Raw[6 + 8]; + } + } + + public SbasStatus[] Status + { + get; + private set; + } #endregion #region public methods @@ -46,7 +100,7 @@ namespace Yubeshi.Ubx public static bool TryParse(byte[] sentence, out UnknownPacket packet) { packet = null; - if (sentence.Length < 12) + if (sentence.Length < 20) { return false; } diff --git a/Yubeshi/Ubx/NavSol.cs b/Yubeshi/Ubx/NavSol.cs index adb4cfa..106aca9 100755 --- a/Yubeshi/Ubx/NavSol.cs +++ b/Yubeshi/Ubx/NavSol.cs @@ -50,7 +50,7 @@ namespace Yubeshi.Ubx } } - public GpsFixType PositionFixType + public GpsFixType FixType { get { @@ -73,8 +73,8 @@ namespace Yubeshi.Ubx 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); + double acc = BitConverter.ToUInt32(Raw, 6 + 24) * 0.01; + return new EcefCoordinate(x * 0.01, y * 0.01, z * 0.01, acc); } } diff --git a/Yubeshi/Ubx/NavStatus.cs b/Yubeshi/Ubx/NavStatus.cs index 4c7311e..e08d4e4 100755 --- a/Yubeshi/Ubx/NavStatus.cs +++ b/Yubeshi/Ubx/NavStatus.cs @@ -17,13 +17,22 @@ namespace Yubeshi.Ubx [Flags] public enum NavigationStatus : byte { - + GpsFixOk = 0x01, + DgpsFix = 0x02, + WeekNumberValid = 0x04, + TimeOfWeekValid = 0x08, + } + + public enum DifferentialStatus : byte + { + None = 0, + PrPrr = 1, + PrPrrCp = 2, + HighAccuracyPrPrrCp = 3, } #endregion #region fields - - private NavigationStatus flags; #endregion @@ -38,10 +47,54 @@ namespace Yubeshi.Ubx #region properties + public decimal TimeOfWeek + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 0) * 1e-3m; + } + } + public GpsFixType PositionFixType { - get; - private set; + get + { + return (GpsFixType)Raw[6 + 4]; + } + } + + public NavigationStatus NavigationStatusFlags + { + get + { + return (NavigationStatus)Raw[6 + 5]; + } + } + + public DifferentialStatus DgpsStatus + { + get + { + return (DifferentialStatus)(Raw[6 + 6] & 3); + } + } + + public TimeSpan TimeToFirstFix + { + get + { + long ms = BitConverter.ToUInt32(Raw, 6 + 8); + return new TimeSpan(ms * 10000); // 1e-3 / 100e-9 + } + } + + public TimeSpan TimeSinceStartup + { + get + { + long ms = BitConverter.ToUInt32(Raw, 6 + 12); + return new TimeSpan(ms * 10000); // 1e-3 / 100e-9 + } } #endregion @@ -50,7 +103,8 @@ namespace Yubeshi.Ubx public static bool TryParse(byte[] sentence, out UnknownPacket packet) { - return TryParse(sentence, out packet, MessageID.NavSol, 52, Build); + return TryParse(sentence, out packet, + MessageID.NavStatus, 16, Build); } #endregion @@ -59,7 +113,7 @@ namespace Yubeshi.Ubx private static Packet Build(byte[] sentence, int length) { - return new NavSol(sentence, length); + return new NavStatus(sentence, length); } #endregion diff --git a/Yubeshi/Ubx/NavSvInfo.cs b/Yubeshi/Ubx/NavSvInfo.cs index f52a033..4434e3c 100755 --- a/Yubeshi/Ubx/NavSvInfo.cs +++ b/Yubeshi/Ubx/NavSvInfo.cs @@ -47,7 +47,7 @@ namespace Yubeshi.Ubx } } - public int SatelliteId + public int SatelliteID { get { @@ -83,7 +83,7 @@ namespace Yubeshi.Ubx { get { - return ((int)(Raw[4] << 24)) >> 24; + return (sbyte)(Raw[5]); } } @@ -164,7 +164,7 @@ namespace Yubeshi.Ubx public static bool TryParse(byte[] sentence, out UnknownPacket packet) { packet = null; - if (sentence.Length < 8) + if (sentence.Length < 16) { return false; } diff --git a/Yubeshi/Ubx/NavTimeUtc.cs b/Yubeshi/Ubx/NavTimeUtc.cs index 552601b..b456b41 100755 --- a/Yubeshi/Ubx/NavTimeUtc.cs +++ b/Yubeshi/Ubx/NavTimeUtc.cs @@ -13,6 +13,18 @@ namespace Yubeshi.Ubx { public class NavTimeUtc : Packet { + #region type definitions + + [Flags] + public enum Validity : byte + { + TimeOfWeek = 0x01, + WeekNumber = 0x02, + Utc = 0x04, + } + + #endregion + #region constructors public NavTimeUtc(byte[] sentence, int length) @@ -48,16 +60,24 @@ namespace Yubeshi.Ubx { int y = BitConverter.ToUInt16(Raw, 6 + 12); int m = Raw[6 + 14]; - int d = Raw[6 + 14]; - int h = Raw[6 + 14]; - int min = Raw[6 + 14]; - int s = Raw[6 + 14]; + int d = Raw[6 + 15]; + int h = Raw[6 + 16]; + int min = Raw[6 + 17]; + int s = Raw[6 + 18]; int ftow = BitConverter.ToInt32(Raw, 6 + 8); DateTime utc = new DateTime(y, m, d, h, min, s, DateTimeKind.Utc); return utc.AddTicks(ftow / 100); } } + + public Validity ValidityFlag + { + get + { + return (Validity)Raw[19]; + } + } #endregion #region public methods diff --git a/Yubeshi/Ubx/NavVelEcef.cs b/Yubeshi/Ubx/NavVelEcef.cs index 1096e1b..653bdae 100755 --- a/Yubeshi/Ubx/NavVelEcef.cs +++ b/Yubeshi/Ubx/NavVelEcef.cs @@ -39,8 +39,8 @@ namespace Yubeshi.Ubx 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); + double acc = BitConverter.ToUInt32(Raw, 6 + 16) * 0.01; + return new EcefVelocity(x * 0.01, y * 0.01, z * 0.01, acc); } } diff --git a/Yubeshi/Ubx/NavVelNed.cs b/Yubeshi/Ubx/NavVelNed.cs index 12d1c87..335f81c 100755 --- a/Yubeshi/Ubx/NavVelNed.cs +++ b/Yubeshi/Ubx/NavVelNed.cs @@ -39,7 +39,7 @@ namespace Yubeshi.Ubx 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); + return new EnuVelocity(e * 0.01, n * 0.01, -d * 0.01); } } @@ -47,20 +47,40 @@ namespace Yubeshi.Ubx { get { - return BitConverter.ToUInt32(Raw, 6 + 16); + return BitConverter.ToUInt32(Raw, 6 + 16) * 0.01; } } public double GroundSpeed { - get; - private set; + get + { + return BitConverter.ToUInt32(Raw, 6 + 20) * 0.01; + } } public Degree Heading { - get; - private set; + get + { + return BitConverter.ToInt32(Raw, 6 + 24) * 1e-5; + } + } + + public double SpeedAccuracy + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 28) * 0.01; + } + } + + public Degree HeadingAccuracy + { + get + { + return BitConverter.ToUInt32(Raw, 6 + 32) * 1e-5; + } } #endregion diff --git a/Yubeshi/Ubx/Parser.cs b/Yubeshi/Ubx/Parser.cs index d463e66..6f16ff6 100755 --- a/Yubeshi/Ubx/Parser.cs +++ b/Yubeshi/Ubx/Parser.cs @@ -90,11 +90,11 @@ namespace Yubeshi.Ubx { return false; } - Packet.MessageID messageId = + Packet.MessageID messageID = (Packet.MessageID)(sentence[2] << 8 | sentence[3]); - if (parsers.ContainsKey(messageId)) + if (parsers.ContainsKey(messageID)) { - return parsers[messageId](sentence, out packet); + return parsers[messageID](sentence, out packet); } return Unknown.TryParse(sentence, out packet); } diff --git a/Yubeshi/Ubx/RxmAlm.cs b/Yubeshi/Ubx/RxmAlm.cs index 14ea90c..d7ade82 100755 --- a/Yubeshi/Ubx/RxmAlm.cs +++ b/Yubeshi/Ubx/RxmAlm.cs @@ -20,14 +20,14 @@ namespace Yubeshi.Ubx { } - public RxmAlm(uint svId) + public RxmAlm(uint svID) : base( new byte[] { Packet.SyncCharacter1, Packet.SyncCharacter2, 0x02, 0x30, // ID 0x01, 0x00, // length - (byte)svId, // svid + (byte)svID, // svid 0x00, 0x00 // checksum }) { @@ -54,7 +54,7 @@ namespace Yubeshi.Ubx #region properties - public uint SvId + public uint SvID { get { diff --git a/Yubeshi/Ubx/RxmRaw.cs b/Yubeshi/Ubx/RxmRaw.cs index fecfda1..8756fca 100755 --- a/Yubeshi/Ubx/RxmRaw.cs +++ b/Yubeshi/Ubx/RxmRaw.cs @@ -58,7 +58,7 @@ namespace Yubeshi.Ubx { get { - return ((int)(Raw[21] << 24)) >> 24; // SByte + return (sbyte)Raw[21]; } } @@ -66,7 +66,7 @@ namespace Yubeshi.Ubx { get { - return ((int)(Raw[22] << 24)) >> 24; // SByte + return (sbyte)Raw[22]; } } @@ -147,7 +147,7 @@ namespace Yubeshi.Ubx public static bool TryParse(byte[] sentence, out UnknownPacket packet) { packet = null; - if (sentence.Length < 8) + if (sentence.Length < 16) { return false; } diff --git a/Yubeshi/Ubx/RxmSfrB.cs b/Yubeshi/Ubx/RxmSfrB.cs index 6664db9..c9fac30 100755 --- a/Yubeshi/Ubx/RxmSfrB.cs +++ b/Yubeshi/Ubx/RxmSfrB.cs @@ -37,7 +37,7 @@ namespace Yubeshi.Ubx } } - public int SatelliteId + public int SatelliteID { get { -- 2.11.0