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

NAV-PVT : Automotive dead reckoning related parameters, Speed acc estimate and heading acc estimation parametes parsed. #159

Merged
merged 4 commits into from
Dec 11, 2020
Merged

NAV-PVT : Automotive dead reckoning related parameters, Speed acc estimate and heading acc estimation parametes parsed. #159

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b7beb48
getSpeedAccEst, getHeadingAccEst, getInvalidLlh, getHeadVeh, getMagDe…
balamuruganky Dec 10, 2020
9479135
getHeadVehValid function added.
balamuruganky Dec 10, 2020
378007a
Example1_AutoPVT.ino file is updated for newly added functions.
balamuruganky Dec 10, 2020
347dc08
Update SparkFun_Ublox_Arduino_Library.cpp
balamuruganky Dec 11, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 29 additions & 1 deletion examples/Example13_PVT/Example1_AutoPVT/Example1_AutoPVT.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,8 @@ void setup()
void loop()
{
// Calling getPVT returns true if there actually is a fresh navigation solution available.
if (myGPS.getPVT())
// Start the reading only when valid LLH is available
if (myGPS.getPVT() && (myGPS.getInvalidLlh() == false))
{
Serial.println();
long latitude = myGPS.getLatitude();
Expand Down Expand Up @@ -105,6 +106,33 @@ void loop()
Serial.print(horizontalAccEst);
Serial.print(F(" (mm)"));

int speedAccEst = myGPS.getSpeedAccEst();
Serial.print(F(" SpeedAccEst: "));
Serial.print(speedAccEst);
Serial.print(F(" (mm/s)"));

int headAccEst = myGPS.getHeadingAccEst();
Serial.print(F(" HeadAccEst: "));
Serial.print(headAccEst);
Serial.print(F(" (degrees * 10^-5)"));

if (myGPS.getHeadVehValid() == true) {
int headVeh = myGPS.getHeadVeh();
Serial.print(F(" HeadVeh: "));
Serial.print(headVeh);
Serial.print(F(" (degrees * 10^-5)"));

int magDec = myGPS.getMagDec();
Serial.print(F(" MagDec: "));
Serial.print(magDec);
Serial.print(F(" (degrees * 10^-2)"));

int magAcc = myGPS.getMagAcc();
Serial.print(F(" MagAcc: "));
Serial.print(magAcc);
Serial.print(F(" (degrees * 10^-2)"));
}

Serial.println();
} else {
Serial.print(".");
Expand Down
7 changes: 7 additions & 0 deletions keywords.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,13 @@ getVerticalAccEst KEYWORD2
getNedNorthVel KEYWORD2
getNedEastVel KEYWORD2
getNedDownVel KEYWORD2
getSpeedAccEst KEYWORD2
getHeadingAccEst KEYWORD2
getInvalidLlh KEYWORD2
getHeadVeh KEYWORD2
getMagDec KEYWORD2
getMagAcc KEYWORD2
getHeadVehValid KEYWORD2

setPortOutput KEYWORD2
setPortInput KEYWORD2
Expand Down
97 changes: 94 additions & 3 deletions src/SparkFun_Ublox_Arduino_Library.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1029,6 +1029,7 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
gnssFixOk = extractByte(21 - startingSpot) & 0x1; //Get the 1st bit
diffSoln = extractByte(21 - startingSpot) >> 1 & 0x1; //Get the 2nd bit
carrierSolution = extractByte(21 - startingSpot) >> 6; //Get 6th&7th bits of this byte
headVehValid = extractByte(21 - startingSpot) >> 5 & 0x1; // Get the 5th bit
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's change this to:
headVehValid = (extractByte(21 - startingSpot) >> 5) & 0x1; // Get the 5th bit
just to make it clear that the shift happens before the AND

Copy link
Collaborator

@PaulZC PaulZC Dec 11, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, can you change line 1030 too please?
(I know the code works - these changes just help with the clarity)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure. I will update accordingly. Thanks.

SIV = extractByte(23 - startingSpot);
longitude = extractSignedLong(24 - startingSpot);
latitude = extractSignedLong(28 - startingSpot);
Expand All @@ -1039,10 +1040,15 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
nedNorthVel = extractSignedLong(48 - startingSpot);
nedEastVel = extractSignedLong(52 - startingSpot);
nedDownVel = extractSignedLong(56 - startingSpot);

groundSpeed = extractSignedLong(60 - startingSpot);
headingOfMotion = extractSignedLong(64 - startingSpot);
speedAccEst = extractLong(68 - startingSpot);
headingAccEst = extractLong(72 - startingSpot);
pDOP = extractInt(76 - startingSpot);
invalidLlh = extractByte(78 - startingSpot) & 0x1;
headVeh = extractSignedLong(84 - startingSpot);
magDec = extractSignedInt(88 - startingSpot);
magAcc = extractInt(90 - startingSpot);

//Mark all datums as fresh (not read before)
moduleQueried.gpsiTOW = true;
Expand All @@ -1059,23 +1065,28 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
moduleQueried.all = true;
moduleQueried.gnssFixOk = true;
moduleQueried.diffSoln = true;
moduleQueried.headVehValid = true;
moduleQueried.longitude = true;
moduleQueried.latitude = true;
moduleQueried.altitude = true;
moduleQueried.altitudeMSL = true;

moduleQueried.horizontalAccEst = true;
moduleQueried.verticalAccEst = true;
moduleQueried.nedNorthVel = true;
moduleQueried.nedEastVel = true;
moduleQueried.nedDownVel = true;

moduleQueried.SIV = true;
moduleQueried.fixType = true;
moduleQueried.carrierSolution = true;
moduleQueried.groundSpeed = true;
moduleQueried.headingOfMotion = true;
moduleQueried.speedAccEst = true;
moduleQueried.headingAccEst = true;
moduleQueried.pDOP = true;
moduleQueried.invalidLlh = true;
moduleQueried.headVeh = true;
moduleQueried.magDec = true;
moduleQueried.magAcc = true;
}
else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36)
{
Expand Down Expand Up @@ -3111,6 +3122,19 @@ uint16_t SFE_UBLOX_GPS::extractInt(uint8_t spotToStart)
return (val);
}

