Georgi Huklev / C027_Support

Add a bunch of APNs

Fork of C027_Support by Xinlei Cao

GPS.cpp@6:775aef3f1d1f, 2013-11-09 (annotated)

Committer:
mazgch
Date:
Sat Nov 09 12:20:25 2013 +0000
Revision:
6:775aef3f1d1f
Parent:
4:c959dd4c5fe8
Child:
7:9aa830f5811e
only num read bytes that we have available

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