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)
main.cpp
- Committer:
- shimniok
- Date:
- 2013-05-17
- Revision:
- 3:4e131dfd860a
- Parent:
- 2:0ebd24621d18
File content as of revision 3:4e131dfd860a:
/**
* 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, int rate=1)
{
if (!enable) rate = 0;
uint8_t cmdBuf[] = {
0x06, // class CFG
0x01, // id MSG -> CFG-MSG
8, // length, for config message rates
0x01, // class,
id, // id,
0x0, // target 0 rate (DDC/I2C)
rate, // target 1 rate (UART1)
0x0, // target 2 rate (UART2)
0x0, // target 3 rate (USB)
0x0, // target 4 rate (SPI)
0x0, // target 5 rate (reserved)
};
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, 2); // 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 = %3u (%2u) ", data[9+12*ch], data[12+12*ch]);
for (int q=0; q < data[12+12*ch]/5; 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;
}
default:
break;
}
}
}
}
// Convert 1e-7 value packed into long into decimal format
void printLatLon (long val)
{
int d = 10000000;
pc.printf("%d.%d", val/d, abs(val%d));
}
void printHex (unsigned char val)
{
pc.printf("%02x", val);
}
void sendCmd (unsigned char len, uint8_t data[])
{
unsigned char chk1 = 0, chk2 = 0;
gps.putc(SYNC1);
gps.putc(SYNC2);
for (unsigned char ii = 0; ii < len; ii++) {
gps.putc(data[ii]);
chk1 += data[ii];
chk2 += chk1;
}
gps.putc(chk1);
gps.putc(chk2);
}