//Just so there is no ambiguity about whether a uint16_t will cast to a int16_t correctly...
int16_t SFE_UBLOX_GPS::extractSignedInt(int8_t spotToStart)
{
union // Use a union to convert from uint16_t to int16_t
{
uint16_t unsignedInt;
int16_t signedInt;
} stSignedInt;

stSignedInt.unsignedInt = extractInt(spotToStart);
return (stSignedInt.signedInt);
}

//Given a spot, extract a byte from the payload
uint8_t SFE_UBLOX_GPS::extractByte(uint8_t spotToStart)
{
Expand Down Expand Up @@ -3195,6 +3219,54 @@ bool SFE_UBLOX_GPS::getTimeValid(uint16_t maxWait)
return (gpsTimeValid);
}

uint32_t SFE_UBLOX_GPS::getSpeedAccEst(uint16_t maxWait)
{
if (moduleQueried.speedAccEst == false)
getPVT(maxWait);
moduleQueried.speedAccEst = false; //Since we are about to give this to user, mark this data as stale
return (speedAccEst);
}

uint32_t SFE_UBLOX_GPS::getHeadingAccEst(uint16_t maxWait)
{
if (moduleQueried.headingAccEst == false)
getPVT(maxWait);
moduleQueried.headingAccEst = false; //Since we are about to give this to user, mark this data as stale
return (headingAccEst);
}

bool SFE_UBLOX_GPS::getInvalidLlh(uint16_t maxWait)
{
if (moduleQueried.invalidLlh == false)
getPVT(maxWait);
moduleQueried.invalidLlh = false; //Since we are about to give this to user, mark this data as stale
return (invalidLlh);
}

int32_t SFE_UBLOX_GPS::getHeadVeh(uint16_t maxWait)
{
if (moduleQueried.headVeh == false)
getPVT(maxWait);
moduleQueried.headVeh = false; //Since we are about to give this to user, mark this data as stale
return (headVeh);
}

