Naoya Takamura / C027_Support_ForIM920

IM920地温観測システム用 cbUSB()関数定義だけ修正したもの

Fork of C027_Support by u-blox

GPS.cpp@7:9aa830f5811e, 2013-11-09 (annotated)

Committer:
mazgch
Date:
Sat Nov 09 13:31:01 2013 +0000
Revision:
7:9aa830f5811e
Parent:
6:775aef3f1d1f
Child:
9:e7a5959ffae1
fix parsing problems in pipe

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