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