int16_t SFE_UBLOX_GPS::getMagDec(uint16_t maxWait)
{
if (moduleQueried.magDec == false)
getPVT(maxWait);
moduleQueried.magDec = false; //Since we are about to give this to user, mark this data as stale
return (magDec);
}

uint16_t SFE_UBLOX_GPS::getMagAcc(uint16_t maxWait)
{
if (moduleQueried.magAcc == false)
getPVT(maxWait);
moduleQueried.magAcc = false; //Since we are about to give this to user, mark this data as stale
return (magAcc);
}

//Get the current millisecond
uint16_t SFE_UBLOX_GPS::getMillisecond(uint16_t maxWait)
{
Expand Down Expand Up @@ -3780,6 +3852,18 @@ uint8_t SFE_UBLOX_GPS::getCarrierSolutionType(uint16_t maxWait)
return (carrierSolution);
}

//Get whether head vehicle valid or not
bool SFE_UBLOX_GPS::getHeadVehValid(uint16_t maxWait)
{
if (moduleQueried.headVehValid == false)
getPVT(maxWait);
moduleQueried.headVehValid = false; //Since we are about to give this to user, mark this data as stale
moduleQueried.all = false;

return (headVehValid);
}


//Get the ground speed in mm/s
int32_t SFE_UBLOX_GPS::getGroundSpeed(uint16_t maxWait)
{
Expand Down Expand Up @@ -3901,6 +3985,7 @@ void SFE_UBLOX_GPS::flushPVT()
moduleQueried.all = false;
moduleQueried.gnssFixOk = false;
moduleQueried.diffSoln = false;
moduleQueried.headVehValid = false;
moduleQueried.longitude = false;
moduleQueried.latitude = false;
moduleQueried.altitude = false;
Expand All @@ -3910,7 +3995,13 @@ void SFE_UBLOX_GPS::flushPVT()
moduleQueried.carrierSolution = false;
moduleQueried.groundSpeed = false;
moduleQueried.headingOfMotion = false;
moduleQueried.speedAccEst = false;
moduleQueried.headingAccEst = false;
moduleQueried.pDOP = false;
moduleQueried.invalidLlh = false;
moduleQueried.headVeh = false;
moduleQueried.magDec = false;
moduleQueried.magAcc = false;
}

//Mark all the HPPOSLLH data as read/stale. This is handy to get data alignment after CRC failure
Expand Down
27 changes: 22 additions & 5 deletions src/SparkFun_Ublox_Arduino_Library.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -507,17 +507,16 @@ class SFE_UBLOX_GPS

