Yubeshi/Nmea/Packet.cs

   1 /*\r
   2  *      Yubeshi GPS Parser\r
   3  *\r
   4  *      This software is distributed under a zlib-style license.\r
   5  *      See license.txt for more information.\r
   6  */\r
   7 \r
   8 using System;\r
   9 using System.Collections.Generic;\r
  10 using System.Text;\r
  11 \r
  12 namespace Yubeshi.Nmea\r
  13 {\r
  14     public abstract class Packet : Yubeshi.UnknownPacket\r
  15     {\r
  16         #region type definitions\r
  17         protected delegate Packet PacketBuilder(\r
  18                                         byte[] sentence, Elements elements);\r
  19 \r
  20         protected class Elements\r
  21         {\r
  22             public string[] Values;\r
  23             public int CheckSum;\r
  24             public int PacketLength;\r
  25         }\r
  26 \r
  27         public enum Status\r
  28         {\r
  29             Unknown,\r
  30             Invalid,\r
  31             Valid,\r
  32         }\r
  33         #endregion\r
  34 \r
  35         #region fields\r
  36 \r
  37         #endregion\r
  38 \r
  39         #region constructors\r
  40 \r
  41         public Packet()\r
  42         {\r
  43         }\r
  44 \r
  45         public Packet(byte[] sentence, int length)\r
  46             : base(sentence, length)\r
  47         {\r
  48         }\r
  49 \r
  50         #endregion\r
  51 \r
  52         #region properties\r
  53 \r
  54         public int CheckSum\r
  55         {\r
  56             get;\r
  57             protected set;\r
  58         }\r
  59         #endregion\r
  60 \r
  61         #region public method\r
  62 \r
  63         #endregion\r
  64         #region protected method\r
  65 \r
  66         protected static bool TryParse(byte[] sentence, out UnknownPacket packet,\r
  67                         byte[] header, int elementNum, PacketBuilder builder)\r
  68         {\r
  69             packet = null;\r
  70             if (!Match(sentence, header))\r
  71             {\r
  72                 return false;\r
  73             }\r
  74             Elements elements = GetElements(sentence, elementNum);\r
  75             if (elements == null)\r
  76             {\r
  77                 return false;\r
  78             }\r
  79             try\r
  80             {\r
  81                 packet = builder(sentence, elements);\r
  82             }\r
  83             catch\r
  84             {\r
  85                 packet = null;\r
  86                 return false;\r
  87             }\r
  88             return true;\r
  89         }\r
  90 \r
  91         protected static Elements GetElements(byte[] sentence, int elementNum)\r
  92         {\r
  93             int elementIndex = 0;\r
  94             int lastCommaPos = 6;\r
  95             if (sentence[1] == 'G' && sentence[2] == 'P')\r
  96             {\r
  97                 lastCommaPos = 7;\r
  98             }\r
  99             if (elementNum < 0)\r
 100             {\r
 101                 elementNum = 0;\r
 102                 for (int i = lastCommaPos; i < sentence.Length; ++i)\r
 103                 {\r
 104                     if (sentence[i] == ',')\r
 105                     {\r
 106                         elementNum++;\r
 107                         continue;\r
 108                     }\r
 109                     if (sentence[i] == '*')\r
 110                     {\r
 111                         elementNum++;\r
 112                         break;\r
 113                     }\r
 114                 }\r
 115             }\r
 116             if (elementNum < 1)\r
 117             {\r
 118                 return null;\r
 119             }\r
 120             string[] values = new string[elementNum];\r
 121             for (int i = lastCommaPos; i < sentence.Length; ++i)\r
 122             {\r
 123                 if (sentence[i] == ',' || sentence[i] == '*')\r
 124                 {\r
 125                     if (elementIndex >= elementNum)\r
 126                     {\r
 127                         return null;\r
 128                     }\r
 129                     values[elementIndex++] = Encoding.ASCII.GetString(\r
 130                                     sentence, lastCommaPos, i - lastCommaPos);\r
 131                     lastCommaPos = i + 1;\r
 132                     if (sentence[i] == '*')\r
 133                     {\r
 134                         break;\r
 135                     }\r
 136                 }\r
 137             }\r
 138             if (elementIndex != elementNum)\r
 139             {\r
 140                 return null;\r
 141             }\r
 142 \r
 143             int length = lastCommaPos + 4; // CC, CC, '\r', '\n'\r
 144 \r
 145             if (sentence.Length < length)\r
 146             {\r
 147                 return null;\r
 148             }\r
 149 \r
 150             if (sentence[lastCommaPos + 2] != (byte)'\r' ||\r
 151                 sentence[lastCommaPos + 3] != (byte)'\n')\r
 152             {\r
 153                 return null;\r
 154             }\r
 155             string csString = \r
 156                         Encoding.ASCII.GetString(sentence, lastCommaPos, 2);\r
 157             Elements elements = new Elements();\r
 158             elements.Values = values;\r
 159             elements.CheckSum = Int32.Parse(\r
 160                         csString, System.Globalization.NumberStyles.HexNumber);\r
 161             elements.PacketLength = length;\r
 162             return elements;\r
 163         }\r
 164 \r
 165         protected static bool Match(byte[] sentence, byte[] header)\r
 166         {\r
 167             if (sentence.Length < header.Length)\r
 168             {\r
 169                 return false;\r
 170             }\r
 171             for (int i = 0; i < header.Length; ++i)\r
 172             {\r
 173                 if (header[i] != sentence[i])\r
 174                 {\r
 175                     return false;\r
 176                 }\r
 177             }\r
 178             return true;\r
 179         }\r
 180 \r
 181         protected TimeSpan ParseTime(string time)\r
 182         {\r
 183             int h = Int32.Parse(time.Substring(0, 2));\r
 184             int m = Int32.Parse(time.Substring(2, 2));\r
 185             decimal ds = Decimal.Parse(time.Substring(4));\r
 186             int sec = (int)Math.Floor(ds);\r
 187             int msec = (int)((ds - sec) * 1000m);\r
 188             return new TimeSpan(0, h, m, sec, msec);\r
 189         }\r
 190 \r
 191         protected static double GetLength(string value, string unit)\r
 192         { \r
 193             double v;\r
 194             \r
 195             if (!Double.TryParse(value, out v))\r
 196             {\r
 197                 return Double.NaN;\r
 198             }\r
 199             if (unit == "M")\r
 200             {\r
 201                 return v;\r
 202             }\r
 203             else\r
 204             {\r
 205                 return Double.NaN;\r
 206             }\r
 207         }\r
 208 \r
 209         protected static int ParseInt(string intString, int defaultValue)\r
 210         {\r
 211             int v;\r
 212             if (!Int32.TryParse(intString, out v))\r
 213             {\r
 214                 return defaultValue;\r
 215             }\r
 216             return v;\r
 217         }\r
 218 \r
 219         protected static double ParseDouble(string doubleString)\r
 220         {\r
 221             return ParseDouble(doubleString, Double.NaN);\r
 222         }\r
 223         \r
 224         protected static double ParseDouble(string doubleString,\r
 225                                                         double defaultValue)\r
 226         {\r
 227             double v;\r
 228             if (!Double.TryParse(doubleString, out v))\r
 229             {\r
 230                 return defaultValue;\r
 231             }\r
 232             return v;\r
 233         }\r
 234 \r
 235         protected static Status ParseStatus(string status)\r
 236         {\r
 237             switch (status)\r
 238             {\r
 239                 case "V":\r
 240                     return Status.Invalid;\r
 241                 case "A":\r
 242                     return Status.Valid;\r
 243                 default:\r
 244                     return Status.Unknown;\r
 245             }\r
 246         }\r
 247         #endregion\r
 248     }\r
 249 }\r