uitgy
Dependents: App_Pese_Ruche_SYSBEE
Fork of C027_Support by
GPS.cpp
- Committer:
- mazgch
- Date:
- 2013-10-25
- Revision:
- 3:c7cd4887560d
- Parent:
- 2:b6012cd91657
- Child:
- 4:c959dd4c5fe8
File content as of revision 3:c7cd4887560d:
#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(); 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) 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) 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 ix = 0; pipe->start(); if (++ix > len) return WAIT; if ('$' != pipe->next()) return NOT_FOUND; for (;;) { if (++ix > len) return WAIT; char ch = pipe->next(); if ('\n' == ch) return ix; if (!isprint(ch) && '\r'!= ch) return NOT_FOUND; } } 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::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*/) : Serial(tx, rx, "gps"), _pipe(256) { attach(this, &GPSSerial::serialRxIrq, RxIrq); baud(baudrate); } GPSSerial::~GPSSerial(void) { attach(NULL, RxIrq); } void GPSSerial::serialRxIrq(void) { while (serial_readable(&_serial)) _pipe.putc(serial_getc(&_serial)); } int GPSSerial::getMessage(char* buf, int len) { return _getMessage(&_pipe, buf, len); } char GPSSerial::next(void) { return _pipe.next(); } int GPSSerial::send(const void* buf, int len) { for (int i = 0; i < len; i ++) putc(((char*)buf)[i]); return len; } // ---------------------------------------------------------------- // I2C Implementation // ---------------------------------------------------------------- GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/) : I2C(sda,scl), _pipe(256) { 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) { int sz = _get(buf, len); if (sz) _pipe.put(buf, sz); return _getMessage(&_pipe, buf, len); } 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;