decimal lng = Math.Floor(lngRaw / 100m) + (lngRaw % 100m) / 60m;\r
Latitude = (ns == "S") ? -lat : lat;\r
Longitude = (ns == "W") ? -lng : lng;\r
+ Altitude = 0;\r
}\r
\r
#region properties\r
+\r
+ /// <summary>\r
+ /// in Degree\r
+ /// </summary>\r
public decimal Latitude\r
{\r
get;\r
set;\r
}\r
+ /// <summary>\r
+ /// in Degree\r
+ /// </summary>\r
public decimal Longitude\r
{\r
get;\r
set;\r
}\r
+\r
+ /// <summary>\r
+ /// in metre\r
+ /// </summary>\r
+ public decimal Altitude\r
+ {\r
+ get;\r
+ set;\r
+ }\r
#endregion\r
}\r
}\r
\r
namespace Yubeshi.Nmea\r
{\r
+ /// <summary>\r
+ /// Datum Reference\r
+ /// </summary>\r
public class GpDtm : Packet\r
{\r
- #region fields\r
+ #region type definitions\r
+ public enum Code\r
+ { \r
+ Unknown = 0,\r
+ Wgs84 = 84,\r
+ Wgs72 = 72,\r
+ UserDefined = 999,\r
+ }\r
\r
+ #endregion\r
+\r
+ #region fields\r
+ private static Dictionary<string, Code> codes;\r
private static readonly byte[] header;\r
private const int elementNum = 8;\r
\r
static GpDtm()\r
{\r
header = Encoding.ASCII.GetBytes("$GPDTM,");\r
+ codes = new Dictionary<string,Code>();\r
+ codes["W84"] = Code.Wgs84;\r
+ codes["W72"] = Code.Wgs72;\r
+ codes["999"] = Code.UserDefined;\r
}\r
\r
public GpDtm()\r
#endregion\r
\r
#region properties\r
+ public Code LocalDatumCode\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+\r
+ public Coordinate Offset\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
\r
+ public Code ReferenceDetumCode\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
#endregion\r
\r
#region public method\r
namespace Yubeshi.Nmea\r
{\r
/// <summary>\r
- /// \r
+ /// GNSS Satellite Fault Detection\r
/// </summary>\r
public class GpGbs : Packet\r
{\r
{\r
Raw = new byte[elements.PacketLength];\r
Array.Copy(sentence, Raw, elements.PacketLength);\r
-\r
- TimeOfFix = ParseTime(elements.Values[0]);\r
- Position = new Coordinate(elements.Values[1], elements.Values[2],\r
- elements.Values[3], elements.Values[4]);\r
- FixQuality = (FixQualityClass)Int32.Parse(elements.Values[5]);\r
- TrackedSatellites = Int32.Parse(elements.Values[6]);\r
- Dilution = Decimal.Parse(elements.Values[7]);\r
+ string[] v = elements.Values;\r
+ TimeOfFix = ParseTime(v[0]);\r
+ Position = new Coordinate(v[1], v[2], v[3], v[4]);\r
+ FixQuality = (FixQualityClass)Int32.Parse(v[5]);\r
+ TrackedSatellites = Int32.Parse(v[6]);\r
+ Dilution = Decimal.Parse(v[7]);\r
+ MslAltitude = GetLength(v[8], v[9]);\r
+ GeoidSeparation = GetLength(v[10], v[11]);\r
+ \r
CheckSum = elements.CheckSum;\r
}\r
\r
private set;\r
}\r
\r
+ public decimal MslAltitude\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
\r
+ public decimal GeoidSeparation\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+\r
+ public int StationID\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
#endregion\r
\r
\r
return new GpGga(sentence, elements);\r
}\r
\r
- private TimeSpan ParseTime(string time)\r
- {\r
- string h = time.Substring(0, 2);\r
- string m = time.Substring(2, 2);\r
- string s = time.Substring(4, 2);\r
- return new TimeSpan(Int32.Parse(h), Int32.Parse(m), Int32.Parse(s));\r
- }\r
-\r
}\r
}
\ No newline at end of file
namespace Yubeshi.Nmea\r
{\r
/// <summary>\r
- /// \r
+ /// Latitude and Longitude, with Time of Position Fix and Status\r
/// </summary>\r
public class GpGll : Packet\r
{\r
+ #region fields\r
+\r
+ private static readonly byte[] header;\r
+ private const int elementNum = 7;\r
+\r
+ #endregion\r
+\r
+ #region constructors\r
+ \r
+ static GpGll()\r
+ {\r
+ header = Encoding.ASCII.GetBytes("$GPGLL,");\r
+ }\r
+ \r
+ public GpGll()\r
+ {\r
+ }\r
+\r
+ public GpGll(byte[] sentence)\r
+ : this(sentence, GetElements(sentence, elementNum))\r
+ {\r
+ }\r
+\r
+ private GpGll(byte[] sentence, Elements elements)\r
+ { \r
+\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region public method\r
+\r
+ public static bool TryParse(byte[] sentence, out Packet packet)\r
+ {\r
+ return TryParse(sentence, out packet, header, elementNum, Build);\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region private method\r
+\r
+ private static Packet Build(byte[] sentence, Elements elements)\r
+ {\r
+ return new GpGll(sentence, elements);\r
+ }\r
+\r
+ #endregion\r
}\r
}
\ No newline at end of file
namespace Yubeshi.Nmea\r
{\r
/// <summary>\r
- /// \r
+ /// GNSS Range Residuals\r
/// </summary>\r
public class GpGrs : Packet\r
{\r
+ #region fields\r
+\r
+ private static readonly byte[] header;\r
+ private const int elementNum = 14;\r
+\r
+ #endregion\r
+\r
+ #region constructors\r
+ \r
+ static GpGrs()\r
+ {\r
+ header = Encoding.ASCII.GetBytes("$GPGRS,");\r
+ }\r
+ \r
+ public GpGrs()\r
+ {\r
+ }\r
+\r
+ public GpGrs(byte[] sentence)\r
+ : this(sentence, GetElements(sentence, elementNum))\r
+ {\r
+ }\r
+\r
+ private GpGrs(byte[] sentence, Elements elements)\r
+ {\r
+ Raw = new byte[elements.PacketLength];\r
+ Array.Copy(sentence, Raw, elements.PacketLength);\r
+ string[] v = elements.Values;\r
+ TimeOfFix = ParseTime(v[0]);\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region properties\r
+\r
+ public TimeSpan TimeOfFix\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region public method\r
+\r
+ public static bool TryParse(byte[] sentence, out Packet packet)\r
+ {\r
+ return TryParse(sentence, out packet, header, elementNum, Build);\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region private method\r
+\r
+ private static Packet Build(byte[] sentence, Elements elements)\r
+ {\r
+ return new GpGrs(sentence, elements);\r
+ }\r
+\r
+ #endregion\r
}\r
}
\ No newline at end of file
namespace Yubeshi.Nmea\r
{\r
/// <summary>\r
- /// \r
+ /// GNSS DOP and Active Satellites\r
/// </summary>\r
public class GpGsa : Packet\r
{\r
+ #region fields\r
+\r
+ private static readonly byte[] header;\r
+ private const int elementNum = 17;\r
+\r
+ #endregion\r
+\r
+ #region constructors\r
+ \r
+ static GpGsa()\r
+ {\r
+ header = Encoding.ASCII.GetBytes("$GPGSA,");\r
+ }\r
+ \r
+ public GpGsa()\r
+ {\r
+ }\r
+\r
+ public GpGsa(byte[] sentence)\r
+ : this(sentence, GetElements(sentence, elementNum))\r
+ {\r
+ }\r
+\r
+ private GpGsa(byte[] sentence, Elements elements)\r
+ {\r
+ Raw = new byte[elements.PacketLength];\r
+ Array.Copy(sentence, Raw, elements.PacketLength);\r
+ string[] v = elements.Values;\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region properties\r
+\r
+ #endregion\r
+\r
+ #region public method\r
+\r
+ public static bool TryParse(byte[] sentence, out Packet packet)\r
+ {\r
+ return TryParse(sentence, out packet, header, elementNum, Build);\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region private method\r
+\r
+ private static Packet Build(byte[] sentence, Elements elements)\r
+ {\r
+ return new GpGsa(sentence, elements);\r
+ }\r
+\r
+ #endregion\r
}\r
}
\ No newline at end of file
}\r
catch\r
{\r
+ packet = null;\r
return false;\r
}\r
return true;\r
}\r
return true;\r
}\r
+ protected TimeSpan ParseTime(string time)\r
+ {\r
+ int h = Int32.Parse(time.Substring(0, 2));\r
+ int m = Int32.Parse(time.Substring(2, 2));\r
+ decimal ds = Decimal.Parse(time.Substring(4));\r
+ int sec = (int)Math.Floor(ds);\r
+ int msec = (int)((ds - sec) * 1000m);\r
+ return new TimeSpan(0, h, m, sec, msec);\r
+ }\r
+\r
+ protected static decimal GetLength(string value, string unit)\r
+ { \r
+ decimal v;\r
+ \r
+ if (!Decimal.TryParse(value, out v))\r
+ {\r
+ return -Decimal.MaxValue;;\r
+ }\r
+ if (unit == "M")\r
+ {\r
+ return v;\r
+ }\r
+ else\r
+ {\r
+ return -Decimal.MaxValue; ;\r
+ }\r
+ }\r
#endregion\r
}\r
}\r
{\r
public class Parser : Yubeshi.Parser\r
{\r
+ #region type definitions\r
+ delegate bool TrialParser(byte[] sentence, out Packet packet);\r
+ #endregion\r
+\r
+ #region fields\r
+\r
+ private static Dictionary<string, TrialParser> gpParsers;\r
+ private static Encoding ascii = Encoding.ASCII;\r
+\r
+ #endregion\r
+\r
+ #region constructors\r
+ static Parser()\r
+ {\r
+ gpParsers = new Dictionary<string, TrialParser>();\r
+ gpParsers["DTM"] = GpDtm.TryParse;\r
+ //gpParsers["GBS"] = GpGbs.TryParse;\r
+ gpParsers["GGA"] = GpGga.TryParse;\r
+ gpParsers["GLL"] = GpGll.TryParse;\r
+ gpParsers["GRS"] = GpGrs.TryParse;\r
+ gpParsers["GSA"] = GpGsa.TryParse;\r
+ }\r
+ #endregion\r
+\r
#region public method\r
\r
- public bool TryParse(byte[] sentence, out Packet packet)\r
+ public static bool TryParse(byte[] sentence, out Packet packet)\r
{\r
- packet = new GpGga();\r
+ packet = null;\r
+ // $XXXX,*CCcl\r
+ if (sentence.Length < 11 || sentence[0] != '$')\r
+ {\r
+ return false;\r
+ }\r
+ if (sentence[1] == 'G')\r
+ { \r
+ if (sentence[2] == 'P') // GPS\r
+ {\r
+ string format = ascii.GetString(sentence, 3, 3);\r
+ if (gpParsers.ContainsKey(format))\r
+ {\r
+ return gpParsers[format](sentence, out packet);\r
+ }\r
+ return false;\r
+ }\r
+ }\r
+\r
return false;\r
}\r
+\r
#endregion\r
}\r
}\r
--- /dev/null
+/*\r
+ * Yubeshi GPS Parser\r
+ *\r
+ * This software is distributed under a zlib-style license.\r
+ * See license.txt for more information.\r
+ */\r
+\r
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+\r
+namespace Yubeshi.Ubx\r
+{\r
+ public class NavPosEcef : Packet\r
+ {\r
+ #region constructors\r
+\r
+ public NavPosEcef(byte[] sentence, int length)\r
+ { \r
+ Raw = new byte[length + 8];\r
+ Array.Copy(sentence, Raw, Raw.Length);\r
+ \r
+ uint tow = BitConverter.ToUInt32(sentence, 6 + 0);\r
+ X = 0.01m * BitConverter.ToInt32(sentence, 6 + 4);\r
+ Y = 0.01m * BitConverter.ToInt32(sentence, 6 + 8);\r
+ Z = 0.01m * BitConverter.ToInt32(sentence, 6 + 12);\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region properties\r
+\r
+ public TimeSpan TimeOfWeek\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+\r
+ public decimal X\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+\r
+ public decimal Y\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+\r
+ public decimal Z\r
+ {\r
+ get;\r
+ private set;\r
+ }\r
+ #endregion\r
+\r
+ #region public methods\r
+\r
+ public static bool TryParse(byte[] sentence, out Packet packet)\r
+ {\r
+ return TryParse(sentence, out packet, \r
+ MessageID.NavPosEcef, 20, Build);\r
+ }\r
+\r
+ #endregion\r
+\r
+ #region private methods\r
+\r
+ private static Packet Build(byte[] sentence, int length)\r
+ {\r
+ return new NavPosEcef(sentence, length);\r
+ }\r
+\r
+ #endregion\r
+ }\r
+}\r
\r
namespace Yubeshi.Ubx\r
{\r
- public class Packet : UnknownPacket\r
+ public abstract class Packet : UnknownPacket\r
{\r
+ #region type definitions\r
+\r
+ protected delegate Packet PacketBuilder(byte[] sentence, int length);\r
+\r
+ public enum MessageClass\r
+ { \r
+ /// <summary>\r
+ /// Navigation\r
+ /// </summary>\r
+ Nav = 0x01,\r
+ /// <summary>\r
+ /// Reciever Manager\r
+ /// </summary>\r
+ Rxm = 0x02,\r
+ Inf = 0x04,\r
+ Ack = 0x05,\r
+ Cfg = 0x06,\r
+ Mon = 0x0A,\r
+ Aid = 0x0B,\r
+ Tim = 0x0D,\r
+ }\r
+\r
+ public enum MessageID\r
+ {\r
+ NavPosEcef = 0x0101,\r
+ NavPosLlh = 0x0102,\r
+ NavStatus = 0x0103,\r
+ NavDop = 0x0104,\r
+ NavSol = 0x0105,\r
+ NavVelEcef = 0x0111,\r
+ NavVelNed = 0x0112,\r
+ NavTimeGps = 0x0120,\r
+ NavTimeUtc = 0x0121,\r
+ NavClock = 0x0122,\r
+ NavSvInfo = 0x0130,\r
+ NavSbas = 0x0132,\r
+ }\r
+ \r
+ #endregion\r
+\r
+ \r
+ #region constructors\r
+ \r
+ #endregion\r
+\r
+ #region properties\r
+\r
+ public MessageClass Class\r
+ {\r
+ get;\r
+ protected set;\r
+ }\r
+\r
+ public MessageID ID\r
+ {\r
+ get;\r
+ protected set;\r
+ }\r
+\r
+ public int CheckSum\r
+ {\r
+ get;\r
+ protected set;\r
+ }\r
+ \r
+ #endregion\r
+\r
+ #region protected methods\r
+\r
+ protected static bool TryParse(byte[] sentence, out Packet packet,\r
+ MessageID id, int fixedLength, PacketBuilder builder)\r
+ {\r
+ packet = null;\r
+ if (sentence.Length < 8)\r
+ {\r
+ return false;\r
+ }\r
+ if (sentence[0] != 0xB5 || sentence[1] != 0x62)\r
+ {\r
+ return false;\r
+ }\r
+ int messageId = sentence[2] << 8 | sentence[3];\r
+ if ((int)id != messageId)\r
+ {\r
+ return false;\r
+ }\r
+ int length = sentence[5] << 8 | sentence[4];\r
+ if (length > 0 && fixedLength != length)\r
+ {\r
+ return false;\r
+ }\r
+ if (sentence.Length < length + 8)\r
+ {\r
+ return false;\r
+ }\r
+ try\r
+ {\r
+ packet = builder(sentence, length);\r
+ }\r
+ catch\r
+ {\r
+ packet = null;\r
+ return false;\r
+ }\r
+ return true;\r
+ }\r
+\r
+ \r
+ #endregion\r
+\r
+\r
}\r
}\r
<Compile Include="Nmea\GpRmc.cs" />\r
<Compile Include="Nmea\Packet.cs" />\r
<Compile Include="Properties\AssemblyInfo.cs" />\r
+ <Compile Include="Ubx\NavPosEcef.cs" />\r
<Compile Include="Ubx\Packet.cs" />\r
<Compile Include="UnknownPacket.cs" />\r
</ItemGroup>\r
-using System;\r
+/*\r
+ * Yubeshi GPS Parser\r
+ *\r
+ * This software is distributed under a zlib-style license.\r
+ * See license.txt for more information.\r
+ */\r
+\r
+using System;\r
using System.Collections.Generic;\r
using System.Text;\r
\r
--- /dev/null
+/*\r
+ * Yubeshi GPS Parser\r
+ *\r
+ * This software is distributed under a zlib-style license.\r
+ * See license.txt for more information.\r
+ */\r
+\r
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+using NUnit.Framework;\r
+using Yubeshi.Ubx;\r
+using P = YubeshiTest.Ubx.SamplePackets;\r
+\r
+namespace YubeshiTest.Ubx\r
+{\r
+\r
+ class NavPacketTest\r
+ {\r
+\r
+ [Test]\r
+ public void NavPosEcefTest()\r
+ {\r
+ Packet p;\r
+ Assert.AreEqual(true, NavPosEcef.TryParse(P.NavPosEcef, out p));\r
+ }\r
+\r
+ }\r
+}\r
--- /dev/null
+/*\r
+ * Yubeshi GPS Parser\r
+ *\r
+ * This software is distributed under a zlib-style license.\r
+ * See license.txt for more information.\r
+ */\r
+\r
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+\r
+namespace YubeshiTest.Ubx\r
+{\r
+ public class SamplePackets\r
+ {\r
+ public static readonly byte[] Dummy;\r
+ public static readonly byte[] NavPosEcef;\r
+\r
+\r
+ static SamplePackets()\r
+ {\r
+ Encoding ascii = Encoding.ASCII;\r
+\r
+ Dummy = new byte[] { };\r
+ NavPosEcef = new byte[]{\r
+ 0xB5, 0x62, 0x01, 0x01, 0x14, 0x00,\r
+ 0x00, 0x00, 0x00, 0x00,\r
+ 0x00, 0x00, 0x00, 0x00,\r
+ 0x00, 0x00, 0x00, 0x00,\r
+ 0x00, 0x00, 0x00, 0x00,\r
+ 0x00, 0x00, 0x00, 0x00,\r
+ 0xFF, 0xFF\r
+ };\r
+\r
+\r
+ }\r
+ }\r
+}\r
</ItemGroup>\r
<ItemGroup>\r
<Compile Include="Nmea\PacketTest.cs" />\r
+ <Compile Include="Nmea\ParserTest.cs" />\r
+ <Compile Include="Nmea\SamplePackets.cs" />\r
<Compile Include="ParserTest.cs" />\r
<Compile Include="Properties\AssemblyInfo.cs" />\r
+ <Compile Include="Ubx\NavPacketTest.cs" />\r
+ <Compile Include="Ubx\SamplePackets.cs" />\r
</ItemGroup>\r
<ItemGroup>\r
<None Include="app.config" />\r
OSDN Git Service
