* add C027_Support fork
Fork of C027_Support by
GPS.cpp
- Committer:
- mazgch
- Date:
- 2013-11-10
- Revision:
- 9:e7a5959ffae1
- Parent:
- 7:9aa830f5811e
- Child:
- 10:3f1c13a8763d
File content as of revision 9:e7a5959ffae1:
#include "mbed.h" #include <ctype.h> #include "GPS.h" int GPSParser::_getMessage(Pipe<char>* pipe, char* buf, int len) { int unkn = 0; int sz = pipe->size(); int fr = pipe->free(); if (len > sz) len = sz; while (len > 0) { // NMEA protocol int nmea = _parseNmea(pipe,len); if ((nmea != NOT_FOUND) && (unkn > 0)) return unkn; if (nmea == WAIT && fr) return WAIT; if (nmea > 0) return NMEA | pipe->get(buf,nmea); // UBX protocol int ubx = _parseUbx(pipe,len); if ((ubx != NOT_FOUND) && (unkn > 0)) return unkn; if (ubx == WAIT && fr) return WAIT; if (ubx > 0) return UBX | pipe->get(buf,ubx); // UNKNOWN *buf++ = pipe->getc(); unkn ++; len--; } if (unkn != NOT_FOUND) return unkn; return WAIT; } int GPSParser::_parseNmea(Pipe<char>* pipe, int len) { int o = 0; int c = 0; char ch; pipe->start(); if (++o > len) return WAIT; if ('$' != pipe->next()) return NOT_FOUND; // this needs to be extended by crc checking for (;;) { 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) { int o = 0; pipe->start(); if (++o > l) return WAIT; if ('\xB5' != pipe->next()) return NOT_FOUND; if (++o > l) return WAIT; if ('\x62' != pipe->next()) return NOT_FOUND; o += 4; if (o > l) return WAIT; int i,j,ca,cb; i = pipe->next(); ca = i; cb = ca; // cls i = pipe->next(); ca += i; cb += ca; // id i = pipe->next(); ca += i; cb += ca; // len_lsb j = pipe->next(); ca += j; cb += ca; // len_msb j = i + (j << 8); while (j--) { if (++o > l) return WAIT; i = pipe->next(); ca += i; cb += ca; } ca &= 0xFF; cb &= 0xFF; if (++o > l) return WAIT; if (ca != pipe->next()) return NOT_FOUND; if (++o > l) return WAIT; if (cb != pipe->next()) return NOT_FOUND; return o; } int GPSParser::send(const char* buf, int len) { return _send(buf, len); } int GPSParser::sendNmea(const char* buf, int len) { char head[1] = { '$' }; char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' }; int i; int crc = 0; for (i = 0; i < len; i ++) crc ^= *buf++; i = _send(head, sizeof(head)); i += _send(buf, len); tail[1] = toHex[(crc > 4) & 0xF0]; tail[2] = toHex[(crc > 0) & 0x0F]; i += _send(tail, sizeof(tail)); return i; } int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) { char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 }; char crc[2]; int i; int ca = 0; int cb = 0; for (i = 2; i < 6; i ++) { ca += head[i]; cb += ca; } for (i = 0; i < len; i ++) { ca += ((char*)buf)[i]; cb += ca; } i = _send(head, sizeof(head)); i += _send(buf, len); crc[0] = ca & 0xFF; crc[1] = cb & 0xFF; i += _send(crc, sizeof(crc)); return i; } const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end) { // find the start for (; (start < end) && (ix > 0); start ++) { if (*start == ',') ix --; } // found and check bounds if ((ix == 0) && (start < end) && (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n')) return start; else return NULL; } bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val) { char* end = &buf[len]; const char* pos = findNmeaItemPos(ix, buf, end); // find the start if (!pos) return false; val = strtod(pos, &end); // restore the last character return (end > pos); } bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/) { char* end = &buf[len]; const char* pos = findNmeaItemPos(ix, buf, end); // find the start if (!pos) return false; val = (int)strtol(pos, &end, base); return (end > pos); } bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val) { const char* end = &buf[len]; const char* pos = findNmeaItemPos(ix, buf, end); // find the start if (!pos) return false; // skip leading spaces while ((pos < end) && isspace(*pos)) pos++; // check bound if ((pos < end) && (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n')) { val = *pos; return true; } return false; } const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' }; // ---------------------------------------------------------------- // Serial Implementation // ---------------------------------------------------------------- GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/, int rxSize /*= 256*/, int txSize /*= 128*/) : SerialPipe(tx, rx, rxSize, txSize, "gps") { baud(baudrate); } int GPSSerial::getMessage(char* buf, int len) { return _getMessage(&_pipeRx, buf, len); } char GPSSerial::next(void) { return _pipeRx.next(); } int GPSSerial::_send(const void* buf, int len) { return put((const char*)buf, len, true/*=blocking*/); } // ---------------------------------------------------------------- // I2C Implementation // ---------------------------------------------------------------- GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/, int rxSize /*= 256*/) : I2C(sda,scl), _pipe(rxSize) { found = false; } bool GPSI2C::detect(void) { if (!found) { int w = I2C::write(GPSADR,®STREAM,sizeof(REGSTREAM)); if (w == 0) found = true; } return found; } int GPSI2C::getMessage(char* buf, int len) { // fill the pipe int sz = _pipe.free(); if (sz) sz = _get(buf, sz); if (sz) _pipe.put(buf, sz); // now parse it return _getMessage(&_pipe, buf, len); } int GPSI2C::send(const char* buf, int len) { int sent = 0; if (len) { if (!I2C::write(GPSADR,®STREAM,sizeof(REGSTREAM),true)) sent = _send(buf, len); found = len == sent; stop(); } return sent; } int GPSI2C::sendNmea(const char* buf, int len) { int sent = 0; if (len) { if (!I2C::write(GPSADR,®STREAM,sizeof(REGSTREAM),true)) sent = GPSParser::sendNmea(buf, len); found = len == sent; stop(); } return sent; } int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) { int sent = 0; if (len) { if (!I2C::write(GPSADR,®STREAM,sizeof(REGSTREAM),true)) sent = GPSParser::sendUbx(cls, id, buf, len); found = (len == sent); stop(); } return sent; } int GPSI2C::_get(char* buf, int len) { int read = 0; unsigned char sz[2]; if (!I2C::write(GPSADR,®LEN,sizeof(REGLEN),true) && !I2C::read(GPSADR,(char*)sz,sizeof(sz),true)) { int size = 256 * (int)sz[0] + sz[1]; if (size > len) size = len; if (size > 0) read = !I2C::read(GPSADR,buf,size, true) ? size : 0; stop(); found = read = size; } else found = false; I2C::stop(); return read; } char GPSI2C::next(void) { return _pipe.next(); } int GPSI2C::_send(const void* buf, int len) { return !I2C::write(GPSADR,(const char*)buf,len,true) ? len : 0; } const char GPSI2C::REGLEN = 0xFD; const char GPSI2C::REGSTREAM = 0xFF;