t3
Fork of C027_Support by
Diff: GPS.cpp
- Revision:
- 7:9aa830f5811e
- Parent:
- 6:775aef3f1d1f
- Child:
- 9:e7a5959ffae1
diff -r 775aef3f1d1f -r 9aa830f5811e GPS.cpp --- 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)