Revision 2:b6012cd91657, committed 2013-10-25

Comitter:

mazgch

Date:

Fri Oct 25 08:47:22 2013 +0000

Parent:

1:f41579f4e2ed

Child:

3:c7cd4887560d

Commit message:

GPS for Serial and I2c

Changed in this revision

--- /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;

--- a/GPS.h	Mon Oct 21 19:46:34 2013 +0000
+++ b/GPS.h	Fri Oct 25 08:47:22 2013 +0000
@@ -1,12 +1,74 @@
 #pragma once 
 
+#include "mbed.h"
+#include "Pipe.h"
 #include "SerialPipe.h"
+#include "C027_PinNames.h"
+
+class GPSParser
+{
+public:
+    #define WAIT      -1
+    #define NOT_FOUND  0
+    
+    #define UBX         0x100000
+    #define NMEA        0x200000
+    #define LENGTH(x)   (x & 0x00FFFF)
+    #define PROTOCOL(x) (x & 0xFF0000)
 
-class GPS : public SerialPipe
+    virtual int getMessage(char* buf, int len) = 0; 
+    virtual int putNmea(const char* buf, int len) = 0;
+    virtual int putUbx(const unsigned char cls, unsigned char id, unsigned char* buf, int len) = 0;
+    
+    static const char* findNmeaItemPos(int ix, const char* start, const char* end);
+    static bool getNmeaItem(int ix, char* buf, int len, double& val);
+    static bool getNmeaItem(int ix, char* buf, int len, int& val, int base/*=10*/);
+    static bool getNmeaItem(int ix, char* buf, int len, char& val);
+protected:
+    static int _getMessage(Pipe<char>* pipe, char* buf, int len);
+    static int _parseNmea(Pipe<char>* pipe, int len);
+    static int _parseUbx(Pipe<char>* pipe, int len);
+    static int _putNmea(Stream* stream, const char* buf, int len);
+    static int _putUbx(Stream* stream, const unsigned char cls, unsigned char id, unsigned char* buf, int len);
+    static const char toHex[16];
+};
+
+class GPSSerial : public Serial, public GPSParser
 {
 public:
-    GPS() : SerialPipe(GPSTXD, GPSRXD, 256, "gps")
-    {
-        baud(GPSBAUD);
-    }
+    GPSSerial(PinName tx = GPSTXD, PinName rx = GPSRXD, int baudrate = GPSBAUD);
+    virtual ~GPSSerial(void);
+    
+    virtual int getMessage(char* buf, int len);
+    virtual int putNmea(const char* buf, int len)
+        { return _putNmea(this, buf, len); }
+    virtual int putUbx(const unsigned char cls, unsigned char id, unsigned char* buf, int len)
+        { return _putUbx(this, cls, id, buf, len); }
+protected:
+    void serialRxIrq(void);
+    virtual char next(void) { return _pipe.next(); }
+    Pipe<char> _pipe;
 };
+
+class GPSI2C : public I2C, public GPSParser
+{
+public: 
+    GPSI2C(PinName sda = GPSSDA, PinName scl = GPSSCL);
+    bool detect(void);
+    
+    virtual int getMessage(char* buf, int len);
+    // 
+    virtual int putNmea(const char* buf, int len)
+        { return 0/*_putNmea(NULL, buf, len)*/; }
+    virtual int putUbx(const unsigned char cls, unsigned char id, unsigned char* buf, int len)
+        { return 0/*_putUbx(NULL, cls, id, buf, len)*/; }
+protected:
+    virtual char next(void) { return _pipe.next(); }
+    int _get(char* buf, int len);          // read the NMEA or UBX stream
+    int _put(const char* buf, int len);    // Write the NMEA or UBX stream
+
+    Pipe<char> _pipe; 
+    bool found;
+    static const char REGLEN;
+    static const char REGSTREAM;
+};

--- a/Pipe.h	Mon Oct 21 19:46:34 2013 +0000
+++ b/Pipe.h	Fri Oct 25 08:47:22 2013 +0000
@@ -76,7 +76,7 @@
         return (r != w);
     }
     //! get the number of values avialable in the buffer 
-    int size(void)
+    virtual int size(void)
     {
         int t = w - r;
         if (t < 0)
@@ -93,7 +93,7 @@
         return t;
     }
     // get values from buffer (if the buffer has less values, only the values avilable are returned)
-    int get(T* p, int n)
+    virtual int get(T* p, int n)
     {
         int f = size();
         if (f < n)
@@ -116,9 +116,9 @@
         return n;
     }
     // the following functions are useful if you like to inspect or parse the buffer
