t3
Fork of C027_Support by
GPS.cpp@46:8ce9169e0747, 2014-05-06 (annotated)
- Committer:
- mazgch
- Date:
- Tue May 06 06:54:12 2014 +0000
- Revision:
- 46:8ce9169e0747
- Parent:
- 45:ebc2fd8dcf21
- Child:
- 74:208e3e32d263
Some targets (FSC) do not like repeated starts after reads
Who changed what in which revision?
User | Revision | Line number | New 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 | 21:c4d64830bf02 | 15 | pipe->set(unkn); |
mazgch | 2:b6012cd91657 | 16 | int nmea = _parseNmea(pipe,len); |
mazgch | 9:e7a5959ffae1 | 17 | if ((nmea != NOT_FOUND) && (unkn > 0)) |
mazgch | 31:a0bed6c1e05d | 18 | return UNKNOWN | pipe->get(buf,unkn); |
mazgch | 9:e7a5959ffae1 | 19 | if (nmea == WAIT && fr) |
mazgch | 9:e7a5959ffae1 | 20 | return WAIT; |
mazgch | 9:e7a5959ffae1 | 21 | if (nmea > 0) |
mazgch | 9:e7a5959ffae1 | 22 | return NMEA | pipe->get(buf,nmea); |
mazgch | 2:b6012cd91657 | 23 | // UBX protocol |
mazgch | 21:c4d64830bf02 | 24 | |
mazgch | 21:c4d64830bf02 | 25 | pipe->set(unkn); |
mazgch | 2:b6012cd91657 | 26 | int ubx = _parseUbx(pipe,len); |
mazgch | 9:e7a5959ffae1 | 27 | if ((ubx != NOT_FOUND) && (unkn > 0)) |
mazgch | 31:a0bed6c1e05d | 28 | return UNKNOWN | pipe->get(buf,unkn); |
mazgch | 9:e7a5959ffae1 | 29 | if (ubx == WAIT && fr) |
mazgch | 9:e7a5959ffae1 | 30 | return WAIT; |
mazgch | 9:e7a5959ffae1 | 31 | if (ubx > 0) |
mazgch | 9:e7a5959ffae1 | 32 | return UBX | pipe->get(buf,ubx); |
mazgch | 21:c4d64830bf02 | 33 | |
mazgch | 2:b6012cd91657 | 34 | // UNKNOWN |
mazgch | 2:b6012cd91657 | 35 | unkn ++; |
mazgch | 2:b6012cd91657 | 36 | len--; |
mazgch | 2:b6012cd91657 | 37 | } |
mazgch | 21:c4d64830bf02 | 38 | if (unkn > 0) |
mazgch | 31:a0bed6c1e05d | 39 | return UNKNOWN | pipe->get(buf,unkn); |
mazgch | 2:b6012cd91657 | 40 | return WAIT; |
mazgch | 2:b6012cd91657 | 41 | } |
mazgch | 2:b6012cd91657 | 42 | |
mazgch | 2:b6012cd91657 | 43 | int GPSParser::_parseNmea(Pipe<char>* pipe, int len) |
mazgch | 2:b6012cd91657 | 44 | { |
mazgch | 7:9aa830f5811e | 45 | int o = 0; |
mazgch | 7:9aa830f5811e | 46 | int c = 0; |
mazgch | 7:9aa830f5811e | 47 | char ch; |
mazgch | 7:9aa830f5811e | 48 | if (++o > len) return WAIT; |
mazgch | 2:b6012cd91657 | 49 | if ('$' != pipe->next()) return NOT_FOUND; |
mazgch | 4:c959dd4c5fe8 | 50 | // this needs to be extended by crc checking |
mazgch | 2:b6012cd91657 | 51 | for (;;) |
mazgch | 2:b6012cd91657 | 52 | { |
mazgch | 7:9aa830f5811e | 53 | if (++o > len) return WAIT; |
mazgch | 7:9aa830f5811e | 54 | ch = pipe->next(); |
mazgch | 7:9aa830f5811e | 55 | if ('*' == ch) break; // crc delimiter |
mazgch | 7:9aa830f5811e | 56 | if (!isprint(ch)) return NOT_FOUND; |
mazgch | 7:9aa830f5811e | 57 | c ^= ch; |
mazgch | 2:b6012cd91657 | 58 | } |
mazgch | 7:9aa830f5811e | 59 | if (++o > len) return WAIT; |
mazgch | 7:9aa830f5811e | 60 | ch = toHex[(c >> 4) & 0xF]; // high nibble |
mazgch | 7:9aa830f5811e | 61 | if (ch != pipe->next()) return NOT_FOUND; |
mazgch | 7:9aa830f5811e | 62 | if (++o > len) return WAIT; |
mazgch | 7:9aa830f5811e | 63 | ch = toHex[(c >> 0) & 0xF]; // low nibble |
mazgch | 7:9aa830f5811e | 64 | if (ch != pipe->next()) return NOT_FOUND; |
mazgch | 7:9aa830f5811e | 65 | if (++o > len) return WAIT; |
mazgch | 7:9aa830f5811e | 66 | if ('\r' != pipe->next()) return NOT_FOUND; |
mazgch | 7:9aa830f5811e | 67 | if (++o > len) return WAIT; |
mazgch | 7:9aa830f5811e | 68 | if ('\n' != pipe->next()) return NOT_FOUND; |
mazgch | 7:9aa830f5811e | 69 | return o; |
mazgch | 2:b6012cd91657 | 70 | } |
mazgch | 2:b6012cd91657 | 71 | |
mazgch | 2:b6012cd91657 | 72 | int GPSParser::_parseUbx(Pipe<char>* pipe, int l) |
mazgch | 2:b6012cd91657 | 73 | { |
mazgch | 2:b6012cd91657 | 74 | int o = 0; |
mazgch | 2:b6012cd91657 | 75 | if (++o > l) return WAIT; |
mazgch | 2:b6012cd91657 | 76 | if ('\xB5' != pipe->next()) return NOT_FOUND; |
mazgch | 2:b6012cd91657 | 77 | if (++o > l) return WAIT; |
mazgch | 2:b6012cd91657 | 78 | if ('\x62' != pipe->next()) return NOT_FOUND; |
mazgch | 2:b6012cd91657 | 79 | o += 4; |
mazgch | 2:b6012cd91657 | 80 | if (o > l) return WAIT; |
mazgch | 2:b6012cd91657 | 81 | int i,j,ca,cb; |
mazgch | 2:b6012cd91657 | 82 | i = pipe->next(); ca = i; cb = ca; // cls |
mazgch | 2:b6012cd91657 | 83 | i = pipe->next(); ca += i; cb += ca; // id |
mazgch | 2:b6012cd91657 | 84 | i = pipe->next(); ca += i; cb += ca; // len_lsb |
mazgch | 2:b6012cd91657 | 85 | j = pipe->next(); ca += j; cb += ca; // len_msb |
mazgch | 2:b6012cd91657 | 86 | j = i + (j << 8); |
mazgch | 2:b6012cd91657 | 87 | while (j--) |
mazgch | 2:b6012cd91657 | 88 | { |
mazgch | 2:b6012cd91657 | 89 | if (++o > l) return WAIT; |
mazgch | 2:b6012cd91657 | 90 | i = pipe->next(); |
mazgch | 2:b6012cd91657 | 91 | ca += i; |
mazgch | 2:b6012cd91657 | 92 | cb += ca; |
mazgch | 2:b6012cd91657 | 93 | } |
mazgch | 2:b6012cd91657 | 94 | ca &= 0xFF; cb &= 0xFF; |
mazgch | 2:b6012cd91657 | 95 | if (++o > l) return WAIT; |
mazgch | 2:b6012cd91657 | 96 | if (ca != pipe->next()) return NOT_FOUND; |
mazgch | 2:b6012cd91657 | 97 | if (++o > l) return WAIT; |
mazgch | 2:b6012cd91657 | 98 | if (cb != pipe->next()) return NOT_FOUND; |
mazgch | 2:b6012cd91657 | 99 | return o; |
mazgch | 2:b6012cd91657 | 100 | } |
mazgch | 2:b6012cd91657 | 101 | |
mazgch | 4:c959dd4c5fe8 | 102 | int GPSParser::send(const char* buf, int len) |
mazgch | 4:c959dd4c5fe8 | 103 | { |
mazgch | 4:c959dd4c5fe8 | 104 | return _send(buf, len); |
mazgch | 4:c959dd4c5fe8 | 105 | } |
mazgch | 4:c959dd4c5fe8 | 106 | |
mazgch | 31:a0bed6c1e05d | 107 | void GPSParser::powerOff(void) |
mazgch | 31:a0bed6c1e05d | 108 | { |
mazgch | 31:a0bed6c1e05d | 109 | // set the gps into backup mode using the command RMX-LPREQ |
mazgch | 31:a0bed6c1e05d | 110 | struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/}; |
mazgch | 31:a0bed6c1e05d | 111 | sendUbx(0x02, 0x41, &msg, sizeof(msg)); |
mazgch | 31:a0bed6c1e05d | 112 | } |
mazgch | 31:a0bed6c1e05d | 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 | 2:b6012cd91657 | 241 | } |
mazgch | 2:b6012cd91657 | 242 | |
mazgch | 2:b6012cd91657 | 243 | int GPSSerial::getMessage(char* buf, int len) |
mazgch | 2:b6012cd91657 | 244 | { |
mazgch | 9:e7a5959ffae1 | 245 | return _getMessage(&_pipeRx, buf, len); |
mazgch | 2:b6012cd91657 | 246 | } |
mazgch | 2:b6012cd91657 | 247 | |
mazgch | 4:c959dd4c5fe8 | 248 | int GPSSerial::_send(const void* buf, int len) |
mazgch | 3:c7cd4887560d | 249 | { |
mazgch | 9:e7a5959ffae1 | 250 | return put((const char*)buf, len, true/*=blocking*/); |
mazgch | 3:c7cd4887560d | 251 | } |
mazgch | 3:c7cd4887560d | 252 | |
mazgch | 2:b6012cd91657 | 253 | // ---------------------------------------------------------------- |
mazgch | 2:b6012cd91657 | 254 | // I2C Implementation |
mazgch | 2:b6012cd91657 | 255 | // ---------------------------------------------------------------- |
mazgch | 2:b6012cd91657 | 256 | |
mazgch | 9:e7a5959ffae1 | 257 | GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/, |
mazgch | 19:2b5d097ca15d | 258 | unsigned char i2cAdr /*=GPSADR*/, int rxSize /*= 256*/) : |
mazgch | 2:b6012cd91657 | 259 | I2C(sda,scl), |
mazgch | 19:2b5d097ca15d | 260 | _pipe(rxSize), |
mazgch | 19:2b5d097ca15d | 261 | _i2cAdr(i2cAdr) |
mazgch | 2:b6012cd91657 | 262 | { |
mazgch | 45:ebc2fd8dcf21 | 263 | frequency(100000); |
mazgch | 2:b6012cd91657 | 264 | found = false; |
mazgch | 2:b6012cd91657 | 265 | } |
mazgch | 2:b6012cd91657 | 266 | |
mazgch | 2:b6012cd91657 | 267 | bool GPSI2C::detect(void) |
mazgch | 2:b6012cd91657 | 268 | { |
mazgch | 2:b6012cd91657 | 269 | if (!found) |
mazgch | 2:b6012cd91657 | 270 | { |
mazgch | 19:2b5d097ca15d | 271 | int w = I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM)); |
mazgch | 2:b6012cd91657 | 272 | if (w == 0) |
mazgch | 2:b6012cd91657 | 273 | found = true; |
mazgch | 2:b6012cd91657 | 274 | } |
mazgch | 2:b6012cd91657 | 275 | return found; |
mazgch | 2:b6012cd91657 | 276 | } |
mazgch | 2:b6012cd91657 | 277 | |
mazgch | 2:b6012cd91657 | 278 | int GPSI2C::getMessage(char* buf, int len) |
mazgch | 2:b6012cd91657 | 279 | { |
mazgch | 6:775aef3f1d1f | 280 | // fill the pipe |
mazgch | 6:775aef3f1d1f | 281 | int sz = _pipe.free(); |
mazgch | 6:775aef3f1d1f | 282 | if (sz) |
mazgch | 6:775aef3f1d1f | 283 | sz = _get(buf, sz); |
mazgch | 2:b6012cd91657 | 284 | if (sz) |
mazgch | 2:b6012cd91657 | 285 | _pipe.put(buf, sz); |
mazgch | 6:775aef3f1d1f | 286 | // now parse it |
mazgch | 2:b6012cd91657 | 287 | return _getMessage(&_pipe, buf, len); |
mazgch | 2:b6012cd91657 | 288 | } |
mazgch | 2:b6012cd91657 | 289 | |
mazgch | 4:c959dd4c5fe8 | 290 | int GPSI2C::send(const char* buf, int len) |
mazgch | 4:c959dd4c5fe8 | 291 | { |
mazgch | 4:c959dd4c5fe8 | 292 | int sent = 0; |
mazgch | 4:c959dd4c5fe8 | 293 | if (len) |
mazgch | 4:c959dd4c5fe8 | 294 | { |
mazgch | 19:2b5d097ca15d | 295 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) |
mazgch | 11:b084552b03fe | 296 | sent = send(buf, len); |
mazgch | 10:3f1c13a8763d | 297 | found = (len == sent); |
mazgch | 4:c959dd4c5fe8 | 298 | stop(); |
mazgch | 4:c959dd4c5fe8 | 299 | } |
mazgch | 4:c959dd4c5fe8 | 300 | return sent; |
mazgch | 4:c959dd4c5fe8 | 301 | } |
mazgch | 4:c959dd4c5fe8 | 302 | |
mazgch | 3:c7cd4887560d | 303 | int GPSI2C::sendNmea(const char* buf, int len) |
mazgch | 3:c7cd4887560d | 304 | { |
mazgch | 3:c7cd4887560d | 305 | int sent = 0; |
mazgch | 19:2b5d097ca15d | 306 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) |
mazgch | 11:b084552b03fe | 307 | sent = GPSParser::sendNmea(buf, len); |
mazgch | 11:b084552b03fe | 308 | found = (len == sent); |
mazgch | 11:b084552b03fe | 309 | stop(); |
mazgch | 3:c7cd4887560d | 310 | return sent; |
mazgch | 3:c7cd4887560d | 311 | } |
mazgch | 3:c7cd4887560d | 312 | |
mazgch | 3:c7cd4887560d | 313 | int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) |
mazgch | 3:c7cd4887560d | 314 | { |
mazgch | 3:c7cd4887560d | 315 | int sent = 0; |
mazgch | 19:2b5d097ca15d | 316 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) |
mazgch | 11:b084552b03fe | 317 | sent = GPSParser::sendUbx(cls, id, buf, len); |
mazgch | 11:b084552b03fe | 318 | found = (len == sent); |
mazgch | 15:5eda64e5b9d1 | 319 | I2C::stop(); |
mazgch | 3:c7cd4887560d | 320 | return sent; |
mazgch | 3:c7cd4887560d | 321 | } |
mazgch | 3:c7cd4887560d | 322 | |
mazgch | 2:b6012cd91657 | 323 | int GPSI2C::_get(char* buf, int len) |
mazgch | 2:b6012cd91657 | 324 | { |
mazgch | 3:c7cd4887560d | 325 | int read = 0; |
mazgch | 46:8ce9169e0747 | 326 | unsigned char sz[2] = {0,0}; |
mazgch | 19:2b5d097ca15d | 327 | if (!I2C::write(_i2cAdr,®LEN,sizeof(REGLEN),true) && |
mazgch | 46:8ce9169e0747 | 328 | !I2C::read(_i2cAdr,(char*)sz,sizeof(sz))) |
mazgch | 2:b6012cd91657 | 329 | { |
mazgch | 2:b6012cd91657 | 330 | int size = 256 * (int)sz[0] + sz[1]; |
mazgch | 2:b6012cd91657 | 331 | if (size > len) |
mazgch | 2:b6012cd91657 | 332 | size = len; |
mazgch | 46:8ce9169e0747 | 333 | if (size > 0) |
mazgch | 46:8ce9169e0747 | 334 | { |
mazgch | 46:8ce9169e0747 | 335 | if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true) && |
mazgch | 46:8ce9169e0747 | 336 | !I2C::read(_i2cAdr,buf,size)) { |
mazgch | 46:8ce9169e0747 | 337 | read = size; |
mazgch | 46:8ce9169e0747 | 338 | } |
mazgch | 46:8ce9169e0747 | 339 | else |
mazgch | 46:8ce9169e0747 | 340 | found = false; |
mazgch | 46:8ce9169e0747 | 341 | } |
mazgch | 2:b6012cd91657 | 342 | } |
mazgch | 2:b6012cd91657 | 343 | else |
mazgch | 2:b6012cd91657 | 344 | found = false; |
mazgch | 3:c7cd4887560d | 345 | return read; |
mazgch | 2:b6012cd91657 | 346 | } |
mazgch | 2:b6012cd91657 | 347 | |
mazgch | 4:c959dd4c5fe8 | 348 | int GPSI2C::_send(const void* buf, int len) |
mazgch | 3:c7cd4887560d | 349 | { |
mazgch | 19:2b5d097ca15d | 350 | return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; |
mazgch | 2:b6012cd91657 | 351 | } |
mazgch | 2:b6012cd91657 | 352 | |
mazgch | 2:b6012cd91657 | 353 | const char GPSI2C::REGLEN = 0xFD; |
mazgch | 2:b6012cd91657 | 354 | const char GPSI2C::REGSTREAM = 0xFF; |