Simon Pfeifhofer
* add C027_Support fork
Fork of
C027_Support
by 
u-blox
Diff: SerialPipe.h
- Revision:
- 0:cb2d45baaca3
- Child:
- 2:b6012cd91657
diff -r 000000000000 -r cb2d45baaca3 SerialPipe.h
--- /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' };