Michael Shimniok
/
ublox_demo
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /** 00002 * uBlox UBX Protocol Reader - Wayne Holder 00003 * Ported to mbed - Michael Shimniok 00004 * 00005 * Note: RX pad on 3DR Module is output, TX is input 00006 */ 00007 #include "mbed.h" 00008 00009 void printLatLon(long val); 00010 void sendCmd(unsigned char len, uint8_t data[]); 00011 void printHex(unsigned char val); 00012 00013 #define MAX_LENGTH 512 00014 00015 #define SYNC1 0xB5 00016 #define SYNC2 0x62 00017 #define POSLLH_MSG 0x02 00018 #define SBAS_MSG 0x32 00019 #define VELNED_MSG 0x12 00020 #define STATUS_MSG 0x03 00021 #define SOL_MSG 0x06 00022 #define DOP_MSG 0x04 00023 #define DGPS_MSG 0x31 00024 #define SVINFO_MSG 0x30 00025 00026 00027 #define LONG(X) *(int32_t *)(&data[X]) 00028 #define ULONG(X) *(uint32_t *)(&data[X]) 00029 #define INT(X) *(int16_t *)(&data[X]) 00030 #define UINT(X) *(uint16_t *)(&data[X]) 00031 00032 unsigned char state, lstate, code, id, chk1, chk2, ck1, ck2; 00033 unsigned int length, idx, cnt; 00034 bool gpsReady = false; 00035 bool checkOk = false; 00036 00037 unsigned char data[MAX_LENGTH]; 00038 00039 long lastTime = 0; 00040 00041 Serial pc(USBTX, USBRX); 00042 Serial gps(p9, p10); 00043 DigitalOut led(LED1); 00044 00045 unsigned char buf[MAX_LENGTH]; 00046 int in=0; 00047 int out=0; 00048 00049 void enableMsg(unsigned char id, bool enable, int rate=1) 00050 { 00051 if (!enable) rate = 0; 00052 uint8_t cmdBuf[] = { 00053 0x06, // class CFG 00054 0x01, // id MSG -> CFG-MSG 00055 8, // length, for config message rates 00056 0x01, // class, 00057 id, // id, 00058 0x0, // target 0 rate (DDC/I2C) 00059 rate, // target 1 rate (UART1) 00060 0x0, // target 2 rate (UART2) 00061 0x0, // target 3 rate (USB) 00062 0x0, // target 4 rate (SPI) 00063 0x0, // target 5 rate (reserved) 00064 }; 00065 sendCmd(sizeof(cmdBuf), cmdBuf); 00066 } 00067 00068 void rx_handler() 00069 { 00070 buf[in++] = gps.getc(); 00071 in &= (MAX_LENGTH-1); 00072 led = !led; 00073 } 00074 00075 int main() 00076 { 00077 pc.baud(115200); 00078 gps.baud(38400); 00079 00080 gps.attach( &rx_handler ); 00081 00082 pc.printf("ublox demo starting...\n"); 00083 led = 1; 00084 00085 // Configure GPS 00086 00087 uint8_t cmdbuf[40]; 00088 for (int i=0; i < 38; i++) 00089 cmdbuf[i] = 0; 00090 cmdbuf[0] = 0x06; // NAV-CFG5 00091 cmdbuf[1] = 0x24; // NAV-CFG5 00092 cmdbuf[2] = 0x00; // X2 bitmask 00093 cmdbuf[3] = 0x01; // bitmask: dynamic model 00094 cmdbuf[4] = 0x04; // U1 automotive dyn model 00095 sendCmd(38, cmdbuf); 00096 00097 00098 // Modify these to control which messages are sent from module 00099 enableMsg(POSLLH_MSG, true); // Enable position messages 00100 enableMsg(SBAS_MSG, false); // Enable SBAS messages 00101 enableMsg(VELNED_MSG, true); // Enable velocity messages 00102 enableMsg(STATUS_MSG, true); // Enable status messages 00103 enableMsg(SOL_MSG, true); // Enable soluton messages 00104 enableMsg(DOP_MSG, false); // Enable DOP messages 00105 enableMsg(SVINFO_MSG, true, 2); // Enable SV info messages 00106 enableMsg(DGPS_MSG, false); // Disable DGPS messages 00107 00108 while (1) { 00109 if (in != out) { 00110 00111 unsigned char cc = buf[out++]; 00112 out &= (MAX_LENGTH-1); 00113 00114 switch (state) { 00115 case 0: // wait for sync 1 (0xB5) 00116 ck1 = ck2 = 0; 00117 checkOk = false; 00118 if (cc == SYNC1) 00119 state++; 00120 break; 00121 case 1: // wait for sync 2 (0x62) 00122 if (cc == SYNC2) 00123 state++; 00124 else 00125 state = 0; 00126 break; 00127 case 2: // wait for class code 00128 code = cc; 00129 ck1 += cc; 00130 ck2 += ck1; 00131 state++; 00132 break; 00133 case 3: // wait for Id 00134 id = cc; 00135 ck1 += cc; 00136 ck2 += ck1; 00137 state++; 00138 break; 00139 case 4: // wait for length uint8_t 1 00140 length = cc; 00141 ck1 += cc; 00142 ck2 += ck1; 00143 state++; 00144 break; 00145 case 5: // wait for length uint8_t 2 00146 length |= (unsigned int) cc << 8; 00147 ck1 += cc; 00148 ck2 += ck1; 00149 idx = 0; 00150 state++; 00151 if (length > MAX_LENGTH) 00152 state= 0; 00153 break; 00154 case 6: // wait for <length> payload uint8_ts 00155 data[idx++] = cc; 00156 ck1 += cc; 00157 ck2 += ck1; 00158 if (idx >= length) { 00159 state++; 00160 } 00161 break; 00162 case 7: // wait for checksum 1 00163 chk1 = cc; 00164 state++; 00165 break; 00166 case 8: { // wait for checksum 2 00167 chk2 = cc; 00168 checkOk = ck1 == chk1 && ck2 == chk2; 00169 if (!checkOk) { 00170 pc.printf("Checksum failed\n"); 00171 } else { 00172 switch (code) { 00173 case 0x01: // NAV- 00174 // Add blank line between time groups 00175 if (lastTime != ULONG(0)) { 00176 lastTime = ULONG(0); 00177 pc.printf("\nTime: %u\n", ULONG(0) ); 00178 } 00179 pc.printf("NAV-"); 00180 switch (id) { 00181 case POSLLH_MSG: // NAV-POSLLH 00182 pc.printf("POSLLH: lon = "); 00183 printLatLon(LONG(4)); 00184 pc.printf(", lat = "); 00185 printLatLon(LONG(8)); 00186 pc.printf(", vAcc = %u", ULONG(24)); 00187 pc.printf(" mm, hAcc = %u", ULONG(20)); 00188 pc.printf(" mm"); 00189 break; 00190 case STATUS_MSG: // NAV-STATUS 00191 pc.printf("STATUS: gpsFix = %d", data[4]); 00192 if (data[5] & 2) { 00193 pc.printf(", dgpsFix"); 00194 } 00195 break; 00196 case DOP_MSG: // NAV-DOP 00197 pc.printf("DOP: gDOP = %.1f", ((float) UINT(4))/100.0); 00198 pc.printf(", tDOP = %.1f", ((float) UINT(8))/100.0); 00199 pc.printf(", vDOP = %.1f", ((float) UINT(10))/100.0); 00200 pc.printf(", hDOP = %.1f", ((float) UINT(12))/100.0); 00201 break; 00202 case SOL_MSG: // NAV-SOL 00203 pc.printf("SOL: week = %u", UINT(8)); 00204 pc.printf(", gpsFix = ", data[10]); 00205 pc.printf(", pDOP = %.1f", ((float) UINT(44))/ 100.0); 00206 pc.printf(", pAcc = %u", ULONG(24)); 00207 pc.printf(" cm, numSV = %d", data[47]); 00208 break; 00209 case VELNED_MSG: // NAV-VELNED 00210 pc.printf("VELNED: gSpeed = %u", ULONG(20)); 00211 pc.printf(" cm/sec, sAcc = %u", ULONG(28)); 00212 pc.printf(" cm/sec, heading = %.2f", ((float) LONG(24))/100000.0); 00213 pc.printf(" deg, cAcc = %.2f", ((float) LONG(32))/100000.0); 00214 pc.printf(" deg"); 00215 break; 00216 case SVINFO_MSG: { // NAV-SVINFO 00217 unsigned int nc = data[4]; // number of channels 00218 pc.printf("SVINFO: channels = %u\n", nc); 00219 for (int ch = 0; ch < nc; ch++) { 00220 pc.printf(" id = %3u (%2u) ", data[9+12*ch], data[12+12*ch]); 00221 for (int q=0; q < data[12+12*ch]/5; q++) 00222 pc.printf("*"); 00223 pc.printf("\n"); 00224 } 00225 break; 00226 } 00227 case DGPS_MSG: // NAV-DGPS 00228 pc.printf("DGPS: age = %d", LONG(4)); 00229 pc.printf(", baseId = %d", INT(8)); 00230 pc.printf(", numCh = %d", INT(12)); 00231 break; 00232 case SBAS_MSG: // NAV-SBAS 00233 pc.printf("SBAS: geo = "); 00234 switch (data[4]) { 00235 case 133: 00236 pc.printf("Inmarsat 4F3"); 00237 break; 00238 case 135: 00239 pc.printf("Galaxy 15"); 00240 break; 00241 case 138: 00242 pc.printf("Anik F1R"); 00243 break; 00244 default: 00245 pc.printf("%d", data[4]); 00246 break; 00247 } 00248 pc.printf(", mode = "); 00249 switch (data[5]) { 00250 case 0: 00251 pc.printf("disabled"); 00252 break; 00253 case 1: 00254 pc.printf("enabled integrity"); 00255 break; 00256 case 2: 00257 pc.printf("enabled test mode"); 00258 break; 00259 default: 00260 pc.printf("%d", data[5]); 00261 } 00262 pc.printf(", sys = "); 00263 switch (data[6]) { 00264 case 0: 00265 pc.printf("WAAS"); 00266 break; 00267 case 1: 00268 pc.printf("EGNOS"); 00269 break; 00270 case 2: 00271 pc.printf("MSAS"); 00272 break; 00273 case 16: 00274 pc.printf("GPS"); 00275 break; 00276 default: 00277 pc.printf("%d", data[6]); 00278 } 00279 break; 00280 default: 00281 printHex(id); 00282 } 00283 pc.printf("\n"); 00284 break; 00285 case 0x05: // ACK- 00286 pc.printf("ACK-"); 00287 switch (id) { 00288 case 0x00: // ACK-NAK 00289 pc.printf("NAK: "); 00290 break; 00291 case 0x01: // ACK-ACK 00292 pc.printf("ACK: "); 00293 break; 00294 } 00295 printHex(data[0]); 00296 pc.printf(" "); 00297 printHex(data[1]); 00298 pc.printf("\n"); 00299 break; 00300 } 00301 } 00302 state = 0; 00303 break; 00304 } 00305 default: 00306 break; 00307 } 00308 } 00309 } 00310 } 00311 00312 // Convert 1e-7 value packed into long into decimal format 00313 void printLatLon (long val) 00314 { 00315 int d = 10000000; 00316 pc.printf("%d.%d", val/d, abs(val%d)); 00317 } 00318 00319 void printHex (unsigned char val) 00320 { 00321 pc.printf("%02x", val); 00322 } 00323 00324 void sendCmd (unsigned char len, uint8_t data[]) 00325 { 00326 unsigned char chk1 = 0, chk2 = 0; 00327 00328 gps.putc(SYNC1); 00329 gps.putc(SYNC2); 00330 for (unsigned char ii = 0; ii < len; ii++) { 00331 gps.putc(data[ii]); 00332 chk1 += data[ii]; 00333 chk2 += chk1; 00334 } 00335 gps.putc(chk1); 00336 gps.putc(chk2); 00337 }
Generated on Sun Jul 17 2022 08:18:00 by
1.7.2