Skip to content
This repository was archived by the owner on Jan 28, 2021. It is now read-only.

Commit fd76204

Browse files
PaulZCPaulZC
authored
Merge pull request #159 from balamuruganky/release_candidate
NAV-PVT : Automotive dead reckoning related parameters, Speed acc estimate and heading acc estimation parametes parsed.
2 parents 25c9918 + 347dc08 commit fd76204

File tree

4 files changed

+153
-10
lines changed

4 files changed

+153
-10
lines changed

‎examples/Example13_PVT/Example1_AutoPVT/Example1_AutoPVT.ino‎

Lines changed: 29 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,8 @@ void setup()
5454
void loop()
5555
{
5656
// Calling getPVT returns true if there actually is a fresh navigation solution available.
57-
if (myGPS.getPVT())
57+
// Start the reading only when valid LLH is available
58+
if (myGPS.getPVT() && (myGPS.getInvalidLlh() == false))
5859
{
5960
Serial.println();
6061
long latitude = myGPS.getLatitude();
@@ -105,6 +106,33 @@ void loop()
105106
Serial.print(horizontalAccEst);
106107
Serial.print(F(" (mm)"));
107108

109+
int speedAccEst = myGPS.getSpeedAccEst();
110+
Serial.print(F(" SpeedAccEst: "));
111+
Serial.print(speedAccEst);
112+
Serial.print(F(" (mm/s)"));
113+
114+
int headAccEst = myGPS.getHeadingAccEst();
115+
Serial.print(F(" HeadAccEst: "));
116+
Serial.print(headAccEst);
117+
Serial.print(F(" (degrees * 10^-5)"));
118+
119+
if (myGPS.getHeadVehValid() == true) {
120+
int headVeh = myGPS.getHeadVeh();
121+
Serial.print(F(" HeadVeh: "));
122+
Serial.print(headVeh);
123+
Serial.print(F(" (degrees * 10^-5)"));
124+
125+
int magDec = myGPS.getMagDec();
126+
Serial.print(F(" MagDec: "));
127+
Serial.print(magDec);
128+
Serial.print(F(" (degrees * 10^-2)"));
129+
130+
int magAcc = myGPS.getMagAcc();
131+
Serial.print(F(" MagAcc: "));
132+
Serial.print(magAcc);
133+
Serial.print(F(" (degrees * 10^-2)"));
134+
}
135+
108136
Serial.println();
109137
} else {
110138
Serial.print(".");

‎keywords.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,13 @@ getVerticalAccEst KEYWORD2
5858
getNedNorthVel KEYWORD2
5959
getNedEastVel KEYWORD2
6060
getNedDownVel KEYWORD2
61+
getSpeedAccEst KEYWORD2
62+
getHeadingAccEst KEYWORD2
63+
getInvalidLlh KEYWORD2
64+
getHeadVeh KEYWORD2
65+
getMagDec KEYWORD2
66+
getMagAcc KEYWORD2
67+
getHeadVehValid KEYWORD2
6168

6269
setPortOutput KEYWORD2
6370
setPortInput KEYWORD2

‎src/SparkFun_Ublox_Arduino_Library.cpp‎

Lines changed: 95 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1027,8 +1027,9 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
10271027

10281028
fixType = extractByte(20 - startingSpot);
10291029
gnssFixOk = extractByte(21 - startingSpot) & 0x1; //Get the 1st bit
1030-
diffSoln = extractByte(21 - startingSpot) >> 1 & 0x1; //Get the 2nd bit
1030+
diffSoln = (extractByte(21 - startingSpot) >> 1) & 0x1; //Get the 2nd bit
10311031
carrierSolution = extractByte(21 - startingSpot) >> 6; //Get 6th&7th bits of this byte
1032+
headVehValid = (extractByte(21 - startingSpot) >> 5) & 0x1; // Get the 5th bit
10321033
SIV = extractByte(23 - startingSpot);
10331034
longitude = extractSignedLong(24 - startingSpot);
10341035
latitude = extractSignedLong(28 - startingSpot);
@@ -1039,10 +1040,15 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
10391040
nedNorthVel = extractSignedLong(48 - startingSpot);
10401041
nedEastVel = extractSignedLong(52 - startingSpot);
10411042
nedDownVel = extractSignedLong(56 - startingSpot);
1042-
10431043
groundSpeed = extractSignedLong(60 - startingSpot);
10441044
headingOfMotion = extractSignedLong(64 - startingSpot);
1045+
speedAccEst = extractLong(68 - startingSpot);
1046+
headingAccEst = extractLong(72 - startingSpot);
10451047
pDOP = extractInt(76 - startingSpot);
1048+
invalidLlh = extractByte(78 - startingSpot) & 0x1;
1049+
headVeh = extractSignedLong(84 - startingSpot);
1050+
magDec = extractSignedInt(88 - startingSpot);
1051+
magAcc = extractInt(90 - startingSpot);
10461052

10471053
//Mark all datums as fresh (not read before)
10481054
moduleQueried.gpsiTOW = true;
@@ -1059,23 +1065,28 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
10591065
moduleQueried.all = true;
10601066
moduleQueried.gnssFixOk = true;
10611067
moduleQueried.diffSoln = true;
1068+
moduleQueried.headVehValid = true;
10621069
moduleQueried.longitude = true;
10631070
moduleQueried.latitude = true;
10641071
moduleQueried.altitude = true;
10651072
moduleQueried.altitudeMSL = true;
1066-
10671073
moduleQueried.horizontalAccEst = true;
10681074
moduleQueried.verticalAccEst = true;
10691075
moduleQueried.nedNorthVel = true;
10701076
moduleQueried.nedEastVel = true;
10711077
moduleQueried.nedDownVel = true;
1072-
10731078
moduleQueried.SIV = true;
10741079
moduleQueried.fixType = true;
10751080
moduleQueried.carrierSolution = true;
10761081
moduleQueried.groundSpeed = true;
10771082
moduleQueried.headingOfMotion = true;
1083+
moduleQueried.speedAccEst = true;
1084+
moduleQueried.headingAccEst = true;
10781085
moduleQueried.pDOP = true;
1086+
moduleQueried.invalidLlh = true;
1087+
moduleQueried.headVeh = true;
1088+
moduleQueried.magDec = true;
1089+
moduleQueried.magAcc = true;
10791090
}
10801091
else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36)
10811092
{
@@ -3111,6 +3122,19 @@ uint16_t SFE_UBLOX_GPS::extractInt(uint8_t spotToStart)
31113122
return (val);
31123123
}
31133124

