t3
Fork of C027_Support by
Diff: GPS.cpp
- Revision:
- 3:c7cd4887560d
- Parent:
- 2:b6012cd91657
- Child:
- 4:c959dd4c5fe8
diff -r b6012cd91657 -r c7cd4887560d GPS.cpp --- a/GPS.cpp Fri Oct 25 08:47:22 2013 +0000 +++ b/GPS.cpp Fri Oct 25 09:45:55 2013 +0000 @@ -75,47 +75,45 @@ return o; } -int GPSParser::_putNmea(Stream* stream, const char* buf, int len) +int GPSParser::sendNmea(const char* buf, int len) { - stream->putc('$'); - int c = 0; - for (int i = 0; i < len; i ++) - { - int t = *buf++; - stream->putc(t); - c ^= t; - } - stream->putc('*'); - stream->putc(toHex[(c >> 4) & 0xF]); - stream->putc(toHex[(c >> 0) & 0xF]); - stream->putc('\r'); - stream->putc('\n'); - return len + 6; + 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::_putUbx(Stream* stream, const unsigned char cls, unsigned char id, unsigned char* buf, int len) +int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) { - stream->putc('\xB5'); // 'µ' - stream->putc('\x62'); // 'b' - int ca = cls, cb = cls; - stream->putc(cls); - ca += id; cb += ca; - stream->putc(id); - int t = (len >> 0) & 0xFF; - ca += t; cb += ca; - stream->putc(t); - t = (len >> 8) & 0xFF; - ca += t; cb += ca; - stream->putc(t); - for (int i = 0; i < len; i ++) + 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 ++) { - t = *buf++; - ca += t; cb += ca; - stream->putc(t); + ca += head[i]; + cb += ca; } - stream->putc(ca & 0xFF); - stream->putc(cb & 0xFF); - return len + 8; + 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) @@ -206,6 +204,18 @@ 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 // ---------------------------------------------------------------- @@ -236,8 +246,35 @@ 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)) @@ -246,43 +283,24 @@ if (size > len) size = len; if (size > 0) - { - if (!I2C::read(GPSADR,buf,size)) - { - found = true; - return size; - } - // error reading data - found = false; - return 0; - } - else - { - found = true; - // no data -> ok - } + read = !I2C::read(GPSADR,buf,size, true) ? size : 0; + stop(); + found = read = size; } else - { - // error setting address and reading length found = false; - } I2C::stop(); - return 0; + return read; } -int GPSI2C::_put(const char* buf, int len) -{ - if (len == 0) - return 0; - if (!I2C::write(GPSADR,®STREAM,sizeof(REGSTREAM),true) && - !I2C::write(GPSADR,buf,len,false)) - { - found = true; - return len; - } - found = false; - return 0; +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;