Revision 0:cb2d45baaca3, committed 2013-10-20

Comitter:

mazgch

Date:

Sun Oct 20 15:12:52 2013 +0000

Child:

1:f41579f4e2ed

Commit message:

moved to lib

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pipe.h	Sun Oct 20 15:12:52 2013 +0000
@@ -0,0 +1,130 @@
+#pragma once 
+
+template <class T>
+class Pipe
+{
+private:
+    inline int _inc(int i, int n = 1)
+    {
+        i += n;
+        if (i >= s)
+            i -= s;
+        return i;
+    }
+public:
+    Pipe(int n)
+    {
+        r = 0;
+        w = 0;
+        n ++; // we add one more element to be able to identify empty from full
+        b = new T[n];
+        s = n;
+    }    
+    virtual ~Pipe()
+    {
+        delete [] b;
+    }
+    // writing thread
+    bool writeable(void) // = not full
+    {
+        int i = _inc(w);
+        return (i = r);
+    }
+    int free(void)      // number of elements that can be added
+    {
+        int t = r - w;
+        if (t <= 0)
+            t += s;
+        return t - 1;
+    }
+    void putc(T c)
+    {
+        int i = w;
+        int j = i;
+        i = _inc(i);
+        //assert(i != r);
+        b[j] = c;
+        w = i;   
+    }
+    int put(T* p, int n)
+    {
+        int f = free();
+        if (f < n)
+            n = f;
+        if (n)
+        {
+            int m = s - w;
+            if (m > n)
+                m = n;
+            memcpy(&b[w], &p[0], m);
+            int t = n - m;
+            if (t)
+            {
+                memcpy(&b[0], &p[m], t);
+                w = t;
+            }
+            else 
+                w += m; 
+        }
+        return n;
+    }
+    // reading thread 
+    // --------------------------------------------------------
+    //! check if there are any values available
+    bool readable(void) // = not empty
+    {
+        return (r != w);
+    }
+    //! get the number of values avialable in the buffer 
+    int size(void)
+    {
+        int t = w - r;
+        if (t < 0)
+            t += s;
+        return t;
+    }
+    //! get a value from buffer (this function will block if no values available)
+    T getc(void)
+    {
+        //while (r == w)
+        //    /* just wait until w changes*/;
+        T t = b[r];
+        r = _inc(r);
+        return t;
+    }
+    // get values from buffer (if the buffer has less values, only the values avilable are returned)
+    int get(T* p, int n)
+    {
+        int f = size();
+        if (f < n)
+            n = f;
+        if (n)
+        {
+            int m = s - r;
+            if (m > n)
+                m = n;
+            memcpy(&p[0], &b[r], m);
+            int t = n - m;
+            if (t)
+            {
+                memcpy(&p[m] ,&b[0], t);
+                r = t;
+            }
+            else 
+                r += m;
+        }
+        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 
+    
+private:
+    // buffer
+    T*            b; //!< buffer
+    int           s; //!< size of buffer (s - 1) elements can be stored
+    volatile int  w; //! write index 
+    volatile int  r; //! read index 
+    int           o; //! offest index used by parsing functions  
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialPipe.h	Sun Oct 20 15:12:52 2013 +0000
@@ -0,0 +1,262 @@
+#pragma once 
+
+#include "Pipe.h"
+#include <ctype.h>
+
+class SerialPipe : public Serial
+{
+protected:
+    Pipe<char> rxPipe;
+private:
+    void rxIrqBuf(void)
+    {
+        while (serial_readable(&_serial))
+            rxPipe.putc(serial_getc(&_serial));
+    }
+public:
+    SerialPipe(PinName tx, PinName rx, int rxSize = 128, const char* name = NULL) 
+        : Serial(tx,rx,name), rxPipe(rxSize)
+    {
+        attach(this, &SerialPipe::rxIrqBuf, RxIrq);
+    }
+    virtual ~SerialPipe(void)
+    {
+        attach(NULL, RxIrq);
+    }
+    // tx channel
+    int writeBuf(char* b, int s)    
+    { 
+        for (int i = 0; i < s; i ++)
+            putc(b[i]);
+        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); }
+
+    #define WAIT      -1
+    #define NOT_FOUND  0
+
+    // special parsing
+    int getLine(char* b, int s)
+    {
+        int o = 0;
+        int i = 0;
+        int l = rxPipe.start();
+        while ((i < l) && (o < s))
+        {
+            int t = rxPipe.next();
+            i ++;
+            if (t == '\r')     // terminate commands with carriage return
+            {
+                 rxPipe.done();
+                 return o;          // if enter send the zero char
+            }
+            else if (t == '\n')     // skip/filter new line 
+                 /* skip */;
+            else if (t != '\b')     // normal char (no backspace)
+                b[o++] = t;
+            else if (o > 0)         // backspace
+                o --;               // remove it
+        }
+        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' };