3125+
//Just so there is no ambiguity about whether a uint16_t will cast to a int16_t correctly...
3126+
int16_t SFE_UBLOX_GPS::extractSignedInt(int8_t spotToStart)
3127+
{
3128+
union // Use a union to convert from uint16_t to int16_t
3129+
{
3130+
uint16_t unsignedInt;
3131+
int16_t signedInt;
3132+
} stSignedInt;
3133+
3134+
stSignedInt.unsignedInt = extractInt(spotToStart);
3135+
return (stSignedInt.signedInt);
3136+
}
3137+
31143138
//Given a spot, extract a byte from the payload
31153139
uint8_t SFE_UBLOX_GPS::extractByte(uint8_t spotToStart)
31163140
{
@@ -3195,6 +3219,54 @@ bool SFE_UBLOX_GPS::getTimeValid(uint16_t maxWait)
31953219
return (gpsTimeValid);
31963220
}
31973221

3222+
uint32_t SFE_UBLOX_GPS::getSpeedAccEst(uint16_t maxWait)
3223+
{
3224+
if (moduleQueried.speedAccEst == false)
3225+
getPVT(maxWait);
3226+
moduleQueried.speedAccEst = false; //Since we are about to give this to user, mark this data as stale
3227+
return (speedAccEst);
3228+
}
3229+
3230+
uint32_t SFE_UBLOX_GPS::getHeadingAccEst(uint16_t maxWait)
3231+
{
3232+
if (moduleQueried.headingAccEst == false)
3233+
getPVT(maxWait);
3234+
moduleQueried.headingAccEst = false; //Since we are about to give this to user, mark this data as stale
3235+
return (headingAccEst);
3236+
}
3237+
3238+
bool SFE_UBLOX_GPS::getInvalidLlh(uint16_t maxWait)
3239+
{
3240+
if (moduleQueried.invalidLlh == false)
3241+
getPVT(maxWait);
3242+
moduleQueried.invalidLlh = false; //Since we are about to give this to user, mark this data as stale
3243+
return (invalidLlh);
3244+
}
3245+
3246+
int32_t SFE_UBLOX_GPS::getHeadVeh(uint16_t maxWait)
3247+
{
3248+
if (moduleQueried.headVeh == false)
3249+
getPVT(maxWait);
3250+
moduleQueried.headVeh = false; //Since we are about to give this to user, mark this data as stale
3251+
return (headVeh);
3252+
}
3253+
3254+
int16_t SFE_UBLOX_GPS::getMagDec(uint16_t maxWait)
3255+
{
3256+
if (moduleQueried.magDec == false)
3257+
getPVT(maxWait);
3258+
moduleQueried.magDec = false; //Since we are about to give this to user, mark this data as stale
3259+
return (magDec);
3260+
}
3261+
3262+
uint16_t SFE_UBLOX_GPS::getMagAcc(uint16_t maxWait)
3263+
{
3264+
if (moduleQueried.magAcc == false)
3265+
getPVT(maxWait);
3266+
moduleQueried.magAcc = false; //Since we are about to give this to user, mark this data as stale
3267+
return (magAcc);
3268+
}
3269+
31983270
//Get the current millisecond
31993271
uint16_t SFE_UBLOX_GPS::getMillisecond(uint16_t maxWait)
32003272
{
@@ -3780,6 +3852,18 @@ uint8_t SFE_UBLOX_GPS::getCarrierSolutionType(uint16_t maxWait)
37803852
return (carrierSolution);
37813853
}
37823854

