irsan julfikar / C027_Support_New_

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of C027_Support_Plus_Dialing by irsan julfikar

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;