UBX decode program that is modified from i2dforce UBX program for NAV-STATUS and NAV POSLLH. I used NEO-7M and mbed LPC1768.
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2020-12-18
- Revision:
- 0:a9095d98a2b9
- Child:
- 1:63a0fd33d5a5
File content as of revision 0:a9095d98a2b9:
#include "mbed.h" //NAV- DigitalOut myled(LED1); // serial port Serial pc(USBTX, USBRX); // tx, rx Serial ublox_ubx(p13,p14);//tx,rx const unsigned char UBX_HEADER[] = { 0xB5, 0x62 }; const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 }; const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 }; enum _ubxMsgType { MT_NONE, MT_NAV_POSLLH, MT_NAV_STATUS }; struct NAV_POSLLH { unsigned char cls; unsigned char id; unsigned short len; unsigned long iTOW; long lon; long lat; long height; long hMSL; unsigned long hAcc; unsigned long vAcc; }; struct NAV_STATUS { unsigned char cls; unsigned char id; unsigned short len; unsigned long iTOW; unsigned char gpsFix; char flags; char fixStat; char flags2; unsigned long ttff; unsigned long msss; }; union UBXMessage { NAV_POSLLH navPosllh; NAV_STATUS navStatus; }; UBXMessage ubxMessage; // The last two bytes of the message is a checksum value, used to confirm that the received payload is valid. // The procedure used to calculate this is given as pseudo-code in the uBlox manual. void calcChecksum(unsigned char* CK, int msgSize) { memset(CK, 0, 2); for (int i = 0; i < msgSize; i++) { CK[0] += ((unsigned char*)(&ubxMessage))[i]; CK[1] += CK[0]; } } // Compares the first two bytes of the ubxMessage struct with a specific message header. // Returns true if the two bytes match. bool compareMsgHeader(const unsigned char* msgHeader) { unsigned char* ptr = (unsigned char*)(&ubxMessage); return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1]; } // Reads in bytes from the GPS module and checks to see if a valid message has been constructed. // Returns the type of the message found if successful, or MT_NONE if no message was found. // After a successful return the contents of the ubxMessage union will be valid, for the // message type that was found. Note that further calls to this function can invalidate the // message content, so you must use the obtained values before calling this function again. int processGPS() { static int fpos = 0; static unsigned char checksum[2]; static unsigned char currentMsgType = MT_NONE; static int payloadSize = sizeof(UBXMessage); while ( ublox_ubx.readable() ) { unsigned char c = ublox_ubx.getc(); //Serial.write(c); if ( fpos < 2 ) { // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62) if ( c == UBX_HEADER[fpos] ) fpos++; else fpos = 0; // Reset to beginning state. } else { // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER // and we are now reading in the bytes that make up the payload. // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because // the struct does not include the initial two-byte header (UBX_HEADER). if ( (fpos-2) < payloadSize ) ((unsigned char*)(&ubxMessage))[fpos-2] = c; fpos++; if ( fpos == 4 ) { // We have just received the second byte of the message type header, // so now we can check to see what kind of message it is. if ( compareMsgHeader(NAV_POSLLH_HEADER) ) { currentMsgType = MT_NAV_POSLLH; payloadSize = sizeof(NAV_POSLLH); } else if ( compareMsgHeader(NAV_STATUS_HEADER) ) { currentMsgType = MT_NAV_STATUS; payloadSize = sizeof(NAV_STATUS); } else { // unknown message type, bail fpos = 0; continue; } } if ( fpos == (payloadSize+2) ) { // All payload bytes have now been received, so we can calculate the // expected checksum value to compare with the next two incoming bytes. calcChecksum(checksum, payloadSize); } else if ( fpos == (payloadSize+3) ) { // First byte after the payload, ie. first byte of the checksum. // Does it match the first byte of the checksum we calculated? if ( c != checksum[0] ) { // Checksum doesn't match, reset to beginning state and try again. fpos = 0; } } else if ( fpos == (payloadSize+4) ) { // Second byte after the payload, ie. second byte of the checksum. // Does it match the second byte of the checksum we calculated? fpos = 0; // We will reset the state regardless of whether the checksum matches. if ( c == checksum[1] ) { // Checksum matches, we have a valid message. return currentMsgType; } } else if ( fpos > (payloadSize+4) ) { // We have now read more bytes than both the expected payload and checksum // together, so something went wrong. Reset to beginning state and try again. fpos = 0; } } } return MT_NONE; } float latitude,longitude,height_float; int gps_Fix; /*--------------------------------------------*/ int main() { //UART initialization pc.baud(460800); //115.2 kbps ublox_ubx.baud(460800); //115.2 kbps while(1) { int msgType = processGPS(); if ( msgType == MT_NAV_POSLLH ) { latitude=ubxMessage.navPosllh.lat/10000000.0f; longitude=ubxMessage.navPosllh.lon/10000000.0f; height_float=float(ubxMessage.navPosllh.height); pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float); } else if ( msgType == MT_NAV_STATUS ) { gps_Fix=int(ubxMessage.navStatus.gpsFix); pc.printf("gps_Fix=%d\r\n",gps_Fix); } } }