3855+
//Get whether head vehicle valid or not
3856+
bool SFE_UBLOX_GPS::getHeadVehValid(uint16_t maxWait)
3857+
{
3858+
if (moduleQueried.headVehValid == false)
3859+
getPVT(maxWait);
3860+
moduleQueried.headVehValid = false; //Since we are about to give this to user, mark this data as stale
3861+
moduleQueried.all = false;
3862+
3863+
return (headVehValid);
3864+
}
3865+
3866+
37833867
//Get the ground speed in mm/s
37843868
int32_t SFE_UBLOX_GPS::getGroundSpeed(uint16_t maxWait)
37853869
{
@@ -3901,6 +3985,7 @@ void SFE_UBLOX_GPS::flushPVT()
39013985
moduleQueried.all = false;
39023986
moduleQueried.gnssFixOk = false;
39033987
moduleQueried.diffSoln = false;
3988+
moduleQueried.headVehValid = false;
39043989
moduleQueried.longitude = false;
39053990
moduleQueried.latitude = false;
39063991
moduleQueried.altitude = false;
@@ -3910,7 +3995,13 @@ void SFE_UBLOX_GPS::flushPVT()
39103995
moduleQueried.carrierSolution = false;
39113996
moduleQueried.groundSpeed = false;
39123997
moduleQueried.headingOfMotion = false;
3998+
moduleQueried.speedAccEst = false;
3999+
moduleQueried.headingAccEst = false;
39134000
moduleQueried.pDOP = false;
4001+
moduleQueried.invalidLlh = false;
4002+
moduleQueried.headVeh = false;
4003+
moduleQueried.magDec = false;
4004+
moduleQueried.magAcc = false;
39144005
}
39154006

39164007
//Mark all the HPPOSLLH data as read/stale. This is handy to get data alignment after CRC failure

‎src/SparkFun_Ublox_Arduino_Library.h‎

Lines changed: 22 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -507,17 +507,16 @@ class SFE_UBLOX_GPS
507507

