3DR uBlox LEA-6H demo by Wayne Holder ported to mbed and tweaked by Michael Shimniok (https://sites.google.com/site/wayneholder/self-driving-car---part/evaluating-the-3dr-ublox-lea-6-gps)
main.cpp@2:0ebd24621d18, 2013-05-17 (annotated)
- Committer:
- shimniok
- Date:
- Fri May 17 16:25:57 2013 +0000
- Revision:
- 2:0ebd24621d18
- Parent:
- 1:2ea4783273cb
- Child:
- 3:4e131dfd860a
Minor comment tweaks
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 2:0ebd24621d18 | 1 | /** |
shimniok | 0:e19e2b0d0114 | 2 | * uBlox UBX Protocol Reader - Wayne Holder |
shimniok | 0:e19e2b0d0114 | 3 | * Ported to mbed - Michael Shimniok |
shimniok | 0:e19e2b0d0114 | 4 | * |
shimniok | 0:e19e2b0d0114 | 5 | * Note: RX pad on 3DR Module is output, TX is input |
shimniok | 0:e19e2b0d0114 | 6 | */ |
shimniok | 0:e19e2b0d0114 | 7 | #include "mbed.h" |
shimniok | 0:e19e2b0d0114 | 8 | |
shimniok | 0:e19e2b0d0114 | 9 | void printLatLon(long val); |
shimniok | 0:e19e2b0d0114 | 10 | void sendCmd(unsigned char len, uint8_t data[]); |
shimniok | 0:e19e2b0d0114 | 11 | void printHex(unsigned char val); |
shimniok | 0:e19e2b0d0114 | 12 | |
shimniok | 0:e19e2b0d0114 | 13 | #define MAX_LENGTH 512 |
shimniok | 0:e19e2b0d0114 | 14 | |
shimniok | 0:e19e2b0d0114 | 15 | #define SYNC1 0xB5 |
shimniok | 0:e19e2b0d0114 | 16 | #define SYNC2 0x62 |
shimniok | 0:e19e2b0d0114 | 17 | #define POSLLH_MSG 0x02 |
shimniok | 0:e19e2b0d0114 | 18 | #define SBAS_MSG 0x32 |
shimniok | 0:e19e2b0d0114 | 19 | #define VELNED_MSG 0x12 |
shimniok | 0:e19e2b0d0114 | 20 | #define STATUS_MSG 0x03 |
shimniok | 0:e19e2b0d0114 | 21 | #define SOL_MSG 0x06 |
shimniok | 0:e19e2b0d0114 | 22 | #define DOP_MSG 0x04 |
shimniok | 0:e19e2b0d0114 | 23 | #define DGPS_MSG 0x31 |
shimniok | 0:e19e2b0d0114 | 24 | #define SVINFO_MSG 0x30 |
shimniok | 0:e19e2b0d0114 | 25 | |
shimniok | 0:e19e2b0d0114 | 26 | |
shimniok | 0:e19e2b0d0114 | 27 | #define LONG(X) *(int32_t *)(&data[X]) |
shimniok | 0:e19e2b0d0114 | 28 | #define ULONG(X) *(uint32_t *)(&data[X]) |
shimniok | 0:e19e2b0d0114 | 29 | #define INT(X) *(int16_t *)(&data[X]) |
shimniok | 0:e19e2b0d0114 | 30 | #define UINT(X) *(uint16_t *)(&data[X]) |
shimniok | 0:e19e2b0d0114 | 31 | |
shimniok | 0:e19e2b0d0114 | 32 | unsigned char state, lstate, code, id, chk1, chk2, ck1, ck2; |
shimniok | 0:e19e2b0d0114 | 33 | unsigned int length, idx, cnt; |
shimniok | 0:e19e2b0d0114 | 34 | bool gpsReady = false; |
shimniok | 0:e19e2b0d0114 | 35 | bool checkOk = false; |
shimniok | 0:e19e2b0d0114 | 36 | |
shimniok | 0:e19e2b0d0114 | 37 | unsigned char data[MAX_LENGTH]; |
shimniok | 0:e19e2b0d0114 | 38 | |
shimniok | 0:e19e2b0d0114 | 39 | long lastTime = 0; |
shimniok | 0:e19e2b0d0114 | 40 | |
shimniok | 0:e19e2b0d0114 | 41 | Serial pc(USBTX, USBRX); |
shimniok | 0:e19e2b0d0114 | 42 | Serial gps(p9, p10); |
shimniok | 0:e19e2b0d0114 | 43 | DigitalOut led(LED1); |
shimniok | 0:e19e2b0d0114 | 44 | |
shimniok | 0:e19e2b0d0114 | 45 | unsigned char buf[MAX_LENGTH]; |
shimniok | 0:e19e2b0d0114 | 46 | int in=0; |
shimniok | 0:e19e2b0d0114 | 47 | int out=0; |
shimniok | 0:e19e2b0d0114 | 48 | |
shimniok | 0:e19e2b0d0114 | 49 | void enableMsg(unsigned char id, bool enable) |
shimniok | 0:e19e2b0d0114 | 50 | { |
shimniok | 0:e19e2b0d0114 | 51 | // MSG NAV < length > NAV |
shimniok | 0:e19e2b0d0114 | 52 | uint8_t cmdBuf[] = {0x06, 0x01, 0x03, 0x00, 0x01, id, enable ? 1 : 0}; |
shimniok | 0:e19e2b0d0114 | 53 | sendCmd(sizeof(cmdBuf), cmdBuf); |
shimniok | 0:e19e2b0d0114 | 54 | } |
shimniok | 0:e19e2b0d0114 | 55 | |
shimniok | 0:e19e2b0d0114 | 56 | void rx_handler() |
shimniok | 0:e19e2b0d0114 | 57 | { |
shimniok | 0:e19e2b0d0114 | 58 | buf[in++] = gps.getc(); |
shimniok | 0:e19e2b0d0114 | 59 | in &= (MAX_LENGTH-1); |
shimniok | 0:e19e2b0d0114 | 60 | led = !led; |
shimniok | 0:e19e2b0d0114 | 61 | } |
shimniok | 0:e19e2b0d0114 | 62 | |
shimniok | 0:e19e2b0d0114 | 63 | int main() |
shimniok | 0:e19e2b0d0114 | 64 | { |
shimniok | 0:e19e2b0d0114 | 65 | pc.baud(115200); |
shimniok | 0:e19e2b0d0114 | 66 | gps.baud(38400); |
shimniok | 0:e19e2b0d0114 | 67 | |
shimniok | 0:e19e2b0d0114 | 68 | gps.attach( &rx_handler ); |
shimniok | 0:e19e2b0d0114 | 69 | |
shimniok | 0:e19e2b0d0114 | 70 | pc.printf("ublox demo starting...\n"); |
shimniok | 0:e19e2b0d0114 | 71 | led = 1; |
shimniok | 0:e19e2b0d0114 | 72 | |
shimniok | 0:e19e2b0d0114 | 73 | // Configure GPS |
shimniok | 0:e19e2b0d0114 | 74 | |
shimniok | 0:e19e2b0d0114 | 75 | uint8_t cmdbuf[40]; |
shimniok | 0:e19e2b0d0114 | 76 | for (int i=0; i < 38; i++) |
shimniok | 0:e19e2b0d0114 | 77 | cmdbuf[i] = 0; |
shimniok | 0:e19e2b0d0114 | 78 | cmdbuf[0] = 0x06; // NAV-CFG5 |
shimniok | 0:e19e2b0d0114 | 79 | cmdbuf[1] = 0x24; // NAV-CFG5 |
shimniok | 0:e19e2b0d0114 | 80 | cmdbuf[2] = 0x00; // X2 bitmask |
shimniok | 0:e19e2b0d0114 | 81 | cmdbuf[3] = 0x01; // bitmask: dynamic model |
shimniok | 0:e19e2b0d0114 | 82 | cmdbuf[4] = 0x04; // U1 automotive dyn model |
shimniok | 0:e19e2b0d0114 | 83 | sendCmd(38, cmdbuf); |
shimniok | 0:e19e2b0d0114 | 84 | |
shimniok | 0:e19e2b0d0114 | 85 | |
shimniok | 0:e19e2b0d0114 | 86 | // Modify these to control which messages are sent from module |
shimniok | 0:e19e2b0d0114 | 87 | enableMsg(POSLLH_MSG, true); // Enable position messages |
shimniok | 0:e19e2b0d0114 | 88 | enableMsg(SBAS_MSG, false); // Enable SBAS messages |
shimniok | 0:e19e2b0d0114 | 89 | enableMsg(VELNED_MSG, true); // Enable velocity messages |
shimniok | 0:e19e2b0d0114 | 90 | enableMsg(STATUS_MSG, true); // Enable status messages |
shimniok | 0:e19e2b0d0114 | 91 | enableMsg(SOL_MSG, true); // Enable soluton messages |
shimniok | 0:e19e2b0d0114 | 92 | enableMsg(DOP_MSG, false); // Enable DOP messages |
shimniok | 0:e19e2b0d0114 | 93 | enableMsg(SVINFO_MSG, true); // Enable SV info messages |
shimniok | 0:e19e2b0d0114 | 94 | enableMsg(DGPS_MSG, false); // Disable DGPS messages |
shimniok | 0:e19e2b0d0114 | 95 | |
shimniok | 0:e19e2b0d0114 | 96 | while (1) { |
shimniok | 0:e19e2b0d0114 | 97 | if (in != out) { |
shimniok | 0:e19e2b0d0114 | 98 | |
shimniok | 0:e19e2b0d0114 | 99 | unsigned char cc = buf[out++]; |
shimniok | 0:e19e2b0d0114 | 100 | out &= (MAX_LENGTH-1); |
shimniok | 0:e19e2b0d0114 | 101 | |
shimniok | 0:e19e2b0d0114 | 102 | switch (state) { |
shimniok | 0:e19e2b0d0114 | 103 | case 0: // wait for sync 1 (0xB5) |
shimniok | 0:e19e2b0d0114 | 104 | ck1 = ck2 = 0; |
shimniok | 0:e19e2b0d0114 | 105 | checkOk = false; |
shimniok | 0:e19e2b0d0114 | 106 | if (cc == SYNC1) |
shimniok | 0:e19e2b0d0114 | 107 | state++; |
shimniok | 0:e19e2b0d0114 | 108 | break; |
shimniok | 0:e19e2b0d0114 | 109 | case 1: // wait for sync 2 (0x62) |
shimniok | 0:e19e2b0d0114 | 110 | if (cc == SYNC2) |
shimniok | 0:e19e2b0d0114 | 111 | state++; |
shimniok | 0:e19e2b0d0114 | 112 | else |
shimniok | 0:e19e2b0d0114 | 113 | state = 0; |
shimniok | 0:e19e2b0d0114 | 114 | break; |
shimniok | 0:e19e2b0d0114 | 115 | case 2: // wait for class code |
shimniok | 0:e19e2b0d0114 | 116 | code = cc; |
shimniok | 0:e19e2b0d0114 | 117 | ck1 += cc; |
shimniok | 0:e19e2b0d0114 | 118 | ck2 += ck1; |
shimniok | 0:e19e2b0d0114 | 119 | state++; |
shimniok | 0:e19e2b0d0114 | 120 | break; |
shimniok | 0:e19e2b0d0114 | 121 | case 3: // wait for Id |
shimniok | 0:e19e2b0d0114 | 122 | id = cc; |
shimniok | 0:e19e2b0d0114 | 123 | ck1 += cc; |
shimniok | 0:e19e2b0d0114 | 124 | ck2 += ck1; |
shimniok | 0:e19e2b0d0114 | 125 | state++; |
shimniok | 0:e19e2b0d0114 | 126 | break; |
shimniok | 0:e19e2b0d0114 | 127 | case 4: // wait for length uint8_t 1 |
shimniok | 0:e19e2b0d0114 | 128 | length = cc; |
shimniok | 0:e19e2b0d0114 | 129 | ck1 += cc; |
shimniok | 0:e19e2b0d0114 | 130 | ck2 += ck1; |
shimniok | 0:e19e2b0d0114 | 131 | state++; |
shimniok | 0:e19e2b0d0114 | 132 | break; |
shimniok | 0:e19e2b0d0114 | 133 | case 5: // wait for length uint8_t 2 |
shimniok | 0:e19e2b0d0114 | 134 | length |= (unsigned int) cc << 8; |
shimniok | 0:e19e2b0d0114 | 135 | ck1 += cc; |
shimniok | 0:e19e2b0d0114 | 136 | ck2 += ck1; |
shimniok | 0:e19e2b0d0114 | 137 | idx = 0; |
shimniok | 0:e19e2b0d0114 | 138 | state++; |
shimniok | 0:e19e2b0d0114 | 139 | if (length > MAX_LENGTH) |
shimniok | 0:e19e2b0d0114 | 140 | state= 0; |
shimniok | 0:e19e2b0d0114 | 141 | break; |
shimniok | 0:e19e2b0d0114 | 142 | case 6: // wait for <length> payload uint8_ts |
shimniok | 0:e19e2b0d0114 | 143 | data[idx++] = cc; |
shimniok | 0:e19e2b0d0114 | 144 | ck1 += cc; |
shimniok | 0:e19e2b0d0114 | 145 | ck2 += ck1; |
shimniok | 0:e19e2b0d0114 | 146 | if (idx >= length) { |
shimniok | 0:e19e2b0d0114 | 147 | state++; |
shimniok | 0:e19e2b0d0114 | 148 | } |
shimniok | 0:e19e2b0d0114 | 149 | break; |
shimniok | 0:e19e2b0d0114 | 150 | case 7: // wait for checksum 1 |
shimniok | 0:e19e2b0d0114 | 151 | chk1 = cc; |
shimniok | 0:e19e2b0d0114 | 152 | state++; |
shimniok | 0:e19e2b0d0114 | 153 | break; |
shimniok | 1:2ea4783273cb | 154 | case 8: { // wait for checksum 2 |
shimniok | 0:e19e2b0d0114 | 155 | chk2 = cc; |
shimniok | 0:e19e2b0d0114 | 156 | checkOk = ck1 == chk1 && ck2 == chk2; |
shimniok | 0:e19e2b0d0114 | 157 | if (!checkOk) { |
shimniok | 0:e19e2b0d0114 | 158 | pc.printf("Checksum failed\n"); |
shimniok | 0:e19e2b0d0114 | 159 | } else { |
shimniok | 0:e19e2b0d0114 | 160 | switch (code) { |
shimniok | 0:e19e2b0d0114 | 161 | case 0x01: // NAV- |
shimniok | 0:e19e2b0d0114 | 162 | // Add blank line between time groups |
shimniok | 0:e19e2b0d0114 | 163 | if (lastTime != ULONG(0)) { |
shimniok | 0:e19e2b0d0114 | 164 | lastTime = ULONG(0); |
shimniok | 0:e19e2b0d0114 | 165 | pc.printf("\nTime: %u\n", ULONG(0) ); |
shimniok | 0:e19e2b0d0114 | 166 | } |
shimniok | 0:e19e2b0d0114 | 167 | pc.printf("NAV-"); |
shimniok | 0:e19e2b0d0114 | 168 | switch (id) { |
shimniok | 0:e19e2b0d0114 | 169 | case POSLLH_MSG: // NAV-POSLLH |
shimniok | 0:e19e2b0d0114 | 170 | pc.printf("POSLLH: lon = "); |
shimniok | 0:e19e2b0d0114 | 171 | printLatLon(LONG(4)); |
shimniok | 0:e19e2b0d0114 | 172 | pc.printf(", lat = "); |
shimniok | 0:e19e2b0d0114 | 173 | printLatLon(LONG(8)); |
shimniok | 0:e19e2b0d0114 | 174 | pc.printf(", vAcc = %u", ULONG(24)); |
shimniok | 0:e19e2b0d0114 | 175 | pc.printf(" mm, hAcc = %u", ULONG(20)); |
shimniok | 0:e19e2b0d0114 | 176 | pc.printf(" mm"); |
shimniok | 0:e19e2b0d0114 | 177 | break; |
shimniok | 0:e19e2b0d0114 | 178 | case STATUS_MSG: // NAV-STATUS |
shimniok | 0:e19e2b0d0114 | 179 | pc.printf("STATUS: gpsFix = %d", data[4]); |
shimniok | 0:e19e2b0d0114 | 180 | if (data[5] & 2) { |
shimniok | 0:e19e2b0d0114 | 181 | pc.printf(", dgpsFix"); |
shimniok | 0:e19e2b0d0114 | 182 | } |
shimniok | 0:e19e2b0d0114 | 183 | break; |
shimniok | 0:e19e2b0d0114 | 184 | case DOP_MSG: // NAV-DOP |
shimniok | 0:e19e2b0d0114 | 185 | pc.printf("DOP: gDOP = %.1f", ((float) UINT(4))/100.0); |
shimniok | 0:e19e2b0d0114 | 186 | pc.printf(", tDOP = %.1f", ((float) UINT(8))/100.0); |
shimniok | 0:e19e2b0d0114 | 187 | pc.printf(", vDOP = %.1f", ((float) UINT(10))/100.0); |
shimniok | 0:e19e2b0d0114 | 188 | pc.printf(", hDOP = %.1f", ((float) UINT(12))/100.0); |
shimniok | 0:e19e2b0d0114 | 189 | break; |
shimniok | 0:e19e2b0d0114 | 190 | case SOL_MSG: // NAV-SOL |
shimniok | 0:e19e2b0d0114 | 191 | pc.printf("SOL: week = %u", UINT(8)); |
shimniok | 0:e19e2b0d0114 | 192 | pc.printf(", gpsFix = ", data[10]); |
shimniok | 0:e19e2b0d0114 | 193 | pc.printf(", pDOP = %.1f", ((float) UINT(44))/ 100.0); |
shimniok | 0:e19e2b0d0114 | 194 | pc.printf(", pAcc = %u", ULONG(24)); |
shimniok | 0:e19e2b0d0114 | 195 | pc.printf(" cm, numSV = %d", data[47]); |
shimniok | 0:e19e2b0d0114 | 196 | break; |
shimniok | 0:e19e2b0d0114 | 197 | case VELNED_MSG: // NAV-VELNED |
shimniok | 0:e19e2b0d0114 | 198 | pc.printf("VELNED: gSpeed = %u", ULONG(20)); |
shimniok | 0:e19e2b0d0114 | 199 | pc.printf(" cm/sec, sAcc = %u", ULONG(28)); |
shimniok | 0:e19e2b0d0114 | 200 | pc.printf(" cm/sec, heading = %.2f", ((float) LONG(24))/100000.0); |
shimniok | 0:e19e2b0d0114 | 201 | pc.printf(" deg, cAcc = %.2f", ((float) LONG(32))/100000.0); |
shimniok | 0:e19e2b0d0114 | 202 | pc.printf(" deg"); |
shimniok | 0:e19e2b0d0114 | 203 | break; |
shimniok | 1:2ea4783273cb | 204 | case SVINFO_MSG: { // NAV-SVINFO |
shimniok | 0:e19e2b0d0114 | 205 | unsigned int nc = data[4]; // number of channels |
shimniok | 0:e19e2b0d0114 | 206 | pc.printf("SVINFO: channels = %u\n", nc); |
shimniok | 0:e19e2b0d0114 | 207 | for (int ch = 0; ch < nc; ch++) { |
shimniok | 0:e19e2b0d0114 | 208 | pc.printf(" id = %u ", data[9+12*ch]); |
shimniok | 0:e19e2b0d0114 | 209 | for (int q=0; q < data[12+12*ch]/10; q++) |
shimniok | 0:e19e2b0d0114 | 210 | pc.printf("*"); |
shimniok | 0:e19e2b0d0114 | 211 | pc.printf("\n"); |
shimniok | 0:e19e2b0d0114 | 212 | } |
shimniok | 0:e19e2b0d0114 | 213 | break; |
shimniok | 1:2ea4783273cb | 214 | } |
shimniok | 0:e19e2b0d0114 | 215 | case DGPS_MSG: // NAV-DGPS |
shimniok | 0:e19e2b0d0114 | 216 | pc.printf("DGPS: age = %d", LONG(4)); |
shimniok | 0:e19e2b0d0114 | 217 | pc.printf(", baseId = %d", INT(8)); |
shimniok | 0:e19e2b0d0114 | 218 | pc.printf(", numCh = %d", INT(12)); |
shimniok | 0:e19e2b0d0114 | 219 | break; |
shimniok | 0:e19e2b0d0114 | 220 | case SBAS_MSG: // NAV-SBAS |
shimniok | 0:e19e2b0d0114 | 221 | pc.printf("SBAS: geo = "); |
shimniok | 0:e19e2b0d0114 | 222 | switch (data[4]) { |
shimniok | 0:e19e2b0d0114 | 223 | case 133: |
shimniok | 0:e19e2b0d0114 | 224 | pc.printf("Inmarsat 4F3"); |
shimniok | 0:e19e2b0d0114 | 225 | break; |
shimniok | 0:e19e2b0d0114 | 226 | case 135: |
shimniok | 0:e19e2b0d0114 | 227 | pc.printf("Galaxy 15"); |
shimniok | 0:e19e2b0d0114 | 228 | break; |
shimniok | 0:e19e2b0d0114 | 229 | case 138: |
shimniok | 0:e19e2b0d0114 | 230 | pc.printf("Anik F1R"); |
shimniok | 0:e19e2b0d0114 | 231 | break; |
shimniok | 0:e19e2b0d0114 | 232 | default: |
shimniok | 0:e19e2b0d0114 | 233 | pc.printf("%d", data[4]); |
shimniok | 0:e19e2b0d0114 | 234 | break; |
shimniok | 0:e19e2b0d0114 | 235 | } |
shimniok | 0:e19e2b0d0114 | 236 | pc.printf(", mode = "); |
shimniok | 0:e19e2b0d0114 | 237 | switch (data[5]) { |
shimniok | 0:e19e2b0d0114 | 238 | case 0: |
shimniok | 0:e19e2b0d0114 | 239 | pc.printf("disabled"); |
shimniok | 0:e19e2b0d0114 | 240 | break; |
shimniok | 0:e19e2b0d0114 | 241 | case 1: |
shimniok | 0:e19e2b0d0114 | 242 | pc.printf("enabled integrity"); |
shimniok | 0:e19e2b0d0114 | 243 | break; |
shimniok | 0:e19e2b0d0114 | 244 | case 2: |
shimniok | 0:e19e2b0d0114 | 245 | pc.printf("enabled test mode"); |
shimniok | 0:e19e2b0d0114 | 246 | break; |
shimniok | 0:e19e2b0d0114 | 247 | default: |
shimniok | 0:e19e2b0d0114 | 248 | pc.printf("%d", data[5]); |
shimniok | 0:e19e2b0d0114 | 249 | } |
shimniok | 0:e19e2b0d0114 | 250 | pc.printf(", sys = "); |
shimniok | 0:e19e2b0d0114 | 251 | switch (data[6]) { |
shimniok | 0:e19e2b0d0114 | 252 | case 0: |
shimniok | 0:e19e2b0d0114 | 253 | pc.printf("WAAS"); |
shimniok | 0:e19e2b0d0114 | 254 | break; |
shimniok | 0:e19e2b0d0114 | 255 | case 1: |
shimniok | 0:e19e2b0d0114 | 256 | pc.printf("EGNOS"); |
shimniok | 0:e19e2b0d0114 | 257 | break; |
shimniok | 0:e19e2b0d0114 | 258 | case 2: |
shimniok | 0:e19e2b0d0114 | 259 | pc.printf("MSAS"); |
shimniok | 0:e19e2b0d0114 | 260 | break; |
shimniok | 0:e19e2b0d0114 | 261 | case 16: |
shimniok | 0:e19e2b0d0114 | 262 | pc.printf("GPS"); |
shimniok | 0:e19e2b0d0114 | 263 | break; |
shimniok | 0:e19e2b0d0114 | 264 | default: |
shimniok | 0:e19e2b0d0114 | 265 | pc.printf("%d", data[6]); |
shimniok | 0:e19e2b0d0114 | 266 | } |
shimniok | 0:e19e2b0d0114 | 267 | break; |
shimniok | 0:e19e2b0d0114 | 268 | default: |
shimniok | 0:e19e2b0d0114 | 269 | printHex(id); |
shimniok | 0:e19e2b0d0114 | 270 | } |
shimniok | 0:e19e2b0d0114 | 271 | pc.printf("\n"); |
shimniok | 0:e19e2b0d0114 | 272 | break; |
shimniok | 0:e19e2b0d0114 | 273 | case 0x05: // ACK- |
shimniok | 0:e19e2b0d0114 | 274 | pc.printf("ACK-"); |
shimniok | 0:e19e2b0d0114 | 275 | switch (id) { |
shimniok | 0:e19e2b0d0114 | 276 | case 0x00: // ACK-NAK |
shimniok | 0:e19e2b0d0114 | 277 | pc.printf("NAK: "); |
shimniok | 0:e19e2b0d0114 | 278 | break; |
shimniok | 0:e19e2b0d0114 | 279 | case 0x01: // ACK-ACK |
shimniok | 0:e19e2b0d0114 | 280 | pc.printf("ACK: "); |
shimniok | 0:e19e2b0d0114 | 281 | break; |
shimniok | 0:e19e2b0d0114 | 282 | } |
shimniok | 0:e19e2b0d0114 | 283 | printHex(data[0]); |
shimniok | 0:e19e2b0d0114 | 284 | pc.printf(" "); |
shimniok | 0:e19e2b0d0114 | 285 | printHex(data[1]); |
shimniok | 0:e19e2b0d0114 | 286 | pc.printf("\n"); |
shimniok | 0:e19e2b0d0114 | 287 | break; |
shimniok | 0:e19e2b0d0114 | 288 | } |
shimniok | 0:e19e2b0d0114 | 289 | } |
shimniok | 0:e19e2b0d0114 | 290 | state = 0; |
shimniok | 0:e19e2b0d0114 | 291 | break; |
shimniok | 1:2ea4783273cb | 292 | } |
shimniok | 1:2ea4783273cb | 293 | default: |
shimniok | 1:2ea4783273cb | 294 | break; |
shimniok | 0:e19e2b0d0114 | 295 | } |
shimniok | 0:e19e2b0d0114 | 296 | } |
shimniok | 0:e19e2b0d0114 | 297 | } |
shimniok | 0:e19e2b0d0114 | 298 | } |
shimniok | 0:e19e2b0d0114 | 299 | |
shimniok | 0:e19e2b0d0114 | 300 | // Convert 1e-7 value packed into long into decimal format |
shimniok | 0:e19e2b0d0114 | 301 | void printLatLon (long val) |
shimniok | 0:e19e2b0d0114 | 302 | { |
shimniok | 1:2ea4783273cb | 303 | int d = 10000000; |
shimniok | 1:2ea4783273cb | 304 | pc.printf("%d.%d", val/d, abs(val%d)); |
shimniok | 0:e19e2b0d0114 | 305 | } |
shimniok | 0:e19e2b0d0114 | 306 | |
shimniok | 0:e19e2b0d0114 | 307 | void printHex (unsigned char val) |
shimniok | 0:e19e2b0d0114 | 308 | { |
shimniok | 0:e19e2b0d0114 | 309 | pc.printf("%02x", val); |
shimniok | 0:e19e2b0d0114 | 310 | } |
shimniok | 0:e19e2b0d0114 | 311 | |
shimniok | 0:e19e2b0d0114 | 312 | void sendCmd (unsigned char len, uint8_t data[]) |
shimniok | 0:e19e2b0d0114 | 313 | { |
shimniok | 0:e19e2b0d0114 | 314 | gps.printf("%c%c", SYNC1, SYNC2); |
shimniok | 0:e19e2b0d0114 | 315 | unsigned char chk1 = 0, chk2 = 0; |
shimniok | 0:e19e2b0d0114 | 316 | for (unsigned char ii = 0; ii < len; ii++) { |
shimniok | 0:e19e2b0d0114 | 317 | unsigned char cc = data[ii]; |
shimniok | 0:e19e2b0d0114 | 318 | gps.printf("%c", cc); |
shimniok | 0:e19e2b0d0114 | 319 | chk1 += cc; |
shimniok | 0:e19e2b0d0114 | 320 | chk2 += chk1; |
shimniok | 0:e19e2b0d0114 | 321 | } |
shimniok | 0:e19e2b0d0114 | 322 | gps.printf("%c%c", chk1, chk2); |
shimniok | 0:e19e2b0d0114 | 323 | } |