Hiroaki Okoshi / C027_Support_mbed-os

C027 の mbes os 5 での動作を確認しました。 I confirmed the operation at mbes os 5 of C 027.

Fork of C027_Support by u-blox

SerialPipe.h@0:cb2d45baaca3, 2013-10-20 (annotated)

Committer:
mazgch
Date:
Sun Oct 20 15:12:52 2013 +0000
Revision:
0:cb2d45baaca3
Child:
2:b6012cd91657
moved to lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mazgch 0:cb2d45baaca3 1 #pragma once
mazgch 0:cb2d45baaca3 2
mazgch 0:cb2d45baaca3 3 #include "Pipe.h"
mazgch 0:cb2d45baaca3 4 #include <ctype.h>
mazgch 0:cb2d45baaca3 5
mazgch 0:cb2d45baaca3 6 class SerialPipe : public Serial
mazgch 0:cb2d45baaca3 7 {
mazgch 0:cb2d45baaca3 8 protected:
mazgch 0:cb2d45baaca3 9 Pipe<char> rxPipe;
mazgch 0:cb2d45baaca3 10 private:
mazgch 0:cb2d45baaca3 11 void rxIrqBuf(void)
mazgch 0:cb2d45baaca3 12 {
mazgch 0:cb2d45baaca3 13 while (serial_readable(&_serial))
mazgch 0:cb2d45baaca3 14 rxPipe.putc(serial_getc(&_serial));
mazgch 0:cb2d45baaca3 15 }
mazgch 0:cb2d45baaca3 16 public:
mazgch 0:cb2d45baaca3 17 SerialPipe(PinName tx, PinName rx, int rxSize = 128, const char* name = NULL)
mazgch 0:cb2d45baaca3 18 : Serial(tx,rx,name), rxPipe(rxSize)
mazgch 0:cb2d45baaca3 19 {
mazgch 0:cb2d45baaca3 20 attach(this, &SerialPipe::rxIrqBuf, RxIrq);
mazgch 0:cb2d45baaca3 21 }
mazgch 0:cb2d45baaca3 22 virtual ~SerialPipe(void)
mazgch 0:cb2d45baaca3 23 {
mazgch 0:cb2d45baaca3 24 attach(NULL, RxIrq);
mazgch 0:cb2d45baaca3 25 }
mazgch 0:cb2d45baaca3 26 // tx channel
mazgch 0:cb2d45baaca3 27 int writeBuf(char* b, int s)
mazgch 0:cb2d45baaca3 28 {
mazgch 0:cb2d45baaca3 29 for (int i = 0; i < s; i ++)
mazgch 0:cb2d45baaca3 30 putc(b[i]);
mazgch 0:cb2d45baaca3 31 return s;
mazgch 0:cb2d45baaca3 32 }
mazgch 0:cb2d45baaca3 33 // rx channel
mazgch 0:cb2d45baaca3 34 int readable(void) { return rxPipe.readable() ? 1 : 0; }
mazgch 0:cb2d45baaca3 35 int getc(void) { return rxPipe.getc(); }
mazgch 0:cb2d45baaca3 36 int readBuf(char* b, int s) { return rxPipe.get(b,s); }
mazgch 0:cb2d45baaca3 37
mazgch 0:cb2d45baaca3 38 #define WAIT -1
mazgch 0:cb2d45baaca3 39 #define NOT_FOUND 0
mazgch 0:cb2d45baaca3 40
mazgch 0:cb2d45baaca3 41 // special parsing
mazgch 0:cb2d45baaca3 42 int getLine(char* b, int s)
mazgch 0:cb2d45baaca3 43 {
mazgch 0:cb2d45baaca3 44 int o = 0;
mazgch 0:cb2d45baaca3 45 int i = 0;
mazgch 0:cb2d45baaca3 46 int l = rxPipe.start();
mazgch 0:cb2d45baaca3 47 while ((i < l) && (o < s))
mazgch 0:cb2d45baaca3 48 {
mazgch 0:cb2d45baaca3 49 int t = rxPipe.next();
mazgch 0:cb2d45baaca3 50 i ++;
mazgch 0:cb2d45baaca3 51 if (t == '\r') // terminate commands with carriage return
mazgch 0:cb2d45baaca3 52 {
mazgch 0:cb2d45baaca3 53 rxPipe.done();
mazgch 0:cb2d45baaca3 54 return o; // if enter send the zero char
mazgch 0:cb2d45baaca3 55 }
mazgch 0:cb2d45baaca3 56 else if (t == '\n') // skip/filter new line
mazgch 0:cb2d45baaca3 57 /* skip */;
mazgch 0:cb2d45baaca3 58 else if (t != '\b') // normal char (no backspace)
mazgch 0:cb2d45baaca3 59 b[o++] = t;
mazgch 0:cb2d45baaca3 60 else if (o > 0) // backspace
mazgch 0:cb2d45baaca3 61 o --; // remove it
mazgch 0:cb2d45baaca3 62 }
mazgch 0:cb2d45baaca3 63 o = 0;
mazgch 0:cb2d45baaca3 64 return WAIT;
mazgch 0:cb2d45baaca3 65 }
mazgch 0:cb2d45baaca3 66
mazgch 0:cb2d45baaca3 67 static const char toHex[16];
mazgch 0:cb2d45baaca3 68
mazgch 0:cb2d45baaca3 69 #define UBX 0x100000
mazgch 0:cb2d45baaca3 70 #define NMEA 0x200000
mazgch 0:cb2d45baaca3 71 #define LENGTH(x) (x & 0x00FFFF)
mazgch 0:cb2d45baaca3 72 #define PROTOCOL(x) (x & 0xFF0000)
mazgch 0:cb2d45baaca3 73
mazgch 0:cb2d45baaca3 74 // NMEA
mazgch 0:cb2d45baaca3 75 int parseNmea(int l)
mazgch 0:cb2d45baaca3 76 {
mazgch 0:cb2d45baaca3 77 int o = 0;
mazgch 0:cb2d45baaca3 78 rxPipe.start();
mazgch 0:cb2d45baaca3 79 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 80 if ('$' != rxPipe.next()) return NOT_FOUND;
mazgch 0:cb2d45baaca3 81 for (;;)
mazgch 0:cb2d45baaca3 82 {
mazgch 0:cb2d45baaca3 83 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 84 int t = rxPipe.next();
mazgch 0:cb2d45baaca3 85 if ('\n' == t) return o;
mazgch 0:cb2d45baaca3 86 if (!isprint(t) && '\r'!= t) return NOT_FOUND;
mazgch 0:cb2d45baaca3 87 }
mazgch 0:cb2d45baaca3 88 }
mazgch 0:cb2d45baaca3 89
mazgch 0:cb2d45baaca3 90 const char* findNmeaItemPos(int i, const char* s, const char* e)
mazgch 0:cb2d45baaca3 91 {
mazgch 0:cb2d45baaca3 92 // find the start
mazgch 0:cb2d45baaca3 93 for (; (s < e) && (i > 0); s ++)
mazgch 0:cb2d45baaca3 94 {
mazgch 0:cb2d45baaca3 95 if (*s == ',')
mazgch 0:cb2d45baaca3 96 i --;
mazgch 0:cb2d45baaca3 97 }
mazgch 0:cb2d45baaca3 98 // found and check bounds
mazgch 0:cb2d45baaca3 99 if ((i == 0) && (s < e) &&
mazgch 0:cb2d45baaca3 100 (*s != ',') && (*s != '*') && (*s != '\r') && (*s != '\n'))
mazgch 0:cb2d45baaca3 101 return s;
mazgch 0:cb2d45baaca3 102 else
mazgch 0:cb2d45baaca3 103 return NULL;
mazgch 0:cb2d45baaca3 104 }
mazgch 0:cb2d45baaca3 105
mazgch 0:cb2d45baaca3 106 bool getNmeaItem(int i, char* b, int s, double& v)
mazgch 0:cb2d45baaca3 107 {
mazgch 0:cb2d45baaca3 108 char* e = &b[s];
mazgch 0:cb2d45baaca3 109 const char* p = findNmeaItemPos(i, b, e);
mazgch 0:cb2d45baaca3 110 // find the start
mazgch 0:cb2d45baaca3 111 if (!p || (e <= p))
mazgch 0:cb2d45baaca3 112 return false;
mazgch 0:cb2d45baaca3 113 char* t;
mazgch 0:cb2d45baaca3 114 // M$ specific - because the strtod function uses a strlen we make sure that
mazgch 0:cb2d45baaca3 115 // the string is zero terminated, this ensures correct behaviour of the function
mazgch 0:cb2d45baaca3 116 char ch = e[-1];
mazgch 0:cb2d45baaca3 117 e[-1] = '\0';
mazgch 0:cb2d45baaca3 118 v = strtod(p, &t);
mazgch 0:cb2d45baaca3 119 // restore the last character
mazgch 0:cb2d45baaca3 120 e[-1] = ch;
mazgch 0:cb2d45baaca3 121 return (t > p);
mazgch 0:cb2d45baaca3 122 }
mazgch 0:cb2d45baaca3 123
mazgch 0:cb2d45baaca3 124 bool getNmeaItem(int i, const char* b, int s, int& v, int x /*=10*/)
mazgch 0:cb2d45baaca3 125 {
mazgch 0:cb2d45baaca3 126 const char* e = &b[s];
mazgch 0:cb2d45baaca3 127 const char* p = findNmeaItemPos(i, b, e);
mazgch 0:cb2d45baaca3 128 // find the start
mazgch 0:cb2d45baaca3 129 if (!p)
mazgch 0:cb2d45baaca3 130 return false;
mazgch 0:cb2d45baaca3 131 char* t;
mazgch 0:cb2d45baaca3 132 v = (int)strtol(p, &t, x);
mazgch 0:cb2d45baaca3 133 return (t > p);
mazgch 0:cb2d45baaca3 134 }
mazgch 0:cb2d45baaca3 135
mazgch 0:cb2d45baaca3 136 bool getNmeaItem(int i, const char* b, int s, char& ch)
mazgch 0:cb2d45baaca3 137 {
mazgch 0:cb2d45baaca3 138 const char* e = &b[s];
mazgch 0:cb2d45baaca3 139 const char* p = findNmeaItemPos(i, b, e);
mazgch 0:cb2d45baaca3 140 // find the start
mazgch 0:cb2d45baaca3 141 if (!p)
mazgch 0:cb2d45baaca3 142 return false;
mazgch 0:cb2d45baaca3 143 // skip leading spaces
mazgch 0:cb2d45baaca3 144 while ((p < e) && isspace(*p))
mazgch 0:cb2d45baaca3 145 p++;
mazgch 0:cb2d45baaca3 146 // check bound
mazgch 0:cb2d45baaca3 147 if ((p < e) &&
mazgch 0:cb2d45baaca3 148 (*p != ',') && (*p != '*') && (*p != '\r') && (*p != '\n'))
mazgch 0:cb2d45baaca3 149 {
mazgch 0:cb2d45baaca3 150 ch = *p;
mazgch 0:cb2d45baaca3 151 return true;
mazgch 0:cb2d45baaca3 152 }
mazgch 0:cb2d45baaca3 153 return false;
mazgch 0:cb2d45baaca3 154 }
mazgch 0:cb2d45baaca3 155
mazgch 0:cb2d45baaca3 156 int putNmea(const char* b, int len)
mazgch 0:cb2d45baaca3 157 {
mazgch 0:cb2d45baaca3 158 putc('$');
mazgch 0:cb2d45baaca3 159 int c = 0;
mazgch 0:cb2d45baaca3 160 for (int i = 0; i < len; i ++)
mazgch 0:cb2d45baaca3 161 {
mazgch 0:cb2d45baaca3 162 int t = *b++;
mazgch 0:cb2d45baaca3 163 putc(t);
mazgch 0:cb2d45baaca3 164 c ^= t;
mazgch 0:cb2d45baaca3 165 }
mazgch 0:cb2d45baaca3 166 putc('*');
mazgch 0:cb2d45baaca3 167 putc(toHex[(c >> 4) & 0xF]);
mazgch 0:cb2d45baaca3 168 putc(toHex[(c >> 0) & 0xF]);
mazgch 0:cb2d45baaca3 169 putc('\r');
mazgch 0:cb2d45baaca3 170 putc('\n');
mazgch 0:cb2d45baaca3 171 return len + 6;
mazgch 0:cb2d45baaca3 172 }
mazgch 0:cb2d45baaca3 173
mazgch 0:cb2d45baaca3 174 int parseUbx(int l)
mazgch 0:cb2d45baaca3 175 {
mazgch 0:cb2d45baaca3 176 int o = 0;
mazgch 0:cb2d45baaca3 177 rxPipe.start();
mazgch 0:cb2d45baaca3 178 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 179 if ('\xB5' != rxPipe.next()) return NOT_FOUND;
mazgch 0:cb2d45baaca3 180 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 181 if ('b' != rxPipe.next()) return NOT_FOUND;
mazgch 0:cb2d45baaca3 182 o += 4;
mazgch 0:cb2d45baaca3 183 if (o > l) return WAIT;
mazgch 0:cb2d45baaca3 184 int i,j,ca,cb;
mazgch 0:cb2d45baaca3 185 i = rxPipe.next(); ca = i; cb = ca; // cls
mazgch 0:cb2d45baaca3 186 i = rxPipe.next(); ca += i; cb += ca; // id
mazgch 0:cb2d45baaca3 187 i = rxPipe.next(); ca += i; cb += ca; // len_lsb
mazgch 0:cb2d45baaca3 188 j = rxPipe.next(); ca += j; cb += ca; // len_msb
mazgch 0:cb2d45baaca3 189 j = i + (j << 8);
mazgch 0:cb2d45baaca3 190 while (j--)
mazgch 0:cb2d45baaca3 191 {
mazgch 0:cb2d45baaca3 192 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 193 i = rxPipe.next(); ca += i; cb += ca;
mazgch 0:cb2d45baaca3 194 }
mazgch 0:cb2d45baaca3 195 ca &= 0xFF; cb &= 0xFF;
mazgch 0:cb2d45baaca3 196 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 197 if (ca != rxPipe.next()) return NOT_FOUND;
mazgch 0:cb2d45baaca3 198 if (++o > l) return WAIT;
mazgch 0:cb2d45baaca3 199 if (cb != rxPipe.next()) return NOT_FOUND;
mazgch 0:cb2d45baaca3 200 return o;
mazgch 0:cb2d45baaca3 201 }
mazgch 0:cb2d45baaca3 202
mazgch 0:cb2d45baaca3 203 int putUbx(const unsigned char cls, unsigned char id, unsigned char* b, int len)
mazgch 0:cb2d45baaca3 204 {
mazgch 0:cb2d45baaca3 205 putc('\xB5'); // 'µ'
mazgch 0:cb2d45baaca3 206 putc('b');
mazgch 0:cb2d45baaca3 207 int ca = cls, cb = cls;
mazgch 0:cb2d45baaca3 208 putc(cls);
mazgch 0:cb2d45baaca3 209 ca += id; cb += ca;
mazgch 0:cb2d45baaca3 210 putc(id);
mazgch 0:cb2d45baaca3 211 int t = (len >> 0) & 0xFF;
mazgch 0:cb2d45baaca3 212 ca += t; cb += ca;
mazgch 0:cb2d45baaca3 213 putc(t);
mazgch 0:cb2d45baaca3 214 t = (len >> 8) & 0xFF;
mazgch 0:cb2d45baaca3 215 ca += t; cb += ca;
mazgch 0:cb2d45baaca3 216 putc(t);
mazgch 0:cb2d45baaca3 217 for (int i = 0; i < len; i ++)
mazgch 0:cb2d45baaca3 218 {
mazgch 0:cb2d45baaca3 219 t = *b++;
mazgch 0:cb2d45baaca3 220 ca += t; cb += ca;
mazgch 0:cb2d45baaca3 221 putc(t);
mazgch 0:cb2d45baaca3 222 }
mazgch 0:cb2d45baaca3 223 putc(ca & 0xFF);
mazgch 0:cb2d45baaca3 224 putc(cb & 0xFF);
mazgch 0:cb2d45baaca3 225 return len + 8;
mazgch 0:cb2d45baaca3 226 }
mazgch 0:cb2d45baaca3 227
mazgch 0:cb2d45baaca3 228 int getGPS(char* b, int s)
mazgch 0:cb2d45baaca3 229 {
mazgch 0:cb2d45baaca3 230 int g = 0;
mazgch 0:cb2d45baaca3 231 int l = rxPipe.size();
mazgch 0:cb2d45baaca3 232 if (s > l)
mazgch 0:cb2d45baaca3 233 s = l;
mazgch 0:cb2d45baaca3 234 while (s > 0)
mazgch 0:cb2d45baaca3 235 {
mazgch 0:cb2d45baaca3 236 // NMEA protocol
mazgch 0:cb2d45baaca3 237 int n = parseNmea(s);
mazgch 0:cb2d45baaca3 238 if (n == WAIT) return g ? g : WAIT;
mazgch 0:cb2d45baaca3 239 if (n != NOT_FOUND)
mazgch 0:cb2d45baaca3 240 {
mazgch 0:cb2d45baaca3 241 if (g != NOT_FOUND) return g;
mazgch 0:cb2d45baaca3 242 else return NMEA | rxPipe.get(b,n);
mazgch 0:cb2d45baaca3 243 }
mazgch 0:cb2d45baaca3 244 // UBX protocol
mazgch 0:cb2d45baaca3 245 int u = parseUbx(s);
mazgch 0:cb2d45baaca3 246 if (u == WAIT) return g ? g : WAIT;
mazgch 0:cb2d45baaca3 247 if (u != NOT_FOUND)
mazgch 0:cb2d45baaca3 248 {
mazgch 0:cb2d45baaca3 249 if (g != NOT_FOUND) return g;
mazgch 0:cb2d45baaca3 250 else return UBX | rxPipe.get(b,u);
mazgch 0:cb2d45baaca3 251 }
mazgch 0:cb2d45baaca3 252 // UNKNOWN
mazgch 0:cb2d45baaca3 253 *b++ = rxPipe.getc();
mazgch 0:cb2d45baaca3 254 g ++;
mazgch 0:cb2d45baaca3 255 s--;
mazgch 0:cb2d45baaca3 256 }
mazgch 0:cb2d45baaca3 257 if (g != NOT_FOUND) return g;
mazgch 0:cb2d45baaca3 258 return WAIT;
mazgch 0:cb2d45baaca3 259 }
mazgch 0:cb2d45baaca3 260 };
mazgch 0:cb2d45baaca3 261
mazgch 0:cb2d45baaca3 262 const char SerialPipe::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };