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
- Committer:
- shimniok
- Date:
- 2013-05-17
- Revision:
- 0:e19e2b0d0114
- Child:
- 1:2ea4783273cb
File content as of revision 0:e19e2b0d0114:
/* * uBlox UBX Protocol Reader - Wayne Holder * Ported to mbed - Michael Shimniok * * Note: RX pad on 3DR Module is output, TX is input */ #include "mbed.h" void printLatLon(long val); void sendCmd(unsigned char len, uint8_t data[]); void printHex(unsigned char val); #define MAX_LENGTH 512 #define SYNC1 0xB5 #define SYNC2 0x62 #define POSLLH_MSG 0x02 #define SBAS_MSG 0x32 #define VELNED_MSG 0x12 #define STATUS_MSG 0x03 #define SOL_MSG 0x06 #define DOP_MSG 0x04 #define DGPS_MSG 0x31 #define SVINFO_MSG 0x30 #define LONG(X) *(int32_t *)(&data[X]) #define ULONG(X) *(uint32_t *)(&data[X]) #define INT(X) *(int16_t *)(&data[X]) #define UINT(X) *(uint16_t *)(&data[X]) unsigned char state, lstate, code, id, chk1, chk2, ck1, ck2; unsigned int length, idx, cnt; bool gpsReady = false; bool checkOk = false; unsigned char data[MAX_LENGTH]; long lastTime = 0; Serial pc(USBTX, USBRX); Serial gps(p9, p10); DigitalOut led(LED1); unsigned char buf[MAX_LENGTH]; int in=0; int out=0; void enableMsg(unsigned char id, bool enable) { // MSG NAV < length > NAV uint8_t cmdBuf[] = {0x06, 0x01, 0x03, 0x00, 0x01, id, enable ? 1 : 0}; sendCmd(sizeof(cmdBuf), cmdBuf); } void rx_handler() { buf[in++] = gps.getc(); in &= (MAX_LENGTH-1); led = !led; } int main() { pc.baud(115200); gps.baud(38400); gps.attach( &rx_handler ); pc.printf("ublox demo starting...\n"); led = 1; // Configure GPS uint8_t cmdbuf[40]; for (int i=0; i < 38; i++) cmdbuf[i] = 0; cmdbuf[0] = 0x06; // NAV-CFG5 cmdbuf[1] = 0x24; // NAV-CFG5 cmdbuf[2] = 0x00; // X2 bitmask cmdbuf[3] = 0x01; // bitmask: dynamic model cmdbuf[4] = 0x04; // U1 automotive dyn model sendCmd(38, cmdbuf); // Modify these to control which messages are sent from module enableMsg(POSLLH_MSG, true); // Enable position messages enableMsg(SBAS_MSG, false); // Enable SBAS messages enableMsg(VELNED_MSG, true); // Enable velocity messages enableMsg(STATUS_MSG, true); // Enable status messages enableMsg(SOL_MSG, true); // Enable soluton messages enableMsg(DOP_MSG, false); // Enable DOP messages enableMsg(SVINFO_MSG, true); // Enable SV info messages enableMsg(DGPS_MSG, false); // Disable DGPS messages while (1) { if (in != out) { unsigned char cc = buf[out++]; out &= (MAX_LENGTH-1); switch (state) { case 0: // wait for sync 1 (0xB5) ck1 = ck2 = 0; checkOk = false; if (cc == SYNC1) state++; break; case 1: // wait for sync 2 (0x62) if (cc == SYNC2) state++; else state = 0; break; case 2: // wait for class code code = cc; ck1 += cc; ck2 += ck1; state++; break; case 3: // wait for Id id = cc; ck1 += cc; ck2 += ck1; state++; break; case 4: // wait for length uint8_t 1 length = cc; ck1 += cc; ck2 += ck1; state++; break; case 5: // wait for length uint8_t 2 length |= (unsigned int) cc << 8; ck1 += cc; ck2 += ck1; idx = 0; state++; if (length > MAX_LENGTH) state= 0; break; case 6: // wait for <length> payload uint8_ts data[idx++] = cc; ck1 += cc; ck2 += ck1; if (idx >= length) { state++; } break; case 7: // wait for checksum 1 chk1 = cc; state++; break; case 8: // wait for checksum 2 chk2 = cc; checkOk = ck1 == chk1 && ck2 == chk2; if (!checkOk) { pc.printf("Checksum failed\n"); } else { switch (code) { case 0x01: // NAV- // Add blank line between time groups if (lastTime != ULONG(0)) { lastTime = ULONG(0); pc.printf("\nTime: %u\n", ULONG(0) ); } pc.printf("NAV-"); switch (id) { case POSLLH_MSG: // NAV-POSLLH pc.printf("POSLLH: lon = "); printLatLon(LONG(4)); pc.printf(", lat = "); printLatLon(LONG(8)); pc.printf(", vAcc = %u", ULONG(24)); pc.printf(" mm, hAcc = %u", ULONG(20)); pc.printf(" mm"); break; case STATUS_MSG: // NAV-STATUS pc.printf("STATUS: gpsFix = %d", data[4]); if (data[5] & 2) { pc.printf(", dgpsFix"); } break; case DOP_MSG: // NAV-DOP pc.printf("DOP: gDOP = %.1f", ((float) UINT(4))/100.0); pc.printf(", tDOP = %.1f", ((float) UINT(8))/100.0); pc.printf(", vDOP = %.1f", ((float) UINT(10))/100.0); pc.printf(", hDOP = %.1f", ((float) UINT(12))/100.0); break; case SOL_MSG: // NAV-SOL pc.printf("SOL: week = %u", UINT(8)); pc.printf(", gpsFix = ", data[10]); pc.printf(", pDOP = %.1f", ((float) UINT(44))/ 100.0); pc.printf(", pAcc = %u", ULONG(24)); pc.printf(" cm, numSV = %d", data[47]); break; case VELNED_MSG: // NAV-VELNED pc.printf("VELNED: gSpeed = %u", ULONG(20)); pc.printf(" cm/sec, sAcc = %u", ULONG(28)); pc.printf(" cm/sec, heading = %.2f", ((float) LONG(24))/100000.0); pc.printf(" deg, cAcc = %.2f", ((float) LONG(32))/100000.0); pc.printf(" deg"); break; case SVINFO_MSG: // NAV-SVINFO unsigned int nc = data[4]; // number of channels pc.printf("SVINFO: channels = %u\n", nc); for (int ch = 0; ch < nc; ch++) { pc.printf(" id = %u ", data[9+12*ch]); for (int q=0; q < data[12+12*ch]/10; q++) pc.printf("*"); pc.printf("\n"); } break; case DGPS_MSG: // NAV-DGPS pc.printf("DGPS: age = %d", LONG(4)); pc.printf(", baseId = %d", INT(8)); pc.printf(", numCh = %d", INT(12)); break; case SBAS_MSG: // NAV-SBAS pc.printf("SBAS: geo = "); switch (data[4]) { case 133: pc.printf("Inmarsat 4F3"); break; case 135: pc.printf("Galaxy 15"); break; case 138: pc.printf("Anik F1R"); break; default: pc.printf("%d", data[4]); break; } pc.printf(", mode = "); switch (data[5]) { case 0: pc.printf("disabled"); break; case 1: pc.printf("enabled integrity"); break; case 2: pc.printf("enabled test mode"); break; default: pc.printf("%d", data[5]); } pc.printf(", sys = "); switch (data[6]) { case 0: pc.printf("WAAS"); break; case 1: pc.printf("EGNOS"); break; case 2: pc.printf("MSAS"); break; case 16: pc.printf("GPS"); break; default: pc.printf("%d", data[6]); } break; default: printHex(id); } pc.printf("\n"); break; case 0x05: // ACK- pc.printf("ACK-"); switch (id) { case 0x00: // ACK-NAK pc.printf("NAK: "); break; case 0x01: // ACK-ACK pc.printf("ACK: "); break; } printHex(data[0]); pc.printf(" "); printHex(data[1]); pc.printf("\n"); break; } } state = 0; break; } } } } // Convert 1e-7 value packed into long into decimal format void printLatLon (long val) { pc.printf("%x %d", val, val); } void printHex (unsigned char val) { pc.printf("%02x", val); } void sendCmd (unsigned char len, uint8_t data[]) { gps.printf("%c%c", SYNC1, SYNC2); unsigned char chk1 = 0, chk2 = 0; for (unsigned char ii = 0; ii < len; ii++) { unsigned char cc = data[ii]; gps.printf("%c", cc); chk1 += cc; chk2 += chk1; } gps.printf("%c%c", chk1, chk2); }