Atar Babgei / GPSlib

Fork of Ublox Lib

Fork of C027_Support by u-blox

GPS.cpp@139:5df09023c4e9, 2016-12-06 (annotated)

Committer:
atarbey
Date:
Tue Dec 06 10:43:13 2016 +0000
Revision:
139:5df09023c4e9
Parent:
95:8282dbbe1492 
Forked from Ublox Library

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 74:208e3e32d263 4
mazgch 74:208e3e32d263 5 void GPSParser::powerOff(void)
mazgch 74:208e3e32d263 6 {
mazgch 74:208e3e32d263 7 // set the gps into backup mode using the command RMX-LPREQ
mazgch 74:208e3e32d263 8 struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/};
mazgch 74:208e3e32d263 9 sendUbx(0x02, 0x41, &msg, sizeof(msg));
mazgch 74:208e3e32d263 10 }
mazgch 2:b6012cd91657 11
mazgch 2:b6012cd91657 12 int GPSParser::_getMessage(Pipe<char>* pipe, char* buf, int len)
mazgch 2:b6012cd91657 13 {
mazgch 2:b6012cd91657 14 int unkn = 0;
mazgch 2:b6012cd91657 15 int sz = pipe->size();
mazgch 9:e7a5959ffae1 16 int fr = pipe->free();
mazgch 2:b6012cd91657 17 if (len > sz)
mazgch 2:b6012cd91657 18 len = sz;
mazgch 2:b6012cd91657 19 while (len > 0)
mazgch 2:b6012cd91657 20 {
mazgch 2:b6012cd91657 21 // NMEA protocol
mazgch 21:c4d64830bf02 22 pipe->set(unkn);
mazgch 2:b6012cd91657 23 int nmea = _parseNmea(pipe,len);
mazgch 9:e7a5959ffae1 24 if ((nmea != NOT_FOUND) && (unkn > 0))
mazgch 31:a0bed6c1e05d 25 return UNKNOWN | pipe->get(buf,unkn);
mazgch 9:e7a5959ffae1 26 if (nmea == WAIT && fr)
mazgch 9:e7a5959ffae1 27 return WAIT;
mazgch 9:e7a5959ffae1 28 if (nmea > 0)
mazgch 9:e7a5959ffae1 29 return NMEA | pipe->get(buf,nmea);
mazgch 2:b6012cd91657 30 // UBX protocol
mazgch 21:c4d64830bf02 31
mazgch 21:c4d64830bf02 32 pipe->set(unkn);
mazgch 2:b6012cd91657 33 int ubx = _parseUbx(pipe,len);
mazgch 9:e7a5959ffae1 34 if ((ubx != NOT_FOUND) && (unkn > 0))
mazgch 31:a0bed6c1e05d 35 return UNKNOWN | pipe->get(buf,unkn);
mazgch 9:e7a5959ffae1 36 if (ubx == WAIT && fr)
mazgch 9:e7a5959ffae1 37 return WAIT;
mazgch 9:e7a5959ffae1 38 if (ubx > 0)
mazgch 9:e7a5959ffae1 39 return UBX | pipe->get(buf,ubx);
mazgch 21:c4d64830bf02 40
mazgch 2:b6012cd91657 41 // UNKNOWN
mazgch 2:b6012cd91657 42 unkn ++;
mazgch 2:b6012cd91657 43 len--;
mazgch 2:b6012cd91657 44 }
mazgch 21:c4d64830bf02 45 if (unkn > 0)
mazgch 31:a0bed6c1e05d 46 return UNKNOWN | pipe->get(buf,unkn);
mazgch 2:b6012cd91657 47 return WAIT;
mazgch 2:b6012cd91657 48 }
mazgch 2:b6012cd91657 49
mazgch 2:b6012cd91657 50 int GPSParser::_parseNmea(Pipe<char>* pipe, int len)
mazgch 2:b6012cd91657 51 {
mazgch 7:9aa830f5811e 52 int o = 0;
mazgch 7:9aa830f5811e 53 int c = 0;
mazgch 7:9aa830f5811e 54 char ch;
mazgch 7:9aa830f5811e 55 if (++o > len) return WAIT;
mazgch 2:b6012cd91657 56 if ('$' != pipe->next()) return NOT_FOUND;
mazgch 4:c959dd4c5fe8 57 // this needs to be extended by crc checking
mazgch 2:b6012cd91657 58 for (;;)
mazgch 2:b6012cd91657 59 {
mazgch 7:9aa830f5811e 60 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 61 ch = pipe->next();
mazgch 7:9aa830f5811e 62 if ('*' == ch) break; // crc delimiter
mazgch 7:9aa830f5811e 63 if (!isprint(ch)) return NOT_FOUND;
mazgch 7:9aa830f5811e 64 c ^= ch;
mazgch 2:b6012cd91657 65 }
mazgch 7:9aa830f5811e 66 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 67 ch = toHex[(c >> 4) & 0xF]; // high nibble
mazgch 7:9aa830f5811e 68 if (ch != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 69 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 70 ch = toHex[(c >> 0) & 0xF]; // low nibble
mazgch 7:9aa830f5811e 71 if (ch != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 72 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 73 if ('\r' != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 74 if (++o > len) return WAIT;
mazgch 7:9aa830f5811e 75 if ('\n' != pipe->next()) return NOT_FOUND;
mazgch 7:9aa830f5811e 76 return o;
mazgch 2:b6012cd91657 77 }
mazgch 2:b6012cd91657 78
mazgch 2:b6012cd91657 79 int GPSParser::_parseUbx(Pipe<char>* pipe, int l)
mazgch 2:b6012cd91657 80 {
mazgch 2:b6012cd91657 81 int o = 0;
mazgch 2:b6012cd91657 82 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 83 if ('\xB5' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 84 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 85 if ('\x62' != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 86 o += 4;
mazgch 2:b6012cd91657 87 if (o > l) return WAIT;
mazgch 2:b6012cd91657 88 int i,j,ca,cb;
mazgch 2:b6012cd91657 89 i = pipe->next(); ca = i; cb = ca; // cls
mazgch 2:b6012cd91657 90 i = pipe->next(); ca += i; cb += ca; // id
mazgch 2:b6012cd91657 91 i = pipe->next(); ca += i; cb += ca; // len_lsb
mazgch 2:b6012cd91657 92 j = pipe->next(); ca += j; cb += ca; // len_msb
mazgch 2:b6012cd91657 93 j = i + (j << 8);
mazgch 2:b6012cd91657 94 while (j--)
mazgch 2:b6012cd91657 95 {
mazgch 2:b6012cd91657 96 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 97 i = pipe->next();
mazgch 2:b6012cd91657 98 ca += i;
mazgch 2:b6012cd91657 99 cb += ca;
mazgch 2:b6012cd91657 100 }
mazgch 2:b6012cd91657 101 ca &= 0xFF; cb &= 0xFF;
mazgch 2:b6012cd91657 102 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 103 if (ca != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 104 if (++o > l) return WAIT;
mazgch 2:b6012cd91657 105 if (cb != pipe->next()) return NOT_FOUND;
mazgch 2:b6012cd91657 106 return o;
mazgch 2:b6012cd91657 107 }
mazgch 2:b6012cd91657 108
mazgch 4:c959dd4c5fe8 109 int GPSParser::send(const char* buf, int len)
mazgch 4:c959dd4c5fe8 110 {
mazgch 4:c959dd4c5fe8 111 return _send(buf, len);
mazgch 4:c959dd4c5fe8 112 }
mazgch 4:c959dd4c5fe8 113
mazgch 3:c7cd4887560d 114 int GPSParser::sendNmea(const char* buf, int len)
mazgch 2:b6012cd91657 115 {
mazgch 3:c7cd4887560d 116 char head[1] = { '$' };
mazgch 3:c7cd4887560d 117 char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
mazgch 3:c7cd4887560d 118 int i;
mazgch 3:c7cd4887560d 119 int crc = 0;
mazgch 3:c7cd4887560d 120 for (i = 0; i < len; i ++)
mazgch 3:c7cd4887560d 121 crc ^= *buf++;
mazgch 4:c959dd4c5fe8 122 i = _send(head, sizeof(head));
mazgch 4:c959dd4c5fe8 123 i += _send(buf, len);
mazgch 3:c7cd4887560d 124 tail[1] = toHex[(crc > 4) & 0xF0];
mazgch 3:c7cd4887560d 125 tail[2] = toHex[(crc > 0) & 0x0F];
mazgch 4:c959dd4c5fe8 126 i += _send(tail, sizeof(tail));
mazgch 3:c7cd4887560d 127 return i;
mazgch 2:b6012cd91657 128 }
mazgch 2:b6012cd91657 129
mazgch 11:b084552b03fe 130 int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/)
mazgch 2:b6012cd91657 131 {
mazgch 3:c7cd4887560d 132 char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 };
mazgch 3:c7cd4887560d 133 char crc[2];
mazgch 3:c7cd4887560d 134 int i;
mazgch 3:c7cd4887560d 135 int ca = 0;
mazgch 3:c7cd4887560d 136 int cb = 0;
mazgch 3:c7cd4887560d 137 for (i = 2; i < 6; i ++)
mazgch 2:b6012cd91657 138 {
mazgch 3:c7cd4887560d 139 ca += head[i];
mazgch 3:c7cd4887560d 140 cb += ca;
mazgch 2:b6012cd91657 141 }
mazgch 3:c7cd4887560d 142 for (i = 0; i < len; i ++)
mazgch 3:c7cd4887560d 143 {
mazgch 3:c7cd4887560d 144 ca += ((char*)buf)[i];
mazgch 11:b084552b03fe 145 cb += ca;
mazgch 3:c7cd4887560d 146 }
mazgch 4:c959dd4c5fe8 147 i = _send(head, sizeof(head));
mazgch 4:c959dd4c5fe8 148 i += _send(buf, len);
mazgch 3:c7cd4887560d 149 crc[0] = ca & 0xFF;
mazgch 3:c7cd4887560d 150 crc[1] = cb & 0xFF;
mazgch 4:c959dd4c5fe8 151 i += _send(crc, sizeof(crc));
mazgch 3:c7cd4887560d 152 return i;
mazgch 2:b6012cd91657 153 }
mazgch 2:b6012cd91657 154
mazgch 2:b6012cd91657 155 const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end)
mazgch 2:b6012cd91657 156 {
mazgch 2:b6012cd91657 157 // find the start
mazgch 2:b6012cd91657 158 for (; (start < end) && (ix > 0); start ++)
mazgch 2:b6012cd91657 159 {
mazgch 2:b6012cd91657 160 if (*start == ',')
mazgch 2:b6012cd91657 161 ix --;
mazgch 2:b6012cd91657 162 }
mazgch 2:b6012cd91657 163 // found and check bounds
mazgch 2:b6012cd91657 164 if ((ix == 0) && (start < end) &&
mazgch 2:b6012cd91657 165 (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
mazgch 2:b6012cd91657 166 return start;
mazgch 2:b6012cd91657 167 else
mazgch 2:b6012cd91657 168 return NULL;
mazgch 2:b6012cd91657 169 }
mazgch 2:b6012cd91657 170
mazgch 2:b6012cd91657 171 bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val)
mazgch 2:b6012cd91657 172 {
mazgch 2:b6012cd91657 173 char* end = &buf[len];
mazgch 2:b6012cd91657 174 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 175 // find the start
mazgch 2:b6012cd91657 176 if (!pos)
mazgch 2:b6012cd91657 177 return false;
mazgch 2:b6012cd91657 178 val = strtod(pos, &end);
mazgch 2:b6012cd91657 179 // restore the last character
mazgch 2:b6012cd91657 180 return (end > pos);
mazgch 2:b6012cd91657 181 }
mazgch 2:b6012cd91657 182
mazgch 2:b6012cd91657 183 bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
mazgch 2:b6012cd91657 184 {
mazgch 2:b6012cd91657 185 char* end = &buf[len];
mazgch 2:b6012cd91657 186 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 187 // find the start
mazgch 2:b6012cd91657 188 if (!pos)
mazgch 2:b6012cd91657 189 return false;
mazgch 2:b6012cd91657 190 val = (int)strtol(pos, &end, base);
mazgch 2:b6012cd91657 191 return (end > pos);
mazgch 2:b6012cd91657 192 }
mazgch 2:b6012cd91657 193
mazgch 2:b6012cd91657 194 bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val)
mazgch 2:b6012cd91657 195 {
mazgch 2:b6012cd91657 196 const char* end = &buf[len];
mazgch 2:b6012cd91657 197 const char* pos = findNmeaItemPos(ix, buf, end);
mazgch 2:b6012cd91657 198 // find the start
mazgch 2:b6012cd91657 199 if (!pos)
mazgch 2:b6012cd91657 200 return false;
mazgch 2:b6012cd91657 201 // skip leading spaces
mazgch 2:b6012cd91657 202 while ((pos < end) && isspace(*pos))
mazgch 2:b6012cd91657 203 pos++;
mazgch 2:b6012cd91657 204 // check bound
mazgch 2:b6012cd91657 205 if ((pos < end) &&
mazgch 2:b6012cd91657 206 (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
mazgch 2:b6012cd91657 207 {
mazgch 2:b6012cd91657 208 val = *pos;
mazgch 2:b6012cd91657 209 return true;
mazgch 2:b6012cd91657 210 }
mazgch 2:b6012cd91657 211 return false;
mazgch 2:b6012cd91657 212 }
mazgch 2:b6012cd91657 213
mazgch 20:535ef78655df 214 bool GPSParser::getNmeaAngle(int ix, char* buf, int len, double& val)
mazgch 18:e5697801df29 215 {
mazgch 18:e5697801df29 216 char ch;
mazgch 18:e5697801df29 217 if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) &&
mazgch 18:e5697801df29 218 ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
mazgch 18:e5697801df29 219 {
mazgch 18:e5697801df29 220 val *= 0.01;
mazgch 20:535ef78655df 221 int i = (int)val;
mazgch 18:e5697801df29 222 val = (val - i) / 0.6 + i;
mazgch 18:e5697801df29 223 if (ch == 'S' || ch == 'W')
mazgch 18:e5697801df29 224 val = -val;
mazgch 18:e5697801df29 225 return true;
mazgch 18:e5697801df29 226 }
mazgch 18:e5697801df29 227 return false;
mazgch 18:e5697801df29 228 }
mazgch 18:e5697801df29 229
mazgch 2:b6012cd91657 230 const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
mazgch 2:b6012cd91657 231
mazgch 2:b6012cd91657 232 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 233 // Serial Implementation
mazgch 2:b6012cd91657 234 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 235
mazgch 9:e7a5959ffae1 236 GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/,
mazgch 9:e7a5959ffae1 237 int rxSize /*= 256*/, int txSize /*= 128*/) :
mazgch 15:5eda64e5b9d1 238 SerialPipe(tx, rx, rxSize, txSize)
mazgch 2:b6012cd91657 239 {
mazgch 2:b6012cd91657 240 baud(baudrate);
mazgch 74:208e3e32d263 241 #ifdef TARGET_UBLOX_C027
mazgch 74:208e3e32d263 242 _onboard = (tx == GPSTXD) || (rx == GPSRXD);
mazgch 74:208e3e32d263 243 if (_onboard)
mazgch 74:208e3e32d263 244 c027_gps_powerOn();
mazgch 74:208e3e32d263 245 #endif
mazgch 74:208e3e32d263 246 }
mazgch 74:208e3e32d263 247
mazgch 76:f7c3dd568dae 248 GPSSerial::~GPSSerial(void)
mazgch 76:f7c3dd568dae 249 {
mazgch 76:f7c3dd568dae 250 powerOff();
mazgch 76:f7c3dd568dae 251 #ifdef TARGET_UBLOX_C027
mazgch 76:f7c3dd568dae 252 if (_onboard)
mazgch 76:f7c3dd568dae 253 c027_gps_powerOff();
mazgch 76:f7c3dd568dae 254 #endif
mazgch 76:f7c3dd568dae 255 }
mazgch 76:f7c3dd568dae 256
mazgch 75:ce6e12067d0c 257 bool GPSSerial::init(PinName pn)
mazgch 74:208e3e32d263 258 {
mazgch 74:208e3e32d263 259 // send a byte to wakup the device again
mazgch 95:8282dbbe1492 260 putc(0xFF);
mazgch 74:208e3e32d263 261 // wait until we get some bytes
mazgch 74:208e3e32d263 262 int size = _pipeRx.size();
mazgch 95:8282dbbe1492 263 Timer timer;
mazgch 95:8282dbbe1492 264 timer.start();
mazgch 95:8282dbbe1492 265 while ((100 > timer.read_ms()) && (size == _pipeRx.size()))
mazgch 95:8282dbbe1492 266 /* nothing / just wait */;
mazgch 95:8282dbbe1492 267 return (size != _pipeRx.size());
mazgch 2:b6012cd91657 268 }
mazgch 2:b6012cd91657 269
mazgch 2:b6012cd91657 270 int GPSSerial::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 271 {
mazgch 9:e7a5959ffae1 272 return _getMessage(&_pipeRx, buf, len);
mazgch 2:b6012cd91657 273 }
mazgch 2:b6012cd91657 274
mazgch 4:c959dd4c5fe8 275 int GPSSerial::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 276 {
mazgch 9:e7a5959ffae1 277 return put((const char*)buf, len, true/*=blocking*/);
mazgch 3:c7cd4887560d 278 }
mazgch 3:c7cd4887560d 279
mazgch 2:b6012cd91657 280 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 281 // I2C Implementation
mazgch 2:b6012cd91657 282 // ----------------------------------------------------------------
mazgch 2:b6012cd91657 283
mazgch 9:e7a5959ffae1 284 GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/,
mazgch 19:2b5d097ca15d 285 unsigned char i2cAdr /*=GPSADR*/, int rxSize /*= 256*/) :
mazgch 2:b6012cd91657 286 I2C(sda,scl),
mazgch 19:2b5d097ca15d 287 _pipe(rxSize),
mazgch 19:2b5d097ca15d 288 _i2cAdr(i2cAdr)
mazgch 2:b6012cd91657 289 {
mazgch 45:ebc2fd8dcf21 290 frequency(100000);
mazgch 74:208e3e32d263 291 #ifdef TARGET_UBLOX_C027
mazgch 74:208e3e32d263 292 _onboard = (sda == GPSSDA) && (scl == GPSSCL);
mazgch 74:208e3e32d263 293 if (_onboard)
mazgch 74:208e3e32d263 294 c027_gps_powerOn();
mazgch 74:208e3e32d263 295 #endif
mazgch 2:b6012cd91657 296 }
mazgch 2:b6012cd91657 297
mazgch 76:f7c3dd568dae 298 GPSI2C::~GPSI2C(void)
mazgch 76:f7c3dd568dae 299 {
mazgch 76:f7c3dd568dae 300 powerOff();
mazgch 76:f7c3dd568dae 301 #ifdef TARGET_UBLOX_C027
mazgch 76:f7c3dd568dae 302 if (_onboard)
mazgch 76:f7c3dd568dae 303 c027_gps_powerOff();
mazgch 76:f7c3dd568dae 304 #endif
mazgch 76:f7c3dd568dae 305 }
mazgch 76:f7c3dd568dae 306
mazgch 75:ce6e12067d0c 307 bool GPSI2C::init(PinName pn)
mazgch 2:b6012cd91657 308 {
mazgch 75:ce6e12067d0c 309 if (pn != NC) {
mazgch 75:ce6e12067d0c 310 DigitalOut pin(pn, 0);
mazgch 95:8282dbbe1492 311 ::wait_us(1);
mazgch 75:ce6e12067d0c 312 pin = 1;
mazgch 95:8282dbbe1492 313 ::wait_ms(100);
mazgch 75:ce6e12067d0c 314 }
mazgch 74:208e3e32d263 315 return !I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM));
mazgch 2:b6012cd91657 316 }
mazgch 2:b6012cd91657 317
mazgch 2:b6012cd91657 318 int GPSI2C::getMessage(char* buf, int len)
mazgch 2:b6012cd91657 319 {
mazgch 6:775aef3f1d1f 320 // fill the pipe
mazgch 6:775aef3f1d1f 321 int sz = _pipe.free();
mazgch 6:775aef3f1d1f 322 if (sz)
mazgch 6:775aef3f1d1f 323 sz = _get(buf, sz);
mazgch 2:b6012cd91657 324 if (sz)
mazgch 2:b6012cd91657 325 _pipe.put(buf, sz);
mazgch 6:775aef3f1d1f 326 // now parse it
mazgch 2:b6012cd91657 327 return _getMessage(&_pipe, buf, len);
mazgch 2:b6012cd91657 328 }
mazgch 2:b6012cd91657 329
mazgch 4:c959dd4c5fe8 330 int GPSI2C::send(const char* buf, int len)
mazgch 4:c959dd4c5fe8 331 {
mazgch 4:c959dd4c5fe8 332 int sent = 0;
mazgch 4:c959dd4c5fe8 333 if (len)
mazgch 4:c959dd4c5fe8 334 {
mazgch 19:2b5d097ca15d 335 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 336 sent = send(buf, len);
mazgch 4:c959dd4c5fe8 337 stop();
mazgch 4:c959dd4c5fe8 338 }
mazgch 4:c959dd4c5fe8 339 return sent;
mazgch 4:c959dd4c5fe8 340 }
mazgch 4:c959dd4c5fe8 341
mazgch 3:c7cd4887560d 342 int GPSI2C::sendNmea(const char* buf, int len)
mazgch 3:c7cd4887560d 343 {
mazgch 3:c7cd4887560d 344 int sent = 0;
mazgch 19:2b5d097ca15d 345 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 346 sent = GPSParser::sendNmea(buf, len);
mazgch 11:b084552b03fe 347 stop();
mazgch 3:c7cd4887560d 348 return sent;
mazgch 3:c7cd4887560d 349 }
mazgch 3:c7cd4887560d 350
mazgch 3:c7cd4887560d 351 int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
mazgch 3:c7cd4887560d 352 {
mazgch 3:c7cd4887560d 353 int sent = 0;
mazgch 19:2b5d097ca15d 354 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
mazgch 11:b084552b03fe 355 sent = GPSParser::sendUbx(cls, id, buf, len);
mazgch 15:5eda64e5b9d1 356 I2C::stop();
mazgch 3:c7cd4887560d 357 return sent;
mazgch 3:c7cd4887560d 358 }
mazgch 3:c7cd4887560d 359
mazgch 2:b6012cd91657 360 int GPSI2C::_get(char* buf, int len)
mazgch 2:b6012cd91657 361 {
mazgch 3:c7cd4887560d 362 int read = 0;
mazgch 46:8ce9169e0747 363 unsigned char sz[2] = {0,0};
mazgch 19:2b5d097ca15d 364 if (!I2C::write(_i2cAdr,&REGLEN,sizeof(REGLEN),true) &&
mazgch 46:8ce9169e0747 365 !I2C::read(_i2cAdr,(char*)sz,sizeof(sz)))
mazgch 2:b6012cd91657 366 {
mazgch 2:b6012cd91657 367 int size = 256 * (int)sz[0] + sz[1];
mazgch 2:b6012cd91657 368 if (size > len)
mazgch 2:b6012cd91657 369 size = len;
mazgch 46:8ce9169e0747 370 if (size > 0)
mazgch 46:8ce9169e0747 371 {
mazgch 46:8ce9169e0747 372 if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true) &&
mazgch 46:8ce9169e0747 373 !I2C::read(_i2cAdr,buf,size)) {
mazgch 46:8ce9169e0747 374 read = size;
mazgch 46:8ce9169e0747 375 }
mazgch 46:8ce9169e0747 376 }
mazgch 2:b6012cd91657 377 }
mazgch 3:c7cd4887560d 378 return read;
mazgch 2:b6012cd91657 379 }
mazgch 2:b6012cd91657 380
mazgch 4:c959dd4c5fe8 381 int GPSI2C::_send(const void* buf, int len)
mazgch 3:c7cd4887560d 382 {
mazgch 19:2b5d097ca15d 383 return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0;
mazgch 2:b6012cd91657 384 }
mazgch 2:b6012cd91657 385
mazgch 2:b6012cd91657 386 const char GPSI2C::REGLEN = 0xFD;
mazgch 2:b6012cd91657 387 const char GPSI2C::REGSTREAM = 0xFF;