-    int start() { o = r; return size(); }               // reset the parsing index and return the number of available elments
-    T next()    { T t = b[o]; o = _inc(o); return t; }  // get the next element and increment
-    void done() { r = o; }                              // commit the index 
+    virtual int start(void) { o = r; return size(); }               // reset the parsing index and return the number of available elments
+    virtual T next(void)    { T t = b[o]; o = _inc(o); return t; }  // get the next element and increment
+    virtual void done(void) { r = o; }                              // commit the index 
     
 private:
     // buffer

--- a/SerialPipe.h	Mon Oct 21 19:46:34 2013 +0000
+++ b/SerialPipe.h	Fri Oct 25 08:47:22 2013 +0000
@@ -1,21 +1,22 @@
 #pragma once 
 
+#include "mbed.h"
 #include "Pipe.h"
 #include <ctype.h>
 
 class SerialPipe : public Serial
 {
 protected:
-    Pipe<char> rxPipe;
+    Pipe<char> _pipe;
 private:
     void rxIrqBuf(void)
     {
         while (serial_readable(&_serial))
-            rxPipe.putc(serial_getc(&_serial));
+            _pipe.putc(serial_getc(&_serial));
     }
 public:
     SerialPipe(PinName tx, PinName rx, int rxSize = 128, const char* name = NULL) 
-        : Serial(tx,rx,name), rxPipe(rxSize)
+        : Serial(tx,rx,name), _pipe(rxSize)
     {
         attach(this, &SerialPipe::rxIrqBuf, RxIrq);
     }
@@ -31,9 +32,9 @@
         return s; 
     }
     // rx channel
-    int readable(void)              { return rxPipe.readable() ? 1 : 0; } 
-    int getc(void)                  { return rxPipe.getc(); } 
-    int readBuf(char* b, int s)     { return rxPipe.get(b,s); }
+    int readable(void)              { return _pipe.readable() ? 1 : 0; } 
+    int getc(void)                  { return _pipe.getc(); } 
+    int readBuf(char* b, int s)     { return _pipe.get(b,s); }
 
     #define WAIT      -1
     #define NOT_FOUND  0
