Diff: GPS.cpp

Revision:

7:9aa830f5811e

Parent:

6:775aef3f1d1f

Child:

9:e7a5959ffae1

--- a/GPS.cpp	Sat Nov 09 12:20:25 2013 +0000
+++ b/GPS.cpp	Sat Nov 09 13:31:01 2013 +0000
@@ -31,18 +31,32 @@
 
 int GPSParser::_parseNmea(Pipe<char>* pipe, int len)
 {
-    int ix = 0;
+    int o = 0;
+    int c = 0;
+    char ch;
     pipe->start();
-    if (++ix > len)                     return WAIT;
+    if (++o > len)                      return WAIT;
     if ('$' != pipe->next())            return NOT_FOUND;
     // this needs to be extended by crc checking 
     for (;;)
     {
-        if (++ix > len)                 return WAIT;
-        char ch = pipe->next();
-        if ('\n' == ch)                 return ix;
-        if (!isprint(ch) && '\r'!= ch)  return NOT_FOUND; 
+        if (++o > len)                  return WAIT;
+        ch = pipe->next();
+        if ('*' == ch)                  break; // crc delimiter 
+        if (!isprint(ch))               return NOT_FOUND; 
+        c ^= ch;
     }
+    if (++o > len)                      return WAIT;
+    ch = toHex[(c >> 4) & 0xF]; // high nibble
+    if (ch != pipe->next())             return NOT_FOUND;
+    if (++o > len)                      return WAIT;
+    ch = toHex[(c >> 0) & 0xF]; // low nibble
+    if (ch != pipe->next())             return NOT_FOUND;
+    if (++o > len)                      return WAIT;
+    if ('\r' != pipe->next())           return NOT_FOUND;
+    if (++o > len)                      return WAIT;
+    if ('\n' != pipe->next())           return NOT_FOUND;
+    return o;
 }
 
 int GPSParser::_parseUbx(Pipe<char>* pipe, int l)