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