Michael Shimniok
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)
Diff: main.cpp
- Revision:
- 0:e19e2b0d0114
- Child:
- 1:2ea4783273cb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 17 14:50:06 2013 +0000
@@ -0,0 +1,318 @@
+/*
+ * 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);
+}