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@20:535ef78655df, 2014-03-17 (annotated)

Committer:
mazgch
Date:
Mon Mar 17 13:18:04 2014 +0000
Revision:
20:535ef78655df
Parent:
18:e5697801df29
Child:
24:0e287a85ac9e
fix angle api, only one MDM constructor

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 20:535ef78655df 206 bool GPSParser::getNmeaAngle(int ix, char* buf, int len, double& val)
mazgch 18:e5697801df29 207 {
mazgch 18:e5697801df29 208 char ch;
mazgch 18:e5697801df29 209 if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) &&
mazgch 18:e5697801df29 210 ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
mazgch 18:e5697801df29 211 {
mazgch 18:e5697801df29 212 val *= 0.01;
mazgch 20:535ef78655df 213 int i = (int)val;
mazgch 18:e5697801df29 214 val = (val - i) / 0.6 + i;
mazgch 18:e5697801df29 215 if (ch == 'S' || ch == 'W')
mazgch 18:e5697801df29 216 val = -val;
mazgch 18:e5697801df29 217 return true;
mazgch 18:e5697801df29 218 }
mazgch 18:e5697801df29 219 return false;
mazgch 18:e5697801df29 220 }
mazgch 18:e5697801df29 221
mazgch 2:b6012cd91657 222 const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
mazgch 2:b6012cd91657 223
mazgch 2:b6012cd91657 224 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 225 // Serial Implementation
mazgch 2:b6012cd91657 226 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 227
mazgch 9:e7a5959ffae1 228 GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/,
mazgch 9:e7a5959ffae1 229 int rxSize /*= 256*/, int txSize /*= 128*/) :
mazgch 15:5eda64e5b9d1 230 SerialPipe(tx, rx, rxSize, txSize)
mazgch 2:b6012cd91657 231 {
mazgch 2:b6012cd91657 232 baud(baudrate);
mazgch 2:b6012cd91657 233 }
mazgch 2:b6012cd91657 234
mazgch 2:b6012cd91657 235 int GPSSerial::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 236 {
mazgch 9:e7a5959ffae1 237 return _getMessage(&_pipeRx, buf, len);
mazgch 2:b6012cd91657 238 }
mazgch 2:b6012cd91657 239
mazgch 4:c959dd4c5fe8 240 int GPSSerial::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 241 {
mazgch 9:e7a5959ffae1 242 return put((const char*)buf, len, true/*=blocking*/);
mazgch 3:c7cd4887560d 243 }
mazgch 3:c7cd4887560d 244
mazgch 2:b6012cd91657 245 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 246 // I2C Implementation
mazgch 2:b6012cd91657 247 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 248
mazgch 9:e7a5959ffae1 249 GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/,
mazgch 9:e7a5959ffae1 250 int rxSize /*= 256*/) :
mazgch 2:b6012cd91657 251 I2C(sda,scl),
mazgch 9:e7a5959ffae1 252 _pipe(rxSize)
mazgch 2:b6012cd91657 253 {
mazgch 2:b6012cd91657 254 found = false;
mazgch 2:b6012cd91657 255 }
mazgch 2:b6012cd91657 256
mazgch 2:b6012cd91657 257 bool GPSI2C::detect(void)
mazgch 2:b6012cd91657 258 {
mazgch 2:b6012cd91657 259 if (!found)
mazgch 2:b6012cd91657 260 {
mazgch 2:b6012cd91657 261 int w = I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM));
mazgch 2:b6012cd91657 262 if (w == 0)
mazgch 2:b6012cd91657 263 found = true;
mazgch 2:b6012cd91657 264 }
mazgch 2:b6012cd91657 265 return found;
mazgch 2:b6012cd91657 266 }
mazgch 2:b6012cd91657 267
mazgch 2:b6012cd91657 268 int GPSI2C::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 269 {
mazgch 6:775aef3f1d1f 270 // fill the pipe
mazgch 6:775aef3f1d1f 271 int sz = _pipe.free();
mazgch 6:775aef3f1d1f 272 if (sz)
mazgch 6:775aef3f1d1f 273 sz = _get(buf, sz);
mazgch 2:b6012cd91657 274 if (sz)
mazgch 2:b6012cd91657 275 _pipe.put(buf, sz);
mazgch 6:775aef3f1d1f 276 // now parse it
mazgch 2:b6012cd91657 277 return _getMessage(&_pipe, buf, len);
mazgch 2:b6012cd91657 278 }
mazgch 2:b6012cd91657 279
mazgch 4:c959dd4c5fe8 280 int GPSI2C::send(const char* buf, int len)
mazgch 4:c959dd4c5fe8 281 {
mazgch 4:c959dd4c5fe8 282 int sent = 0;
mazgch 4:c959dd4c5fe8 283 if (len)
mazgch 4:c959dd4c5fe8 284 {
mazgch 4:c959dd4c5fe8 285 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 286 sent = send(buf, len);
mazgch 10:3f1c13a8763d 287 found = (len == sent);
mazgch 4:c959dd4c5fe8 288 stop();
mazgch 4:c959dd4c5fe8 289 }
mazgch 4:c959dd4c5fe8 290 return sent;
mazgch 4:c959dd4c5fe8 291 }
mazgch 4:c959dd4c5fe8 292
mazgch 3:c7cd4887560d 293 int GPSI2C::sendNmea(const char* buf, int len)
mazgch 3:c7cd4887560d 294 {
mazgch 3:c7cd4887560d 295 int sent = 0;
mazgch 11:b084552b03fe 296 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 297 sent = GPSParser::sendNmea(buf, len);
mazgch 11:b084552b03fe 298 found = (len == sent);
mazgch 11:b084552b03fe 299 stop();
mazgch 3:c7cd4887560d 300 return sent;
mazgch 3:c7cd4887560d 301 }
mazgch 3:c7cd4887560d 302
mazgch 3:c7cd4887560d 303 int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
mazgch 3:c7cd4887560d 304 {
mazgch 3:c7cd4887560d 305 int sent = 0;
mazgch 11:b084552b03fe 306 if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 307 sent = GPSParser::sendUbx(cls, id, buf, len);
mazgch 11:b084552b03fe 308 found = (len == sent);
mazgch 15:5eda64e5b9d1 309 I2C::stop();
mazgch 3:c7cd4887560d 310 return sent;
mazgch 3:c7cd4887560d 311 }
mazgch 3:c7cd4887560d 312
mazgch 2:b6012cd91657 313 int GPSI2C::_get(char* buf, int len)
mazgch 2:b6012cd91657 314 {
mazgch 3:c7cd4887560d 315 int read = 0;
mazgch 2:b6012cd91657 316 unsigned char sz[2];
mazgch 2:b6012cd91657 317 if (!I2C::write(GPSADR,&REGLEN,sizeof(REGLEN),true) &&
mazgch 2:b6012cd91657 318 !I2C::read(GPSADR,(char*)sz,sizeof(sz),true))
mazgch 2:b6012cd91657 319 {
mazgch 2:b6012cd91657 320 int size = 256 * (int)sz[0] + sz[1];
mazgch 2:b6012cd91657 321 if (size > len)
mazgch 2:b6012cd91657 322 size = len;
mazgch 2:b6012cd91657 323 if (size > 0)
mazgch 3:c7cd4887560d 324 read = !I2C::read(GPSADR,buf,size, true) ? size : 0;
mazgch 10:3f1c13a8763d 325 found = (read == size);
mazgch 2:b6012cd91657 326 }
mazgch 2:b6012cd91657 327 else
mazgch 2:b6012cd91657 328 found = false;
mazgch 2:b6012cd91657 329 I2C::stop();
mazgch 3:c7cd4887560d 330 return read;
mazgch 2:b6012cd91657 331 }
mazgch 2:b6012cd91657 332
mazgch 4:c959dd4c5fe8 333 int GPSI2C::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 334 {
mazgch 3:c7cd4887560d 335 return !I2C::write(GPSADR,(const char*)buf,len,true) ? len : 0;
mazgch 2:b6012cd91657 336 }
mazgch 2:b6012cd91657 337
mazgch 2:b6012cd91657 338 const char GPSI2C::REGLEN = 0xFD;
mazgch 2:b6012cd91657 339 const char GPSI2C::REGSTREAM = 0xFF;