Michael Shimniok / Mbed 2 deprecated ublox_demo

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)

Dependencies:   mbed

main.cpp@0:e19e2b0d0114, 2013-05-17 (annotated)

Committer:
shimniok
Date:
Fri May 17 14:50:06 2013 +0000
Revision:
0:e19e2b0d0114
Child:
1:2ea4783273cb
Initial working port

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:e19e2b0d0114 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 0:e19e2b0d0114 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 0:e19e2b0d0114 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 0:e19e2b0d0114 214 case DGPS_MSG: // NAV-DGPS
shimniok 0:e19e2b0d0114 215 pc.printf("DGPS: age = %d", LONG(4));
shimniok 0:e19e2b0d0114 216 pc.printf(", baseId = %d", INT(8));
shimniok 0:e19e2b0d0114 217 pc.printf(", numCh = %d", INT(12));
shimniok 0:e19e2b0d0114 218 break;
shimniok 0:e19e2b0d0114 219 case SBAS_MSG: // NAV-SBAS
shimniok 0:e19e2b0d0114 220 pc.printf("SBAS: geo = ");
shimniok 0:e19e2b0d0114 221 switch (data[4]) {
shimniok 0:e19e2b0d0114 222 case 133:
shimniok 0:e19e2b0d0114 223 pc.printf("Inmarsat 4F3");
shimniok 0:e19e2b0d0114 224 break;
shimniok 0:e19e2b0d0114 225 case 135:
shimniok 0:e19e2b0d0114 226 pc.printf("Galaxy 15");
shimniok 0:e19e2b0d0114 227 break;
shimniok 0:e19e2b0d0114 228 case 138:
shimniok 0:e19e2b0d0114 229 pc.printf("Anik F1R");
shimniok 0:e19e2b0d0114 230 break;
shimniok 0:e19e2b0d0114 231 default:
shimniok 0:e19e2b0d0114 232 pc.printf("%d", data[4]);
shimniok 0:e19e2b0d0114 233 break;
shimniok 0:e19e2b0d0114 234 }
shimniok 0:e19e2b0d0114 235 pc.printf(", mode = ");
shimniok 0:e19e2b0d0114 236 switch (data[5]) {
shimniok 0:e19e2b0d0114 237 case 0:
shimniok 0:e19e2b0d0114 238 pc.printf("disabled");
shimniok 0:e19e2b0d0114 239 break;
shimniok 0:e19e2b0d0114 240 case 1:
shimniok 0:e19e2b0d0114 241 pc.printf("enabled integrity");
shimniok 0:e19e2b0d0114 242 break;
shimniok 0:e19e2b0d0114 243 case 2:
shimniok 0:e19e2b0d0114 244 pc.printf("enabled test mode");
shimniok 0:e19e2b0d0114 245 break;
shimniok 0:e19e2b0d0114 246 default:
shimniok 0:e19e2b0d0114 247 pc.printf("%d", data[5]);
shimniok 0:e19e2b0d0114 248 }
shimniok 0:e19e2b0d0114 249 pc.printf(", sys = ");
shimniok 0:e19e2b0d0114 250 switch (data[6]) {
shimniok 0:e19e2b0d0114 251 case 0:
shimniok 0:e19e2b0d0114 252 pc.printf("WAAS");
shimniok 0:e19e2b0d0114 253 break;
shimniok 0:e19e2b0d0114 254 case 1:
shimniok 0:e19e2b0d0114 255 pc.printf("EGNOS");
shimniok 0:e19e2b0d0114 256 break;
shimniok 0:e19e2b0d0114 257 case 2:
shimniok 0:e19e2b0d0114 258 pc.printf("MSAS");
shimniok 0:e19e2b0d0114 259 break;
shimniok 0:e19e2b0d0114 260 case 16:
shimniok 0:e19e2b0d0114 261 pc.printf("GPS");
shimniok 0:e19e2b0d0114 262 break;
shimniok 0:e19e2b0d0114 263 default:
shimniok 0:e19e2b0d0114 264 pc.printf("%d", data[6]);
shimniok 0:e19e2b0d0114 265 }
shimniok 0:e19e2b0d0114 266 break;
shimniok 0:e19e2b0d0114 267 default:
shimniok 0:e19e2b0d0114 268 printHex(id);
shimniok 0:e19e2b0d0114 269 }
shimniok 0:e19e2b0d0114 270 pc.printf("\n");
shimniok 0:e19e2b0d0114 271 break;
shimniok 0:e19e2b0d0114 272 case 0x05: // ACK-
shimniok 0:e19e2b0d0114 273 pc.printf("ACK-");
shimniok 0:e19e2b0d0114 274 switch (id) {
shimniok 0:e19e2b0d0114 275 case 0x00: // ACK-NAK
shimniok 0:e19e2b0d0114 276 pc.printf("NAK: ");
shimniok 0:e19e2b0d0114 277 break;
shimniok 0:e19e2b0d0114 278 case 0x01: // ACK-ACK
shimniok 0:e19e2b0d0114 279 pc.printf("ACK: ");
shimniok 0:e19e2b0d0114 280 break;
shimniok 0:e19e2b0d0114 281 }
shimniok 0:e19e2b0d0114 282 printHex(data[0]);
shimniok 0:e19e2b0d0114 283 pc.printf(" ");
shimniok 0:e19e2b0d0114 284 printHex(data[1]);
shimniok 0:e19e2b0d0114 285 pc.printf("\n");
shimniok 0:e19e2b0d0114 286 break;
shimniok 0:e19e2b0d0114 287 }
shimniok 0:e19e2b0d0114 288 }
shimniok 0:e19e2b0d0114 289 state = 0;
shimniok 0:e19e2b0d0114 290 break;
shimniok 0:e19e2b0d0114 291 }
shimniok 0:e19e2b0d0114 292 }
shimniok 0:e19e2b0d0114 293 }
shimniok 0:e19e2b0d0114 294 }
shimniok 0:e19e2b0d0114 295
shimniok 0:e19e2b0d0114 296 // Convert 1e-7 value packed into long into decimal format
shimniok 0:e19e2b0d0114 297 void printLatLon (long val)
shimniok 0:e19e2b0d0114 298 {
shimniok 0:e19e2b0d0114 299 pc.printf("%x %d", val, val);
shimniok 0:e19e2b0d0114 300 }
shimniok 0:e19e2b0d0114 301
shimniok 0:e19e2b0d0114 302 void printHex (unsigned char val)
shimniok 0:e19e2b0d0114 303 {
shimniok 0:e19e2b0d0114 304 pc.printf("%02x", val);
shimniok 0:e19e2b0d0114 305 }
shimniok 0:e19e2b0d0114 306
shimniok 0:e19e2b0d0114 307 void sendCmd (unsigned char len, uint8_t data[])
shimniok 0:e19e2b0d0114 308 {
shimniok 0:e19e2b0d0114 309 gps.printf("%c%c", SYNC1, SYNC2);
shimniok 0:e19e2b0d0114 310 unsigned char chk1 = 0, chk2 = 0;
shimniok 0:e19e2b0d0114 311 for (unsigned char ii = 0; ii < len; ii++) {
shimniok 0:e19e2b0d0114 312 unsigned char cc = data[ii];
shimniok 0:e19e2b0d0114 313 gps.printf("%c", cc);
shimniok 0:e19e2b0d0114 314 chk1 += cc;
shimniok 0:e19e2b0d0114 315 chk2 += chk1;
shimniok 0:e19e2b0d0114 316 }
shimniok 0:e19e2b0d0114 317 gps.printf("%c%c", chk1, chk2);
shimniok 0:e19e2b0d0114 318 }