Simon Pfeifhofer / C027_Support

* add C027_Support fork

Fork of C027_Support by u-blox

GPS.cpp@15:5eda64e5b9d1, 2013-11-22 (annotated)

Committer:
mazgch
Date:
Fri Nov 22 08:23:30 2013 +0000
Revision:
15:5eda64e5b9d1
Parent:
11:b084552b03fe
Child:
18:e5697801df29
- rebase the serial pipe on SerialBase; - fix nonblocking put

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 9:e7a5959ffae1 9 int fr = pipe->free();
mazgch 2:b6012cd91657 10 if (len > sz)
mazgch 2:b6012cd91657 11 len = sz;
mazgch 2:b6012cd91657 12 while (len > 0)
mazgch 2:b6012cd91657 13 {
mazgch 2:b6012cd91657 14 // NMEA protocol
mazgch 2:b6012cd91657 15 int nmea = _parseNmea(pipe,len);
mazgch 9:e7a5959ffae1 16 if ((nmea != NOT_FOUND) && (unkn > 0))
mazgch 9:e7a5959ffae1 17 return unkn;
mazgch 9:e7a5959ffae1 18 if (nmea == WAIT && fr)
mazgch 9:e7a5959ffae1 19 return WAIT;
mazgch 9:e7a5959ffae1 20 if (nmea > 0)
mazgch 9:e7a5959ffae1 21 return NMEA | pipe->get(buf,nmea);
mazgch 2:b6012cd91657 22 // UBX protocol
mazgch 2:b6012cd91657 23 int ubx = _parseUbx(pipe,len);
mazgch 9:e7a5959ffae1 24 if ((ubx != NOT_FOUND) && (unkn > 0))
mazgch 9:e7a5959ffae1 25 return unkn;
mazgch 9:e7a5959ffae1 26 if (ubx == WAIT && fr)
mazgch 9:e7a5959ffae1 27 return WAIT;
mazgch 9:e7a5959ffae1 28 if (ubx > 0)
mazgch 9:e7a5959ffae1 29 return UBX | pipe->get(buf,ubx);
mazgch 2:b6012cd91657 30 // UNKNOWN
mazgch 2:b6012cd91657 31 *buf++ = pipe->getc();
mazgch 2:b6012cd91657 32 unkn ++;
mazgch 2:b6012cd91657 33 len--;
mazgch 2:b6012cd91657 34 }
mazgch 9:e7a5959ffae1 35 if (unkn != NOT_FOUND)
mazgch 9:e7a5959ffae1 36 return unkn;
mazgch 2:b6012cd91657 37 return WAIT;
mazgch 2:b6012cd91657 38 }
mazgch 2:b6012cd91657 39
mazgch 2:b6012cd91657 40 int GPSParser::_parseNmea(Pipe<char>* pipe, int len)
mazgch 2:b6012cd91657 41 {
mazgch 7:9aa830f5811e 42 int o = 0;
mazgch 7:9aa830f5811e 43 int c = 0;
mazgch 7:9aa830f5811e 44 char ch;
mazgch 2:b6012cd91657 45 pipe->start();
mazgch 7:9aa830f5811e 46 if (++o > len) return WAIT;
mazgch 2:b6012cd91657 47 if ('$' != pipe->next()) return NOT_FOUND;
mazgch 4:c959dd4c5fe8 48 // this needs to be extended by crc checking
mazgch 2:b6012cd91657 49 for (;;)
mazgch 2:b6012cd91657 50 {
mazgch 7:9aa830f5811e 51 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 52 ch = pipe->next();
mazgch 7:9aa830f5811e 53 if ('*' == ch) break; // crc delimiter
mazgch 7:9aa830f5811e 54 if (!isprint(ch)) return NOT_FOUND;
mazgch 7:9aa830f5811e 55 c ^= ch;
mazgch 2:b6012cd91657 56 }
mazgch 7:9aa830f5811e 57 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 58 ch = toHex[(c >> 4) & 0xF]; // high nibble
mazgch 7:9aa830f5811e 59 if (ch != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 60 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 61 ch = toHex[(c >> 0) & 0xF]; // low nibble
mazgch 7:9aa830f5811e 62 if (ch != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 63 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 64 if ('\r' != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 65 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 66 if ('\n' != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 67 return o;
mazgch 2:b6012cd91657 68 }
mazgch 2:b6012cd91657 69
mazgch 2:b6012cd91657 70 int GPSParser::_parseUbx(Pipe<char>* pipe, int l)
mazgch 2:b6012cd91657 71 {
mazgch 2:b6012cd91657 72 int o = 0;
mazgch 2:b6012cd91657 73 pipe->start();
mazgch 2:b6012cd91657 74 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 75 if ('\xB5' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 76 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 77 if ('\x62' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 78 o += 4;
mazgch 2:b6012cd91657 79 if (o > l) return WAIT;
mazgch 2:b6012cd91657 80 int i,j,ca,cb;
mazgch 2:b6012cd91657 81 i = pipe->next(); ca = i; cb = ca; // cls
mazgch 2:b6012cd91657 82 i = pipe->next(); ca += i; cb += ca; // id
mazgch 2:b6012cd91657 83 i = pipe->next(); ca += i; cb += ca; // len_lsb
mazgch 2:b6012cd91657 84 j = pipe->next(); ca += j; cb += ca; // len_msb
mazgch 2:b6012cd91657 85 j = i + (j << 8);
mazgch 2:b6012cd91657 86 while (j--)
mazgch 2:b6012cd91657 87 {
mazgch 2:b6012cd91657 88 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 89 i = pipe->next();
mazgch 2:b6012cd91657 90 ca += i;
mazgch 2:b6012cd91657 91 cb += ca;
mazgch 2:b6012cd91657 92 }
mazgch 2:b6012cd91657 93 ca &= 0xFF; cb &= 0xFF;
mazgch 2:b6012cd91657 94 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 95 if (ca != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 96 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 97 if (cb != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 98 return o;
mazgch 2:b6012cd91657 99 }
mazgch 2:b6012cd91657 100
mazgch 4:c959dd4c5fe8 101 int GPSParser::send(const char* buf, int len)
mazgch 4:c959dd4c5fe8 102 {
mazgch 4:c959dd4c5fe8 103 return _send(buf, len);
mazgch 4:c959dd4c5fe8 104 }
mazgch 4:c959dd4c5fe8 105
mazgch 3:c7cd4887560d 106 int GPSParser::sendNmea(const char* buf, int len)
mazgch 2:b6012cd91657 107 {
mazgch 3:c7cd4887560d 108 char head[1] = { '$' };
mazgch 3:c7cd4887560d 109 char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
mazgch 3:c7cd4887560d 110 int i;
mazgch 3:c7cd4887560d 111 int crc = 0;
mazgch 3:c7cd4887560d 112 for (i = 0; i < len; i ++)
mazgch 3:c7cd4887560d 113 crc ^= *buf++;
mazgch 4:c959dd4c5fe8 114 i = _send(head, sizeof(head));
mazgch 4:c959dd4c5fe8 115 i += _send(buf, len);
mazgch 3:c7cd4887560d 116 tail[1] = toHex[(crc > 4) & 0xF0];
mazgch 3:c7cd4887560d 117 tail[2] = toHex[(crc > 0) & 0x0F];
mazgch 4:c959dd4c5fe8 118 i += _send(tail, sizeof(tail));
mazgch 3:c7cd4887560d 119 return i;
mazgch 2:b6012cd91657 120 }
mazgch 2:b6012cd91657 121
mazgch 11:b084552b03fe 122 int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/)
mazgch 2:b6012cd91657 123 {
mazgch 3:c7cd4887560d 124 char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 };
mazgch 3:c7cd4887560d 125 char crc[2];
mazgch 3:c7cd4887560d 126 int i;
mazgch 3:c7cd4887560d 127 int ca = 0;
mazgch 3:c7cd4887560d 128 int cb = 0;
mazgch 3:c7cd4887560d 129 for (i = 2; i < 6; i ++)
mazgch 2:b6012cd91657 130 {
mazgch 3:c7cd4887560d 131 ca += head[i];
mazgch 3:c7cd4887560d 132 cb += ca;
mazgch 2:b6012cd91657 133 }
mazgch 3:c7cd4887560d 134 for (i = 0; i < len; i ++)
mazgch 3:c7cd4887560d 135 {
mazgch 3:c7cd4887560d 136 ca += ((char*)buf)[i];
mazgch 11:b084552b03fe 137 cb += ca;
mazgch 3:c7cd4887560d 138 }
mazgch 4:c959dd4c5fe8 139 i = _send(head, sizeof(head));
mazgch 4:c959dd4c5fe8 140 i += _send(buf, len);
mazgch 3:c7cd4887560d 141 crc[0] = ca & 0xFF;
mazgch 3:c7cd4887560d 142 crc[1] = cb & 0xFF;
mazgch 4:c959dd4c5fe8 143 i += _send(crc, sizeof(crc));
mazgch 3:c7cd4887560d 144 return i;
mazgch 2:b6012cd91657 145 }
mazgch 2:b6012cd91657 146
mazgch 2:b6012cd91657 147 const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end)
mazgch 2:b6012cd91657 148 {
mazgch 2:b6012cd91657 149 // find the start
mazgch 2:b6012cd91657 150 for (; (start < end) && (ix > 0); start ++)
mazgch 2:b6012cd91657 151 {
mazgch 2:b6012cd91657 152 if (*start == ',')
mazgch 2:b6012cd91657 153 ix --;
mazgch 2:b6012cd91657 154 }
mazgch 2:b6012cd91657 155 // found and check bounds
mazgch 2:b6012cd91657 156 if ((ix == 0) && (start < end) &&
mazgch 2:b6012cd91657 157 (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
mazgch 2:b6012cd91657 158 return start;
mazgch 2:b6012cd91657 159 else
mazgch 2:b6012cd91657 160 return NULL;
mazgch 2:b6012cd91657 161 }
mazgch 2:b6012cd91657 162
mazgch 2:b6012cd91657 163 bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val)
mazgch 2:b6012cd91657 164 {
mazgch 2:b6012cd91657 165 char* end = &buf[len];
mazgch 2:b6012cd91657 166 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 167 // find the start
mazgch 2:b6012cd91657 168 if (!pos)
mazgch 2:b6012cd91657 169 return false;
mazgch 2:b6012cd91657 170 val = strtod(pos, &end);
mazgch 2:b6012cd91657 171 // restore the last character
mazgch 2:b6012cd91657 172 return (end > pos);
mazgch 2:b6012cd91657 173 }
mazgch 2:b6012cd91657 174
mazgch 2:b6012cd91657 175 bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
mazgch 2:b6012cd91657 176 {
mazgch 2:b6012cd91657 177 char* end = &buf[len];
mazgch 2:b6012cd91657 178 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 179 // find the start
mazgch 2:b6012cd91657 180 if (!pos)
mazgch 2:b6012cd91657 181 return false;
mazgch 2:b6012cd91657 182 val = (int)strtol(pos, &end, base);
mazgch 2:b6012cd91657 183 return (end > pos);
mazgch 2:b6012cd91657 184 }
mazgch 2:b6012cd91657 185
mazgch 2:b6012cd91657 186 bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val)
mazgch 2:b6012cd91657 187 {
mazgch 2:b6012cd91657 188 const char* end = &buf[len];
mazgch 2:b6012cd91657 189 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 190 // find the start
mazgch 2:b6012cd91657 191 if (!pos)
mazgch 2:b6012cd91657 192 return false;
mazgch 2:b6012cd91657 193 // skip leading spaces
mazgch 2:b6012cd91657 194 while ((pos < end) && isspace(*pos))
mazgch 2:b6012cd91657 195 pos++;
mazgch 2:b6012cd91657 196 // check bound
mazgch 2:b6012cd91657 197 if ((pos < end) &&
mazgch 2:b6012cd91657 198 (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
mazgch 2:b6012cd91657 199 {
mazgch 2:b6012cd91657 200 val = *pos;
mazgch 2:b6012cd91657 201 return true;
mazgch 2:b6012cd91657 202 }
mazgch 2:b6012cd91657 203 return false;
mazgch 2:b6012cd91657 204 }
mazgch 2:b6012cd91657 205
mazgch 2:b6012cd91657 206 const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
mazgch 2:b6012cd91657 207
mazgch 2:b6012cd91657 208 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 209 // Serial Implementation
mazgch 2:b6012cd91657 210 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 211
mazgch 9:e7a5959ffae1 212 GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/,
mazgch 9:e7a5959ffae1 213 int rxSize /*= 256*/, int txSize /*= 128*/) :
mazgch 15:5eda64e5b9d1 214 SerialPipe(tx, rx, rxSize, txSize)
mazgch 2:b6012cd91657 215 {
mazgch 2:b6012cd91657 216 baud(baudrate);
mazgch 2:b6012cd91657 217 }
mazgch 2:b6012cd91657 218
mazgch 2:b6012cd91657 219 int GPSSerial::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 220 {
mazgch 9:e7a5959ffae1 221 return _getMessage(&_pipeRx, buf, len);
mazgch 2:b6012cd91657 222 }
mazgch 2:b6012cd91657 223
mazgch 3:c7cd4887560d 224 char GPSSerial::next(void)
mazgch 3:c7cd4887560d 225 {
mazgch 9:e7a5959ffae1 226 return _pipeRx.next();
mazgch 3:c7cd4887560d 227 }
mazgch 3:c7cd4887560d 228
mazgch 4:c959dd4c5fe8 229 int GPSSerial::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 230 {
mazgch 9:e7a5959ffae1 231 return put((const char*)buf, len, true/*=blocking*/);
mazgch 3:c7cd4887560d 232 }
mazgch 3:c7cd4887560d 233
mazgch 2:b6012cd91657 234 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 235 // I2C Implementation
mazgch 2:b6012cd91657 236 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 237
mazgch 9:e7a5959ffae1 238 GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/,
mazgch 9:e7a5959ffae1 239 int rxSize /*= 256*/) :
mazgch 2:b6012cd91657 240 I2C(sda,scl),
mazgch 9:e7a5959ffae1 241 _pipe(rxSize)
mazgch 2:b6012cd91657 242 {
mazgch 2:b6012cd91657 243 found = false;
mazgch 2:b6012cd91657 244 }
mazgch 2:b6012cd91657 245
mazgch 2:b6012cd91657 246 bool GPSI2C::detect(void)
mazgch 2:b6012cd91657 247 {
mazgch 2:b6012cd91657 248 if (!found)
mazgch 2:b6012cd91657 249 {
mazgch 2:b6012cd91657 250 int w = I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM));
mazgch 2:b6012cd91657 251 if (w == 0)
mazgch 2:b6012cd91657 252 found = true;
mazgch 2:b6012cd91657 253 }
mazgch 2:b6012cd91657 254 return found;
mazgch 2:b6012cd91657 255 }
mazgch 2:b6012cd91657 256
mazgch 2:b6012cd91657 257 int GPSI2C::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 258 {
mazgch 6:775aef3f1d1f 259 // fill the pipe
mazgch 6:775aef3f1d1f 260 int sz = _pipe.free();
mazgch 6:775aef3f1d1f 261 if (sz)
mazgch 6:775aef3f1d1f 262 sz = _get(buf, sz);
mazgch 2:b6012cd91657 263 if (sz)
mazgch 2:b6012cd91657 264 _pipe.put(buf, sz);
mazgch 6:775aef3f1d1f 265 // now parse it
mazgch 2:b6012cd91657 266 return _getMessage(&_pipe, buf, len);
mazgch 2:b6012cd91657 267 }
mazgch 2:b6012cd91657 268
mazgch 4:c959dd4c5fe8 269 int GPSI2C::send(const char* buf, int len)
mazgch 4:c959dd4c5fe8 270 {
mazgch 4:c959dd4c5fe8 271 int sent = 0;
mazgch 4:c959dd4c5fe8 272 if (len)
mazgch 4:c959dd4c5fe8 273 {
mazgch 4:c959dd4c5fe8 274 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 275 sent = send(buf, len);
mazgch 10:3f1c13a8763d 276 found = (len == sent);
mazgch 4:c959dd4c5fe8 277 stop();
mazgch 4:c959dd4c5fe8 278 }
mazgch 4:c959dd4c5fe8 279 return sent;
mazgch 4:c959dd4c5fe8 280 }
mazgch 4:c959dd4c5fe8 281
mazgch 3:c7cd4887560d 282 int GPSI2C::sendNmea(const char* buf, int len)
mazgch 3:c7cd4887560d 283 {
mazgch 3:c7cd4887560d 284 int sent = 0;
mazgch 11:b084552b03fe 285 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 286 sent = GPSParser::sendNmea(buf, len);
mazgch 11:b084552b03fe 287 found = (len == sent);
mazgch 11:b084552b03fe 288 stop();
mazgch 3:c7cd4887560d 289 return sent;
mazgch 3:c7cd4887560d 290 }
mazgch 3:c7cd4887560d 291
mazgch 3:c7cd4887560d 292 int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
mazgch 3:c7cd4887560d 293 {
mazgch 3:c7cd4887560d 294 int sent = 0;
mazgch 11:b084552b03fe 295 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 296 sent = GPSParser::sendUbx(cls, id, buf, len);
mazgch 11:b084552b03fe 297 found = (len == sent);
mazgch 15:5eda64e5b9d1 298 I2C::stop();
mazgch 3:c7cd4887560d 299 return sent;
mazgch 3:c7cd4887560d 300 }
mazgch 3:c7cd4887560d 301
mazgch 2:b6012cd91657 302 int GPSI2C::_get(char* buf, int len)
mazgch 2:b6012cd91657 303 {
mazgch 3:c7cd4887560d 304 int read = 0;
mazgch 2:b6012cd91657 305 unsigned char sz[2];
mazgch 2:b6012cd91657 306 if (!I2C::write(GPSADR,&REGLEN,sizeof(REGLEN),true) &&
mazgch 2:b6012cd91657 307 !I2C::read(GPSADR,(char*)sz,sizeof(sz),true))
mazgch 2:b6012cd91657 308 {
mazgch 2:b6012cd91657 309 int size = 256 * (int)sz[0] + sz[1];
mazgch 2:b6012cd91657 310 if (size > len)
mazgch 2:b6012cd91657 311 size = len;
mazgch 2:b6012cd91657 312 if (size > 0)
mazgch 3:c7cd4887560d 313 read = !I2C::read(GPSADR,buf,size, true) ? size : 0;
mazgch 10:3f1c13a8763d 314 found = (read == size);
mazgch 2:b6012cd91657 315 }
mazgch 2:b6012cd91657 316 else
mazgch 2:b6012cd91657 317 found = false;
mazgch 2:b6012cd91657 318 I2C::stop();
mazgch 3:c7cd4887560d 319 return read;
mazgch 2:b6012cd91657 320 }
mazgch 2:b6012cd91657 321
mazgch 3:c7cd4887560d 322 char GPSI2C::next(void)
mazgch 3:c7cd4887560d 323 {
mazgch 3:c7cd4887560d 324 return _pipe.next();
mazgch 3:c7cd4887560d 325 }
mazgch 3:c7cd4887560d 326
mazgch 4:c959dd4c5fe8 327 int GPSI2C::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 328 {
mazgch 3:c7cd4887560d 329 return !I2C::write(GPSADR,(const char*)buf,len,true) ? len : 0;
mazgch 2:b6012cd91657 330 }
mazgch 2:b6012cd91657 331
mazgch 2:b6012cd91657 332 const char GPSI2C::REGLEN = 0xFD;
mazgch 2:b6012cd91657 333 const char GPSI2C::REGSTREAM = 0xFF;