uitgy
Dependents: App_Pese_Ruche_SYSBEE
Fork of C027_Support by
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?
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 | 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,®STREAM,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,®STREAM,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,®STREAM,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,®STREAM,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,®LEN,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; |