bool getGnssFixOk(uint16_t maxWait = getPVTmaxWait); //Get whether we have a valid fix (i.e within DOP & accuracy masks)
bool getDiffSoln(uint16_t maxWait = getPVTmaxWait); //Get whether differential corrections were applied
bool getHeadVehValid(uint16_t maxWait = getPVTmaxWait);
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.
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.
int32_t getAltitude(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above ellipsoid
int32_t getAltitudeMSL(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above mean sea level

int32_t getHorizontalAccEst(uint16_t maxWait = getPVTmaxWait);
int32_t getVerticalAccEst(uint16_t maxWait = getPVTmaxWait);
int32_t getNedNorthVel(uint16_t maxWait = getPVTmaxWait);
int32_t getNedEastVel(uint16_t maxWait = getPVTmaxWait);
int32_t getNedDownVel(uint16_t maxWait = getPVTmaxWait);

uint8_t getSIV(uint16_t maxWait = getPVTmaxWait); //Returns number of sats used in fix
uint8_t getFixType(uint16_t maxWait = getPVTmaxWait); //Returns the type of fix: 0=no, 3=3D, 4=GNSS+Deadreckoning
uint8_t getCarrierSolutionType(uint16_t maxWait = getPVTmaxWait); //Returns RTK solution: 0=no, 1=float solution, 2=fixed solution
Expand All @@ -535,6 +534,12 @@ class SFE_UBLOX_GPS
uint32_t getTimeOfWeek(uint16_t maxWait = getPVTmaxWait);
bool getDateValid(uint16_t maxWait = getPVTmaxWait);
bool getTimeValid(uint16_t maxWait = getPVTmaxWait);
uint32_t getSpeedAccEst(uint16_t maxWait = getPVTmaxWait);
uint32_t getHeadingAccEst(uint16_t maxWait = getPVTmaxWait);
bool getInvalidLlh(uint16_t maxWait = getPVTmaxWait);
int32_t getHeadVeh(uint16_t maxWait = getPVTmaxWait);
int16_t getMagDec(uint16_t maxWait = getPVTmaxWait);
uint16_t getMagAcc(uint16_t maxWait = getPVTmaxWait);

int32_t getHighResLatitude(uint16_t maxWait = getHPPOSLLHmaxWait);
int8_t getHighResLatitudeHp(uint16_t maxWait = getHPPOSLLHmaxWait);
Expand Down Expand Up @@ -730,9 +735,9 @@ class SFE_UBLOX_GPS
bool gpsDateValid;
bool gpsTimeValid;


bool gnssFixOk; //valid fix (i.e within DOP & accuracy masks)
bool diffSoln; //Differential corrections were applied
bool headVehValid;
int32_t latitude; //Degrees * 10^-7 (more accurate than floats)
int32_t longitude; //Degrees * 10^-7 (more accurate than floats)
int32_t altitude; //Number of mm above ellipsoid
Expand All @@ -747,7 +752,13 @@ class SFE_UBLOX_GPS
uint8_t carrierSolution; //Tells us when we have an RTK float/fixed solution
int32_t groundSpeed; //mm/s
int32_t headingOfMotion; //degrees * 10^-5
uint32_t speedAccEst;
uint32_t headingAccEst;
uint16_t pDOP; //Positional dilution of precision * 10^-2 (dimensionless)
bool invalidLlh;
int32_t headVeh;
int16_t magDec;
uint16_t magAcc;
uint8_t versionLow; //Loaded from getProtocolVersion().
uint8_t versionHigh;

Expand Down Expand Up @@ -943,6 +954,7 @@ class SFE_UBLOX_GPS
uint32_t extractLong(uint8_t spotToStart); //Combine four bytes from payload into long
int32_t extractSignedLong(uint8_t spotToStart); //Combine four bytes from payload into signed long (avoiding any ambiguity caused by casting)
uint16_t extractInt(uint8_t spotToStart); //Combine two bytes from payload into int
int16_t extractSignedInt(int8_t spotToStart);
uint8_t extractByte(uint8_t spotToStart); //Get byte from payload
int8_t extractSignedChar(uint8_t spotToStart); //Get signed 8-bit value from payload
void addToChecksum(uint8_t incoming); //Given an incoming byte, adjust rollingChecksumA/B
Expand Down Expand Up @@ -1022,23 +1034,28 @@ class SFE_UBLOX_GPS
uint32_t all : 1;
uint32_t gnssFixOk : 1;
uint32_t diffSoln : 1;
uint32_t headVehValid : 1;
uint32_t longitude : 1;
uint32_t latitude : 1;
uint32_t altitude : 1;
uint32_t altitudeMSL : 1;

uint32_t horizontalAccEst : 1;
uint32_t verticalAccEst : 1;
uint32_t nedNorthVel : 1;
uint32_t nedEastVel : 1;
uint32_t nedDownVel : 1;

uint32_t SIV : 1;
uint32_t fixType : 1;
uint32_t carrierSolution : 1;
uint32_t groundSpeed : 1;
uint32_t headingOfMotion : 1;
uint32_t speedAccEst : 1;
uint32_t headingAccEst : 1;
uint32_t pDOP : 1;
uint32_t invalidLlh : 1;
uint32_t headVeh : 1;
uint32_t magDec : 1;
uint32_t magAcc : 1;
uint32_t versionNumber : 1;
} moduleQueried;

Expand Down