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