Arduino message parser code

Q&A forumCategory: QuestionsArduino message parser code
Lóránt Szabó asked 4 months 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 4 months 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 4 months 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 4 months 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 4 months 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 3 months 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