Simon Pfeifhofer / C027_Support

* add C027_Support fork

Fork of C027_Support by u-blox

GPS.cpp@3:c7cd4887560d, 2013-10-25 (annotated)

Committer:
mazgch
Date:
Fri Oct 25 09:45:55 2013 +0000
Revision:
3:c7cd4887560d
Parent:
2:b6012cd91657
Child:
4:c959dd4c5fe8
added sending of msgs

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