Simon Pfeifhofer / C027_Support

* add C027_Support fork

Fork of C027_Support by u-blox

GPS.cpp@4:c959dd4c5fe8, 2013-10-25 (annotated)

Committer:
mazgch
Date:
Fri Oct 25 17:50:17 2013 +0000
Revision:
4:c959dd4c5fe8
Parent:
3:c7cd4887560d
Child:
6:775aef3f1d1f
small fix

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 2:b6012cd91657 249 int sz = _get(buf, len);
mazgch 2:b6012cd91657 250 if (sz)
mazgch 2:b6012cd91657 251 _pipe.put(buf, sz);
mazgch 2:b6012cd91657 252 return _getMessage(&_pipe, buf, len);
mazgch 2:b6012cd91657 253 }
mazgch 2:b6012cd91657 254
mazgch 4:c959dd4c5fe8 255 int GPSI2C::send(const char* buf, int len)
mazgch 4:c959dd4c5fe8 256 {
mazgch 4:c959dd4c5fe8 257 int sent = 0;
mazgch 4:c959dd4c5fe8 258 if (len)
mazgch 4:c959dd4c5fe8 259 {
mazgch 4:c959dd4c5fe8 260 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 4:c959dd4c5fe8 261 sent = _send(buf, len);
mazgch 4:c959dd4c5fe8 262 found = len == sent;
mazgch 4:c959dd4c5fe8 263 stop();
mazgch 4:c959dd4c5fe8 264 }
mazgch 4:c959dd4c5fe8 265 return sent;
mazgch 4:c959dd4c5fe8 266 }
mazgch 4:c959dd4c5fe8 267
mazgch 3:c7cd4887560d 268 int GPSI2C::sendNmea(const char* buf, int len)
mazgch 3:c7cd4887560d 269 {
mazgch 3:c7cd4887560d 270 int sent = 0;
mazgch 3:c7cd4887560d 271 if (len)
mazgch 3:c7cd4887560d 272 {
mazgch 3:c7cd4887560d 273 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 3:c7cd4887560d 274 sent = GPSParser::sendNmea(buf, len);
mazgch 3:c7cd4887560d 275 found = len == sent;
mazgch 3:c7cd4887560d 276 stop();
mazgch 3:c7cd4887560d 277 }
mazgch 3:c7cd4887560d 278 return sent;
mazgch 3:c7cd4887560d 279 }
mazgch 3:c7cd4887560d 280
mazgch 3:c7cd4887560d 281 int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
mazgch 3:c7cd4887560d 282 {
mazgch 3:c7cd4887560d 283 int sent = 0;
mazgch 3:c7cd4887560d 284 if (len)
mazgch 3:c7cd4887560d 285 {
mazgch 3:c7cd4887560d 286 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 3:c7cd4887560d 287 sent = GPSParser::sendUbx(cls, id, buf, len);
mazgch 3:c7cd4887560d 288 found = (len == sent);
mazgch 3:c7cd4887560d 289 stop();
mazgch 3:c7cd4887560d 290 }
mazgch 3:c7cd4887560d 291 return sent;
mazgch 3:c7cd4887560d 292 }
mazgch 3:c7cd4887560d 293
mazgch 2:b6012cd91657 294 int GPSI2C::_get(char* buf, int len)
mazgch 2:b6012cd91657 295 {
mazgch 3:c7cd4887560d 296 int read = 0;
mazgch 2:b6012cd91657 297 unsigned char sz[2];
mazgch 2:b6012cd91657 298 if (!I2C::write(GPSADR,&REGLEN,sizeof(REGLEN),true) &&
mazgch 2:b6012cd91657 299 !I2C::read(GPSADR,(char*)sz,sizeof(sz),true))
mazgch 2:b6012cd91657 300 {
mazgch 2:b6012cd91657 301 int size = 256 * (int)sz[0] + sz[1];
mazgch 2:b6012cd91657 302 if (size > len)
mazgch 2:b6012cd91657 303 size = len;
mazgch 2:b6012cd91657 304 if (size > 0)
mazgch 3:c7cd4887560d 305 read = !I2C::read(GPSADR,buf,size, true) ? size : 0;
mazgch 3:c7cd4887560d 306 stop();
mazgch 3:c7cd4887560d 307 found = read = size;
mazgch 2:b6012cd91657 308 }
mazgch 2:b6012cd91657 309 else
mazgch 2:b6012cd91657 310 found = false;
mazgch 2:b6012cd91657 311 I2C::stop();
mazgch 3:c7cd4887560d 312 return read;
mazgch 2:b6012cd91657 313 }
mazgch 2:b6012cd91657 314
mazgch 3:c7cd4887560d 315 char GPSI2C::next(void)
mazgch 3:c7cd4887560d 316 {
mazgch 3:c7cd4887560d 317 return _pipe.next();
mazgch 3:c7cd4887560d 318 }
mazgch 3:c7cd4887560d 319
mazgch 4:c959dd4c5fe8 320 int GPSI2C::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 321 {
mazgch 3:c7cd4887560d 322 return !I2C::write(GPSADR,(const char*)buf,len,true) ? len : 0;
mazgch 2:b6012cd91657 323 }
mazgch 2:b6012cd91657 324
mazgch 2:b6012cd91657 325 const char GPSI2C::REGLEN = 0xFD;
mazgch 2:b6012cd91657 326 const char GPSI2C::REGSTREAM = 0xFF;