508508
bool getGnssFixOk(uint16_t maxWait = getPVTmaxWait); //Get whether we have a valid fix (i.e within DOP & accuracy masks)
509509
bool getDiffSoln(uint16_t maxWait = getPVTmaxWait); //Get whether differential corrections were applied
510+
bool getHeadVehValid(uint16_t maxWait = getPVTmaxWait);
510511
int32_t getLatitude(uint16_t maxWait = getPVTmaxWait); //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module.
511512
int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module.
512513
int32_t getAltitude(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above ellipsoid
513514
int32_t getAltitudeMSL(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above mean sea level
514-
515515
int32_t getHorizontalAccEst(uint16_t maxWait = getPVTmaxWait);
516516
int32_t getVerticalAccEst(uint16_t maxWait = getPVTmaxWait);
517517
int32_t getNedNorthVel(uint16_t maxWait = getPVTmaxWait);
518518
int32_t getNedEastVel(uint16_t maxWait = getPVTmaxWait);
519519
int32_t getNedDownVel(uint16_t maxWait = getPVTmaxWait);
520-
521520
uint8_t getSIV(uint16_t maxWait = getPVTmaxWait); //Returns number of sats used in fix
522521
uint8_t getFixType(uint16_t maxWait = getPVTmaxWait); //Returns the type of fix: 0=no, 3=3D, 4=GNSS+Deadreckoning
523522
uint8_t getCarrierSolutionType(uint16_t maxWait = getPVTmaxWait); //Returns RTK solution: 0=no, 1=float solution, 2=fixed solution
@@ -535,6 +534,12 @@ class SFE_UBLOX_GPS
535534
uint32_t getTimeOfWeek(uint16_t maxWait = getPVTmaxWait);
536535
bool getDateValid(uint16_t maxWait = getPVTmaxWait);
537536
bool getTimeValid(uint16_t maxWait = getPVTmaxWait);
537+
uint32_t getSpeedAccEst(uint16_t maxWait = getPVTmaxWait);
538+
uint32_t getHeadingAccEst(uint16_t maxWait = getPVTmaxWait);
539+
bool getInvalidLlh(uint16_t maxWait = getPVTmaxWait);
540+
int32_t getHeadVeh(uint16_t maxWait = getPVTmaxWait);
541+
int16_t getMagDec(uint16_t maxWait = getPVTmaxWait);
542+
uint16_t getMagAcc(uint16_t maxWait = getPVTmaxWait);
538543

539544
int32_t getHighResLatitude(uint16_t maxWait = getHPPOSLLHmaxWait);
540545
int8_t getHighResLatitudeHp(uint16_t maxWait = getHPPOSLLHmaxWait);
@@ -730,9 +735,9 @@ class SFE_UBLOX_GPS
730735
bool gpsDateValid;
731736
bool gpsTimeValid;
732737

733-
734738
bool gnssFixOk; //valid fix (i.e within DOP & accuracy masks)
735739
bool diffSoln; //Differential corrections were applied
740+
bool headVehValid;
736741
int32_t latitude; //Degrees * 10^-7 (more accurate than floats)
737742
int32_t longitude; //Degrees * 10^-7 (more accurate than floats)
738743
int32_t altitude; //Number of mm above ellipsoid
@@ -747,7 +752,13 @@ class SFE_UBLOX_GPS
747752
uint8_t carrierSolution; //Tells us when we have an RTK float/fixed solution
748753
int32_t groundSpeed; //mm/s
749754
int32_t headingOfMotion; //degrees * 10^-5
755+
uint32_t speedAccEst;
756+
uint32_t headingAccEst;
750757
uint16_t pDOP; //Positional dilution of precision * 10^-2 (dimensionless)
758+
bool invalidLlh;
759+
int32_t headVeh;
760+
int16_t magDec;
761+
uint16_t magAcc;
751762
uint8_t versionLow; //Loaded from getProtocolVersion().
752763
uint8_t versionHigh;
753764

@@ -943,6 +954,7 @@ class SFE_UBLOX_GPS
943954
uint32_t extractLong(uint8_t spotToStart); //Combine four bytes from payload into long
944955
int32_t extractSignedLong(uint8_t spotToStart); //Combine four bytes from payload into signed long (avoiding any ambiguity caused by casting)
945956
uint16_t extractInt(uint8_t spotToStart); //Combine two bytes from payload into int
957+
int16_t extractSignedInt(int8_t spotToStart);
946958
uint8_t extractByte(uint8_t spotToStart); //Get byte from payload
947959
int8_t extractSignedChar(uint8_t spotToStart); //Get signed 8-bit value from payload
948960
void addToChecksum(uint8_t incoming); //Given an incoming byte, adjust rollingChecksumA/B
@@ -1022,23 +1034,28 @@ class SFE_UBLOX_GPS
10221034
uint32_t all : 1;
10231035
uint32_t gnssFixOk : 1;
10241036
uint32_t diffSoln : 1;
1037+
uint32_t headVehValid : 1;
10251038
uint32_t longitude : 1;
10261039
uint32_t latitude : 1;
10271040
uint32_t altitude : 1;
10281041
uint32_t altitudeMSL : 1;
1029-
10301042
uint32_t horizontalAccEst : 1;
10311043
uint32_t verticalAccEst : 1;
10321044
uint32_t nedNorthVel : 1;
10331045
uint32_t nedEastVel : 1;
10341046
uint32_t nedDownVel : 1;
1035-
10361047
uint32_t SIV : 1;
10371048
uint32_t fixType : 1;
10381049
uint32_t carrierSolution : 1;
10391050
uint32_t groundSpeed : 1;
10401051
uint32_t headingOfMotion : 1;
1052+
uint32_t speedAccEst : 1;
1053+
uint32_t headingAccEst : 1;
10411054
uint32_t pDOP : 1;
1055+
uint32_t invalidLlh : 1;
1056+
uint32_t headVeh : 1;
1057+
uint32_t magDec : 1;
1058+
uint32_t magAcc : 1;
10421059
uint32_t versionNumber : 1;
10431060
} moduleQueried;
10441061

0 commit comments

Comments
 (0)