Arduino message parser code

Q&A forumCategory: QuestionsArduino message parser code
Lóránt Szabó asked 2 years ago
Dear Community!
 
With the code I attached try to parse PVT messages on NODEMCU.
It is very stange because only SV,FixType,Date,Lat/Lon messages arrives well, orthers seems to be failed, like Heading : 1200 gSpeed: 870…
Do anybody have idea what is wrong with the code or have an operating one?
Regards,
Lóránt
CODE:
#include <SoftwareSerial.h>
#include <TinyGPS++.h> // library for GPS module

// Connect the GPS RX/TX to arduino pins 3 and 5
SoftwareSerial ss(D6, D5);
TinyGPSPlus gps;  // The TinyGPS++ object
const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
struct NAV_PVT {
  unsigned char cls;
  unsigned char id;
  unsigned short len;
  unsigned long iTOW;          // GPS time of week of the navigation epoch (ms)
  unsigned short year;         // Year (UTC)
  unsigned char month;         // Month, range 1..12 (UTC)
  unsigned char day;           // Day of month, range 1..31 (UTC)
  unsigned char hour;          // Hour of day, range 0..23 (UTC)
  unsigned char minute;        // Minute of hour, range 0..59 (UTC)
  unsigned char second;        // Seconds of minute, range 0..60 (UTC)
  char valid;                  // Validity Flags (see graphic below)
  unsigned long tAcc;          // Time accuracy estimate (UTC) (ns)
  long nano;                   // Fraction of second, range -1e9 .. 1e9 (UTC) (ns)
  unsigned char fixType;       // GNSSfix Type, range 0..5
  char flags;                  // Fix Status Flags
  char flags2;
  //unsigned char reserved1;     // reserved
  unsigned char numSV;         // Number of satellites used in Nav Solution
  long lon;                    // Longitude (deg)
  long lat;                    // Latitude (deg)
  long height;                 // Height above Ellipsoid (mm)
  long hMSL;                   // Height above mean sea level (mm)
  unsigned long hAcc;          // Horizontal Accuracy Estimate (mm)
  unsigned long vAcc;          // Vertical Accuracy Estimate (mm)
  long velN;                   // NED north velocity (mm/s)
  long velE;                   // NED east velocity (mm/s)
  long velD;                   // NED down velocity (mm/s)
  long gSpeed;                 // Ground Speed (2-D) (mm/s)
  long heading;                // Heading of motion 2-D (deg)
  unsigned long sAcc;          // Speed Accuracy Estimate
  unsigned long headingAcc;    // Heading Accuracy Estimate
  unsigned short pDOP;         // Position dilution of precision
  short reserved2;             // Reserved
  unsigned long reserved3;     // Reserved
};
NAV_PVT pvt;
void calcChecksum(unsigned char* CK) {
  memset(CK, 0, 2);
  for (int i = 0; i < (int)sizeof(NAV_PVT); i++) {
    CK[0] += ((unsigned char*)(&pvt))[i];
    CK[1] += CK[0];
  }
}
bool processGPS() {
 
  static int fpos = 0;
  static unsigned char checksum[2];
  const int payloadSize = sizeof(NAV_PVT);
  while ( ss.available() ) {
    byte c = ss.read();
    if ( fpos < 2 ) {
      if ( c == UBX_HEADER[fpos] ) {
        fpos++;
      }
      else {
        fpos = 0;
      }
    }
    else {
      if ( (fpos – 2) < payloadSize )
        ((unsigned char*)(&pvt))[fpos – 2] = c;
      fpos++;
      if ( fpos == (payloadSize + 2) ) {
        calcChecksum(checksum);
      }
      else if ( fpos == (payloadSize + 3) ) {
        if ( c != checksum[0] )
          fpos = 0;
      }
      else if ( fpos == (payloadSize + 4) ) {
        fpos = 0;
        if ( c == checksum[1] ) {
          return true;
        }
      }
      else if ( fpos > (payloadSize + 4) ) {
        fpos = 0;
      }
    }
  }
  return true;
}
void setup()
{
  Serial.begin(115200);
  ss.begin(115200);
}
void loop() {
 
  while (ss.available() > 0) { //while data is available

    if ( processGPS() ) {
      Serial.print(“#SV: “);      Serial.print(pvt.numSV);
      Serial.print(” fixType: “); Serial.print(pvt.fixType);
      Serial.print(” Date:”);     Serial.print(pvt.year); Serial.print(“/”); Serial.print(pvt.month); Serial.print(“/”); Serial.print(pvt.day); Serial.print(” “); Serial.print(pvt.hour); Serial.print(“:”); Serial.print(pvt.minute); Serial.print(“:”); Serial.print(pvt.second);
      Serial.print(” lat/lon: “); Serial.print(pvt.lat / 10000000.0f); Serial.print(“,”); Serial.print(pvt.lon / 10000000.0f);
      Serial.print(” gSpeed: “);  Serial.print(pvt.gSpeed / 1000.0f);
      Serial.print(” heading: “); Serial.print(pvt.heading / 100000.0f);
      Serial.print(” hAcc: “);    Serial.print(pvt.hAcc / 1000.0f);
 
    }
    delay(100);
  }
}
clive1
replied 2 years ago

Show a dump of the packet content (raw bytes), it might be easier to determine if the data makes sense and decode it independently to see what values are actually reported.

3 Answers
lorant.szabo answered 2 years ago
Hello Clive!
Thanks for the help! Below you can see the received messages.
It is a heading unit. In theory only PVT messages coming. I received these messages on TX2 of simpleRTK2B.
Thanks in advance!

18198179202042242361622876106522155800031200210293123413131410611179220762881730252109207890011711006600011000210
18198179201922262361622876106522255800017398503123413914106111762207628124730391102071900111110012000227255255255190
1819817920242292361622876106522255800037168200353123413111410611178220762892730711020609001031100200036000320
1819817920112231236162287610652235580001573524111312341310141061117822076289073051102050900941100900033000400
181981792020023323616228761065223558000211051804731234132521310611171220762822773014111020419008711001992552552552372552552554018198179201882352361622876106522455800016332311730255
1819817920322362361622876106522455800014022822023312341324313106111672207628588302291102033900811100216255255255249255255255238255
18198179202023823616228761065224558000277317053312341324213106111672207628408302101102028900761100225255255255237255255255250255
18198179201082402361622876106522555800014819621029312341324613106111682207628247730161110202090068110023925525525521000120
181981792019624223616228761065226558000126425153123413240131061116522076283583020511020129006011002502552552556000120
181981792028245236162287610652265580001321331904131234132381310611163220762855830226110205900531100238255255255232255255255110
18198179201162472361622876106522755700025102311731234132291310611157220762813583049111202548004611001000025125525525530
18198179201042492361622876106522755700013810118047312341322713106111572207628130830441112024980041110014000254255255255150
181981792019225123616228761065228557000222522023312341321813106111522207628228830143111202468003811001000013000251255
181981792024254236162287610652295570001239250312341321913106111522207628235830149111202458003611002382552552555000255255
181981792011202371622876106522955700024316120035312341322313106111542207628166830811112024880038110024725525525518000260
1819817920200223716228761065230557000107292411131234132251310611154220762815283066111202528004111002500035000250
1819817920188423716228761065230558000250129190413123413230131061115722076289683010111201900441100800042000370
181981792020723716228761065231558000115253230173123413240131061116222076282457301601102089004811004800069000550
18198179201089237162287610652315580002356617053312341323213106111582207628678302371102010900471100300012000350
18198179201961123716228761065232558000100190210293123413229131061115622076281128302611120129004511001400010000120
1819817920281423716228761065233558000220572515312341322813106111572207628127830411112013900431100430004200000
181981792016162371622876106523355800010715820035312341323113106111592207628938307111201390041110025025525525524825525525570
18198179201041823716228761065234558000228252411131234132351310611160220762850830221110201490039110025225525525530000160
1819817920192202371622876106523455800092951804731234132351310611160220762844830215110201590037110024000242255255255110
18198179202423237162287610652355580002132182202331234132381310611162220762823873015311020169003511002700010000200
1819817920112252371622876106523655800077865031234132451310611166220762818973010311020179003211002600050000180
181981792010027237162287610652365580002201862102931234132401310611163220762821473012911020139002711002212552552555000140
1819817920188292371622876106523755800085542515312341324613106111652207628127730411102099002111002272552552553000280
1819817920203223716228761065237558000205123190413123413242131061116322076281707308411020690015110010001700080
18198179201083423716228761065238558000692472301731234132461310611165220762814973063110204900101100130003000030
18198179201963623716228761065238557000190601705331234132451310611165220762818373097110203900611006000253255255255241255
1819817920184382371622876106523955800077215220233123413251131061116722076281437305711020590051100400025000255255
18198179201641237162287610652405580001988250312341361410611172220762838730208109208900311003800036000270
1819817920104432371622876106524055800062152200353123413414106111692207628927306110201190031100100003700090
18198179201924523716228761065241558000183192411131234139141061117122076281973018910920159003110070001000310
1819817920244823716228761065241558000488918047312341320141061117522076282066301201092019900311002900061000250
18198179201250237162287610652425580001912432301731234133014106111812207628134630481092022900311005100099000340
18198179201005223716228761065242558000565717053312341343141061118722076282463019510820269003110075000108000580
181981792018854237162287610652435580001771802102931234134414106111892207628116301811082029900311006200033000500
18198179202057237162287610652445580004148251531234134414106111892207628696302391082033900311002400038000200
181981792010859237162287610652445580001621171904131234134714106111912207628416302111082035900311002300048000240
18198179209661237162287610652455580004916241113123413531410611193220762828630198108203290025410002400016000190
1819817920184632371622876106524555800016985180473123413531410611192220762896630101092028900249100024025525525540000251255
1819817920166623716228761065246558000342092202331234136414106111962207628106301801082025900244100024425525525537000200
18198179201046823716228761065247558000154765031234136514106111962207628176301871082023900240100023025525525523000170
clive1
replied 2 years ago

Yeah, not going to be able to deal with free form decimals with variable width, and no separation
Use a fixed format, hex, or separate. ie “%02X”, “%3d “, “%d,”, etc.

András Tóth answered 2 years ago
Üdv! Bocsi, olyan kérdésem lenne, hogy esetleg kérdezhetnék rtkval kapcsolatban, itt a fórumon kérdeztem, de egy ideig jött válasz és azóta semmi. Kérhetek egy elérhetőséget?

lorant.szabo answered 2 years ago
Szia!
Facebook-on megtalálsz biztos Szabó Lóránt név alatt, barátnőmmel vagyok a profilképen újatlan trikóban :))
Amúgy szabo1994lorant@gmail.com
 
Üdv
Lóri

Want to learn more about GPS/RTK?

1. Our engineering team will contact you to solve any questions
2. We will keep you updated about promotions and new product releases
3.You will only hear from us when we have important news, we won’t spam your email