UBX_NAV_PVT and UBX_NAV_PVT parsing

Q&A forumCategory: QuestionsUBX_NAV_PVT and UBX_NAV_PVT parsing
enn.ellandi asked 3 months ago

I put together this code, it parses NAV_POLLSHH AND NAV_STATUS but not NAV_PVT and NAV_RELPOSNED Can\\’t figure out whars wrong – can anybody tell me? See code in Comment

enn.ellandi replied 3 months ago

const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 };
const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 };
const unsigned char NAV_PVT_HEADER[] = { 0x01, 0x07 };
const unsigned char NAV_RELPOSNED_HEADER[] = { 0x01, 0x3c };

const unsigned char* msgHeader;

enum _ubxMsgType {
MT_NONE,
MT_NAV_POSLLH,
MT_NAV_STATUS,
MT_NAV_PVT,
MT_NAV_RELPOSNED
};

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;
};
struct NAV_PVT {
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;
};
struct NAV_RELPOSNED {
unsigned char cls;
unsigned char id;
unsigned short len;
unsigned long iTOW;
};

union UBXMessage {
NAV_POSLLH navPosllh;
NAV_STATUS navStatus;
NAV_PVT navPvt;
NAV_RELPOSNED navRelposned;
};

UBXMessage ubxMessage;

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];
}
}
boolean compareMsgHeader(const unsigned char* msgHeader) {
unsigned char* ptr = (unsigned char*)(&ubxMessage);
return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1];
}
int processGPS() {
static int fpos = 0;
static unsigned char checksum[2];
static byte currentMsgType = MT_NONE;
static int payloadSize = sizeof(UBXMessage);

while ( Serial1.available() ) {
byte c = Serial1.read();
// Serial.write(c);

if ( fpos < 2 ) {
if ( c == UBX_HEADER[fpos] )
fpos++;
else
fpos = 0;
}
else {
if ( (fpos-2) < payloadSize )
((unsigned char*)(&ubxMessage))[fpos-2] = c;

fpos++;

if ( fpos == 4 ) {
if ( compareMsgHeader(NAV_POSLLH_HEADER) ) {
currentMsgType = MT_NAV_POSLLH;
payloadSize = sizeof(NAV_POSLLH);
/*
Serial.print("<— POSLLH payloadSize: ");
Serial.print(payloadSize);
Serial.print(" currentMsgType: ");
Serial.print(currentMsgType);
Serial.print(" in fpos ");
Serial.println(fpos);
Serial.println("—————-");
*/
}
}
if ( fpos == 4 ) {
if (compareMsgHeader(NAV_STATUS_HEADER)) {
currentMsgType = MT_NAV_STATUS;
payloadSize = sizeof(NAV_STATUS);
/*
Serial.print("<— STATUS payloadSize: ");
Serial.print(payloadSize);
Serial.print(" currentMsgType: ");
Serial.print(currentMsgType);
Serial.print(" in fpos ");
Serial.println(fpos);
Serial.println("—————-");
*/
}
}
if ( fpos == 4 ) {Serial.println(fpos);
if ( compareMsgHeader(NAV_PVT_HEADER) ) {
currentMsgType = MT_NAV_PVT;
payloadSize = sizeof(NAV_PVT);
/*
Serial.print("<— PVT payloadSize: ");
Serial.print(payloadSize);
Serial.print(" currentMsgType: ");
Serial.print(currentMsgType);
Serial.print(" in fpos ");
Serial.println(fpos);
Serial.println("—————-");
*/
}
}
if ( fpos == 8 ) {
if (compareMsgHeader(NAV_RELPOSNED_HEADER)) {
currentMsgType = MT_NAV_RELPOSNED;
payloadSize = sizeof(NAV_RELPOSNED);
/* Serial.print(" (payloadSize+4) ) {
fpos = 0;
}
}
}
return MT_NONE;
}
void setup()
{
Serial.begin(115200);
Serial1.begin(115200);
}

long lat;
long lon;

void loop() {
int msgType = processGPS();
if ( msgType == MT_NAV_POSLLH ) {
Serial.println(“POSLLH”);
Serial.print(“iTOW:”); Serial.println(ubxMessage.navPosllh.iTOW);
Serial.print(” lat/lon: “); Serial.print(ubxMessage.navPosllh.lat/1.0f); Serial.print(“,”); Serial.println(ubxMessage.navPosllh.lon/1.0f);
Serial.print(” hAcc: “); Serial.println(ubxMessage.navPosllh.hAcc/1000.0f);
Serial.println(“————“);
}
else if ( msgType == MT_NAV_STATUS ) {
Serial.println(“STATUS”);
Serial.print(“gpsFix:”); Serial.println(ubxMessage.navStatus.gpsFix);
Serial.print(“flags:”); Serial.println(ubxMessage.navStatus.flags2);
Serial.print(“fixStat:”); Serial.println(ubxMessage.navStatus.fixStat);
Serial.print(“ttff:”); Serial.println(ubxMessage.navStatus.ttff);
Serial.println(“============”);
}
else if ( msgType == MT_NAV_PVT ) {
Serial.println(“======PVT======”);
Serial.print(“id:”); Serial.println(ubxMessage.navPvt.id);
Serial.print(“iTOW:”); Serial.println(ubxMessage.navPvt.iTOW);
Serial.println(“======/PVT======”);
}
else if ( msgType == MT_NAV_RELPOSNED ) {
Serial.println(“======PVT======”);
Serial.print(“id:”); Serial.println(ubxMessage.navRelposned.id);
Serial.print(“iTOW:”); Serial.println(ubxMessage.navPvt.iTOW);
Serial.println(“======/PVT======”);
}
}