Diff: GPS.cpp

Revision:

2:b6012cd91657

Child:

3:c7cd4887560d

diff -r f41579f4e2ed -r b6012cd91657 GPS.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.cpp	Fri Oct 25 08:47:22 2013 +0000
@@ -0,0 +1,289 @@
+#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::_putNmea(Stream* stream, 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;
+}
+
+int GPSParser::_putUbx(Stream* stream, const unsigned char cls, unsigned char id, unsigned char* 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 ++)
+    {
+        t = *buf++;
+        ca += t; cb += ca;
+        stream->putc(t);
+    }
+    stream->putc(ca & 0xFF);
+    stream->putc(cb & 0xFF);
+    return len + 8;
+}
+
+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);   
+}
+
+// ----------------------------------------------------------------
+// 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,&REGSTREAM,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::_get(char* buf, int len)
+{
+    unsigned char sz[2];
+    if (!I2C::write(GPSADR,&REGLEN,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)
+        {
+            if (!I2C::read(GPSADR,buf,size))
+            {
+                found = true;
+                return size;
+            }
+            // error reading data
+            found = false;
+            return 0;
+        }
+        else
+        {
+            found = true;
+            // no data -> ok 
+        }
+    }
+    else 
+    {
+        // error setting address and reading length
+        found = false;
+    }
+    I2C::stop();
+    return 0;
+}
+
+int GPSI2C::_put(const char* buf, int len)
+{
+    if (len == 0)
+        return 0;
+    if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true) && 
+        !I2C::write(GPSADR,buf,len,false))
+    {
+        found = true;
+        return len;
+    }
+    found = false;
+    return 0;
+}
+
+const char GPSI2C::REGLEN    = 0xFD;
+const char GPSI2C::REGSTREAM = 0xFF;