Xinlei Cao / C027_Support

Own fork of C027_Support

Dependents:   MbedSmartRestMain MbedSmartRestMain

Fork of C027_Support by u-blox

GPS.cpp@2:b6012cd91657, 2013-10-25 (annotated)

Committer:
mazgch
Date:
Fri Oct 25 08:47:22 2013 +0000
Revision:
2:b6012cd91657
Child:
3:c7cd4887560d
GPS for Serial and I2c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mazgch 2:b6012cd91657 1 #include "mbed.h"
mazgch 2:b6012cd91657 2 #include <ctype.h>
mazgch 2:b6012cd91657 3 #include "GPS.h"
mazgch 2:b6012cd91657 4
mazgch 2:b6012cd91657 5 int GPSParser::_getMessage(Pipe<char>* pipe, char* buf, int len)
mazgch 2:b6012cd91657 6 {
mazgch 2:b6012cd91657 7 int unkn = 0;
mazgch 2:b6012cd91657 8 int sz = pipe->size();
mazgch 2:b6012cd91657 9 if (len > sz)
mazgch 2:b6012cd91657 10 len = sz;
mazgch 2:b6012cd91657 11 while (len > 0)
mazgch 2:b6012cd91657 12 {
mazgch 2:b6012cd91657 13 // NMEA protocol
mazgch 2:b6012cd91657 14 int nmea = _parseNmea(pipe,len);
mazgch 2:b6012cd91657 15 if ((nmea != NOT_FOUND) && (unkn > 0)) return unkn;
mazgch 2:b6012cd91657 16 if (nmea == WAIT) return WAIT;
mazgch 2:b6012cd91657 17 if (nmea > 0) return NMEA | pipe->get(buf,nmea);
mazgch 2:b6012cd91657 18 // UBX protocol
mazgch 2:b6012cd91657 19 int ubx = _parseUbx(pipe,len);
mazgch 2:b6012cd91657 20 if ((ubx != NOT_FOUND) && (unkn > 0)) return unkn;
mazgch 2:b6012cd91657 21 if (ubx == WAIT) return WAIT;
mazgch 2:b6012cd91657 22 if (ubx > 0) return UBX | pipe->get(buf,ubx);
mazgch 2:b6012cd91657 23 // UNKNOWN
mazgch 2:b6012cd91657 24 *buf++ = pipe->getc();
mazgch 2:b6012cd91657 25 unkn ++;
mazgch 2:b6012cd91657 26 len--;
mazgch 2:b6012cd91657 27 }
mazgch 2:b6012cd91657 28 if (unkn != NOT_FOUND) return unkn;
mazgch 2:b6012cd91657 29 return WAIT;
mazgch 2:b6012cd91657 30 }
mazgch 2:b6012cd91657 31
mazgch 2:b6012cd91657 32 int GPSParser::_parseNmea(Pipe<char>* pipe, int len)
mazgch 2:b6012cd91657 33 {
mazgch 2:b6012cd91657 34 int ix = 0;
mazgch 2:b6012cd91657 35 pipe->start();
mazgch 2:b6012cd91657 36 if (++ix > len) return WAIT;
mazgch 2:b6012cd91657 37 if ('$' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 38 for (;;)
mazgch 2:b6012cd91657 39 {
mazgch 2:b6012cd91657 40 if (++ix > len) return WAIT;
mazgch 2:b6012cd91657 41 char ch = pipe->next();
mazgch 2:b6012cd91657 42 if ('\n' == ch) return ix;
mazgch 2:b6012cd91657 43 if (!isprint(ch) && '\r'!= ch) return NOT_FOUND;
mazgch 2:b6012cd91657 44 }
mazgch 2:b6012cd91657 45 }
mazgch 2:b6012cd91657 46
mazgch 2:b6012cd91657 47 int GPSParser::_parseUbx(Pipe<char>* pipe, int l)
mazgch 2:b6012cd91657 48 {
mazgch 2:b6012cd91657 49 int o = 0;
mazgch 2:b6012cd91657 50 pipe->start();
mazgch 2:b6012cd91657 51 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 52 if ('\xB5' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 53 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 54 if ('\x62' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 55 o += 4;
mazgch 2:b6012cd91657 56 if (o > l) return WAIT;
mazgch 2:b6012cd91657 57 int i,j,ca,cb;
mazgch 2:b6012cd91657 58 i = pipe->next(); ca = i; cb = ca; // cls
mazgch 2:b6012cd91657 59 i = pipe->next(); ca += i; cb += ca; // id
mazgch 2:b6012cd91657 60 i = pipe->next(); ca += i; cb += ca; // len_lsb
mazgch 2:b6012cd91657 61 j = pipe->next(); ca += j; cb += ca; // len_msb
mazgch 2:b6012cd91657 62 j = i + (j << 8);
mazgch 2:b6012cd91657 63 while (j--)
mazgch 2:b6012cd91657 64 {
mazgch 2:b6012cd91657 65 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 66 i = pipe->next();
mazgch 2:b6012cd91657 67 ca += i;
mazgch 2:b6012cd91657 68 cb += ca;
mazgch 2:b6012cd91657 69 }
mazgch 2:b6012cd91657 70 ca &= 0xFF; cb &= 0xFF;
mazgch 2:b6012cd91657 71 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 72 if (ca != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 73 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 74 if (cb != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 75 return o;
mazgch 2:b6012cd91657 76 }
mazgch 2:b6012cd91657 77
mazgch 2:b6012cd91657 78 int GPSParser::_putNmea(Stream* stream, const char* buf, int len)
mazgch 2:b6012cd91657 79 {
mazgch 2:b6012cd91657 80 stream->putc('$');
mazgch 2:b6012cd91657 81 int c = 0;
mazgch 2:b6012cd91657 82 for (int i = 0; i < len; i ++)
mazgch 2:b6012cd91657 83 {
mazgch 2:b6012cd91657 84 int t = *buf++;
mazgch 2:b6012cd91657 85 stream->putc(t);
mazgch 2:b6012cd91657 86 c ^= t;
mazgch 2:b6012cd91657 87 }
mazgch 2:b6012cd91657 88 stream->putc('*');
mazgch 2:b6012cd91657 89 stream->putc(toHex[(c >> 4) & 0xF]);
mazgch 2:b6012cd91657 90 stream->putc(toHex[(c >> 0) & 0xF]);
mazgch 2:b6012cd91657 91 stream->putc('\r');
mazgch 2:b6012cd91657 92 stream->putc('\n');
mazgch 2:b6012cd91657 93 return len + 6;
mazgch 2:b6012cd91657 94 }
mazgch 2:b6012cd91657 95
mazgch 2:b6012cd91657 96 int GPSParser::_putUbx(Stream* stream, const unsigned char cls, unsigned char id, unsigned char* buf, int len)
mazgch 2:b6012cd91657 97 {
mazgch 2:b6012cd91657 98 stream->putc('\xB5'); // 'µ'
mazgch 2:b6012cd91657 99 stream->putc('\x62'); // 'b'
mazgch 2:b6012cd91657 100 int ca = cls, cb = cls;
mazgch 2:b6012cd91657 101 stream->putc(cls);
mazgch 2:b6012cd91657 102 ca += id; cb += ca;
mazgch 2:b6012cd91657 103 stream->putc(id);
mazgch 2:b6012cd91657 104 int t = (len >> 0) & 0xFF;
mazgch 2:b6012cd91657 105 ca += t; cb += ca;
mazgch 2:b6012cd91657 106 stream->putc(t);
mazgch 2:b6012cd91657 107 t = (len >> 8) & 0xFF;
mazgch 2:b6012cd91657 108 ca += t; cb += ca;
mazgch 2:b6012cd91657 109 stream->putc(t);
mazgch 2:b6012cd91657 110 for (int i = 0; i < len; i ++)
mazgch 2:b6012cd91657 111 {
mazgch 2:b6012cd91657 112 t = *buf++;
mazgch 2:b6012cd91657 113 ca += t; cb += ca;
mazgch 2:b6012cd91657 114 stream->putc(t);
mazgch 2:b6012cd91657 115 }
mazgch 2:b6012cd91657 116 stream->putc(ca & 0xFF);
mazgch 2:b6012cd91657 117 stream->putc(cb & 0xFF);
mazgch 2:b6012cd91657 118 return len + 8;
mazgch 2:b6012cd91657 119 }
mazgch 2:b6012cd91657 120
mazgch 2:b6012cd91657 121 const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end)
mazgch 2:b6012cd91657 122 {
mazgch 2:b6012cd91657 123 // find the start
mazgch 2:b6012cd91657 124 for (; (start < end) && (ix > 0); start ++)
mazgch 2:b6012cd91657 125 {
mazgch 2:b6012cd91657 126 if (*start == ',')
mazgch 2:b6012cd91657 127 ix --;
mazgch 2:b6012cd91657 128 }
mazgch 2:b6012cd91657 129 // found and check bounds
mazgch 2:b6012cd91657 130 if ((ix == 0) && (start < end) &&
mazgch 2:b6012cd91657 131 (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
mazgch 2:b6012cd91657 132 return start;
mazgch 2:b6012cd91657 133 else
mazgch 2:b6012cd91657 134 return NULL;
mazgch 2:b6012cd91657 135 }
mazgch 2:b6012cd91657 136
mazgch 2:b6012cd91657 137 bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val)
mazgch 2:b6012cd91657 138 {
mazgch 2:b6012cd91657 139 char* end = &buf[len];
mazgch 2:b6012cd91657 140 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 141 // find the start
mazgch 2:b6012cd91657 142 if (!pos)
mazgch 2:b6012cd91657 143 return false;
mazgch 2:b6012cd91657 144 val = strtod(pos, &end);
mazgch 2:b6012cd91657 145 // restore the last character
mazgch 2:b6012cd91657 146 return (end > pos);
mazgch 2:b6012cd91657 147 }
mazgch 2:b6012cd91657 148
mazgch 2:b6012cd91657 149 bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
mazgch 2:b6012cd91657 150 {
mazgch 2:b6012cd91657 151 char* end = &buf[len];
mazgch 2:b6012cd91657 152 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 153 // find the start
mazgch 2:b6012cd91657 154 if (!pos)
mazgch 2:b6012cd91657 155 return false;
mazgch 2:b6012cd91657 156 val = (int)strtol(pos, &end, base);
mazgch 2:b6012cd91657 157 return (end > pos);
mazgch 2:b6012cd91657 158 }
mazgch 2:b6012cd91657 159
mazgch 2:b6012cd91657 160 bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val)
mazgch 2:b6012cd91657 161 {
mazgch 2:b6012cd91657 162 const char* end = &buf[len];
mazgch 2:b6012cd91657 163 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 164 // find the start
mazgch 2:b6012cd91657 165 if (!pos)
mazgch 2:b6012cd91657 166 return false;
mazgch 2:b6012cd91657 167 // skip leading spaces
mazgch 2:b6012cd91657 168 while ((pos < end) && isspace(*pos))
mazgch 2:b6012cd91657 169 pos++;
mazgch 2:b6012cd91657 170 // check bound
mazgch 2:b6012cd91657 171 if ((pos < end) &&
mazgch 2:b6012cd91657 172 (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
mazgch 2:b6012cd91657 173 {
mazgch 2:b6012cd91657 174 val = *pos;
mazgch 2:b6012cd91657 175 return true;
mazgch 2:b6012cd91657 176 }
mazgch 2:b6012cd91657 177 return false;
mazgch 2:b6012cd91657 178 }
mazgch 2:b6012cd91657 179
mazgch 2:b6012cd91657 180 const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
mazgch 2:b6012cd91657 181
mazgch 2:b6012cd91657 182 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 183 // Serial Implementation
mazgch 2:b6012cd91657 184 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 185
mazgch 2:b6012cd91657 186 GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/) :
mazgch 2:b6012cd91657 187 Serial(tx, rx, "gps"), _pipe(256)
mazgch 2:b6012cd91657 188 {
mazgch 2:b6012cd91657 189 attach(this, &GPSSerial::serialRxIrq, RxIrq);
mazgch 2:b6012cd91657 190 baud(baudrate);
mazgch 2:b6012cd91657 191 }
mazgch 2:b6012cd91657 192
mazgch 2:b6012cd91657 193 GPSSerial::~GPSSerial(void)
mazgch 2:b6012cd91657 194 {
mazgch 2:b6012cd91657 195 attach(NULL, RxIrq);
mazgch 2:b6012cd91657 196 }
mazgch 2:b6012cd91657 197
mazgch 2:b6012cd91657 198 void GPSSerial::serialRxIrq(void)
mazgch 2:b6012cd91657 199 {
mazgch 2:b6012cd91657 200 while (serial_readable(&_serial))
mazgch 2:b6012cd91657 201 _pipe.putc(serial_getc(&_serial));
mazgch 2:b6012cd91657 202 }
mazgch 2:b6012cd91657 203
mazgch 2:b6012cd91657 204 int GPSSerial::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 205 {
mazgch 2:b6012cd91657 206 return _getMessage(&_pipe, buf, len);
mazgch 2:b6012cd91657 207 }
mazgch 2:b6012cd91657 208
mazgch 2:b6012cd91657 209 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 210 // I2C Implementation
mazgch 2:b6012cd91657 211 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 212
mazgch 2:b6012cd91657 213 GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/) :
mazgch 2:b6012cd91657 214 I2C(sda,scl),
mazgch 2:b6012cd91657 215 _pipe(256)
mazgch 2:b6012cd91657 216 {
mazgch 2:b6012cd91657 217 found = false;
mazgch 2:b6012cd91657 218 }
mazgch 2:b6012cd91657 219
mazgch 2:b6012cd91657 220 bool GPSI2C::detect(void)
mazgch 2:b6012cd91657 221 {
mazgch 2:b6012cd91657 222 if (!found)
mazgch 2:b6012cd91657 223 {
mazgch 2:b6012cd91657 224 int w = I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM));
mazgch 2:b6012cd91657 225 if (w == 0)
mazgch 2:b6012cd91657 226 found = true;
mazgch 2:b6012cd91657 227 }
mazgch 2:b6012cd91657 228 return found;
mazgch 2:b6012cd91657 229 }
mazgch 2:b6012cd91657 230
mazgch 2:b6012cd91657 231 int GPSI2C::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 232 {
mazgch 2:b6012cd91657 233 int sz = _get(buf, len);
mazgch 2:b6012cd91657 234 if (sz)
mazgch 2:b6012cd91657 235 _pipe.put(buf, sz);
mazgch 2:b6012cd91657 236 return _getMessage(&_pipe, buf, len);
mazgch 2:b6012cd91657 237 }
mazgch 2:b6012cd91657 238
mazgch 2:b6012cd91657 239 int GPSI2C::_get(char* buf, int len)
mazgch 2:b6012cd91657 240 {
mazgch 2:b6012cd91657 241 unsigned char sz[2];
mazgch 2:b6012cd91657 242 if (!I2C::write(GPSADR,&REGLEN,sizeof(REGLEN),true) &&
mazgch 2:b6012cd91657 243 !I2C::read(GPSADR,(char*)sz,sizeof(sz),true))
mazgch 2:b6012cd91657 244 {
mazgch 2:b6012cd91657 245 int size = 256 * (int)sz[0] + sz[1];
mazgch 2:b6012cd91657 246 if (size > len)
mazgch 2:b6012cd91657 247 size = len;
mazgch 2:b6012cd91657 248 if (size > 0)
mazgch 2:b6012cd91657 249 {
mazgch 2:b6012cd91657 250 if (!I2C::read(GPSADR,buf,size))
mazgch 2:b6012cd91657 251 {
mazgch 2:b6012cd91657 252 found = true;
mazgch 2:b6012cd91657 253 return size;
mazgch 2:b6012cd91657 254 }
mazgch 2:b6012cd91657 255 // error reading data
mazgch 2:b6012cd91657 256 found = false;
mazgch 2:b6012cd91657 257 return 0;
mazgch 2:b6012cd91657 258 }
mazgch 2:b6012cd91657 259 else
mazgch 2:b6012cd91657 260 {
mazgch 2:b6012cd91657 261 found = true;
mazgch 2:b6012cd91657 262 // no data -> ok
mazgch 2:b6012cd91657 263 }
mazgch 2:b6012cd91657 264 }
mazgch 2:b6012cd91657 265 else
mazgch 2:b6012cd91657 266 {
mazgch 2:b6012cd91657 267 // error setting address and reading length
mazgch 2:b6012cd91657 268 found = false;
mazgch 2:b6012cd91657 269 }
mazgch 2:b6012cd91657 270 I2C::stop();
mazgch 2:b6012cd91657 271 return 0;
mazgch 2:b6012cd91657 272 }
mazgch 2:b6012cd91657 273
mazgch 2:b6012cd91657 274 int GPSI2C::_put(const char* buf, int len)
mazgch 2:b6012cd91657 275 {
mazgch 2:b6012cd91657 276 if (len == 0)
mazgch 2:b6012cd91657 277 return 0;
mazgch 2:b6012cd91657 278 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true) &&
mazgch 2:b6012cd91657 279 !I2C::write(GPSADR,buf,len,false))
mazgch 2:b6012cd91657 280 {
mazgch 2:b6012cd91657 281 found = true;
mazgch 2:b6012cd91657 282 return len;
mazgch 2:b6012cd91657 283 }
mazgch 2:b6012cd91657 284 found = false;
mazgch 2:b6012cd91657 285 return 0;
mazgch 2:b6012cd91657 286 }
mazgch 2:b6012cd91657 287
mazgch 2:b6012cd91657 288 const char GPSI2C::REGLEN = 0xFD;
mazgch 2:b6012cd91657 289 const char GPSI2C::REGSTREAM = 0xFF;