@@ -43,14 +44,14 @@
     {
         int o = 0;
         int i = 0;
-        int l = rxPipe.start();
+        int l = _pipe.start();
         while ((i < l) && (o < s))
         {
-            int t = rxPipe.next();
+            int t = _pipe.next();
             i ++;
             if (t == '\r')     // terminate commands with carriage return
             {
-                 rxPipe.done();
+                 _pipe.done();
                  return o;          // if enter send the zero char
             }
             else if (t == '\n')     // skip/filter new line 
@@ -63,200 +64,4 @@
         o = 0;
         return WAIT;
     }
-
-    static const char toHex[16];
-
-    #define UBX         0x100000
-    #define NMEA        0x200000
-    #define LENGTH(x)   (x & 0x00FFFF)
-    #define PROTOCOL(x) (x & 0xFF0000)
-    
-    // NMEA
-    int parseNmea(int l)
-    {
-        int o = 0;
-        rxPipe.start();
-        if (++o > l)                    return WAIT;
-        if ('$' != rxPipe.next())       return NOT_FOUND;
-        for (;;)
-        {
-            if (++o > l)                return WAIT;
-            int t = rxPipe.next();
-            if ('\n' == t)              return o;
-            if (!isprint(t) && '\r'!= t) return NOT_FOUND; 
-        }
-    }
-    
-    const char* findNmeaItemPos(int i, const char* s, const char* e)
-    {
-        // find the start
-        for (; (s < e) && (i > 0); s ++)
-        {
-            if (*s == ',')
-                i --;
-        }
-        // found and check bounds
-        if ((i == 0) && (s < e) && 
-            (*s != ',') && (*s != '*') && (*s != '\r') && (*s != '\n'))
-            return s;
-        else 
-            return NULL;
-    }
-
-    bool getNmeaItem(int i, char* b, int s, double& v)
-    {
-        char* e = &b[s];
-        const char* p = findNmeaItemPos(i, b, e);
-        // find the start
-        if (!p || (e <= p))
-            return false;
-        char* t;
-        // M$ specific - because the strtod function uses a strlen we make sure that 
-        // the string is zero terminated, this ensures correct behaviour of the function 
-        char ch = e[-1];
-        e[-1] = '\0';
-        v = strtod(p, &t);
-        // restore the last character
-        e[-1] = ch;
-        return (t > p);
-    }
-
-    bool getNmeaItem(int i, const char* b, int s, int& v, int x /*=10*/)
-    {
-        const char* e = &b[s];
-        const  char* p = findNmeaItemPos(i, b, e);
-        // find the start
-        if (!p)
-            return false;
-        char* t;
-        v = (int)strtol(p, &t, x);
-        return (t > p);
-    }
-
-    bool getNmeaItem(int i, const char* b, int s, char& ch)
-    {
-        const char* e = &b[s];
-        const char* p = findNmeaItemPos(i, b, e);
-        // find the start
-        if (!p)
-            return false;
-        // skip leading spaces
-        while ((p < e) && isspace(*p))
-            p++;
-        // check bound
-        if ((p < e) && 
-            (*p != ',') && (*p != '*') && (*p != '\r') && (*p != '\n'))
-        {
-            ch = *p;
-            return true;
-        }
-        return false;
-    }
-
-    int putNmea(const char* b, int len)
-    {
-        putc('$');
-        int c = 0;
-        for (int i = 0; i < len; i ++)
-        {
-            int t = *b++;
-            putc(t);
-            c ^= t;
-        }
-        putc('*');
-        putc(toHex[(c >> 4) & 0xF]);
-        putc(toHex[(c >> 0) & 0xF]);
-        putc('\r');
-        putc('\n');
-        return len + 6;
-    }
-    
-    int parseUbx(int l)
-    {
-        int o = 0;
-        rxPipe.start();
-        if (++o > l)                    return WAIT;
-        if ('\xB5' != rxPipe.next())    return NOT_FOUND;   
-        if (++o > l)                    return WAIT;
-        if ('b' != rxPipe.next())       return NOT_FOUND;
-        o += 4;
-        if (o > l)                      return WAIT;
-        int i,j,ca,cb;
-        i = rxPipe.next(); ca  = i; cb  = ca; // cls
-        i = rxPipe.next(); ca += i; cb += ca; // id
-        i = rxPipe.next(); ca += i; cb += ca; // len_lsb
-        j = rxPipe.next(); ca += j; cb += ca; // len_msb 
-        j = i + (j << 8);
-        while (j--)
-        {
-            if (++o > l)                return WAIT;
-            i = rxPipe.next(); ca += i; cb += ca;
-        }
-        ca &= 0xFF; cb &= 0xFF;
-        if (++o > l)                    return WAIT;
-        if (ca != rxPipe.next())        return NOT_FOUND;
-        if (++o > l)                    return WAIT;
-        if (cb != rxPipe.next())        return NOT_FOUND;
-        return o;
-    }
-    
-    int putUbx(const unsigned char cls, unsigned char id, unsigned char* b, int len)
-    {
-        putc('\xB5'); // 'µ'
-        putc('b');
-        int ca = cls, cb = cls;
-        putc(cls);
-        ca += id; cb += ca;
-        putc(id);
-        int t = (len >> 0) & 0xFF;
-        ca += t; cb += ca;
-        putc(t);
-        t = (len >> 8) & 0xFF;
-        ca += t; cb += ca;
-        putc(t);
-        for (int i = 0; i < len; i ++)
-        {
-            t = *b++;
-            ca += t; cb += ca;
-            putc(t);
-        }
-        putc(ca & 0xFF);
-        putc(cb & 0xFF);
-        return len + 8;
-    }
-    
-    int getGPS(char* b, int s)
-    {
-        int g = 0;
-        int l = rxPipe.size();
-        if (s > l)
-            s = l;
-        while (s > 0)
-        {
-            // NMEA protocol
-            int n = parseNmea(s);
-            if (n == WAIT)          return g ? g : WAIT;
-            if (n != NOT_FOUND)
-            {
-                if (g != NOT_FOUND) return g; 
-                else                return NMEA | rxPipe.get(b,n);
-            }
-            // UBX protocol
-            int u = parseUbx(s);
-            if (u == WAIT)          return g ? g : WAIT;
-            if (u != NOT_FOUND) 
-            {
-                if (g != NOT_FOUND) return g; 
-                else                return UBX | rxPipe.get(b,u);
-            }
-            // UNKNOWN
-            *b++ = rxPipe.getc();
-            g ++;
-            s--;
-        }
-        if (g != NOT_FOUND)         return g; 
-        return WAIT;
-    }
 };
-
-const char SerialPipe::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };