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

Better packet validity checking #89

Merged
merged 1 commit into from
Apr 16, 2020
Merged

Better packet validity checking #89

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
116 changes: 67 additions & 49 deletions src/SparkFun_Ublox_Arduino_Library.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -467,13 +467,13 @@ void SFE_UBLOX_GPS::process(uint8_t incoming)
if (incoming == UBX_CLASS_ACK)
{
packetAck.counter = 0;
packetAck.valid = false;
packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
ubxFrameClass = CLASS_ACK;
}
else
{
packetCfg.counter = 0;
packetCfg.valid = false;
packetCfg.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
ubxFrameClass = CLASS_NOT_AN_ACK;
}
}
Expand Down Expand Up @@ -568,7 +568,7 @@ void SFE_UBLOX_GPS::processRTCM(uint8_t incoming)
}

//Given a character, file it away into the uxb packet structure
//Set valid = true once sentence is completely received and passes CRC
//Set valid to VALID or NOT_VALID once sentence is completely received and passes or fails CRC
//The payload portion of the packet can be 100s of bytes but the max array
//size is roughly 64 bytes. startingSpot can be set so we only record
//a subset of bytes within a larger packet.
Expand Down Expand Up @@ -608,7 +608,7 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)
//Validate this sentence
if (incomingUBX->checksumA == rollingChecksumA && incomingUBX->checksumB == rollingChecksumB)
{
incomingUBX->valid = true;
incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_VALID;

if (_printDebug == true)
{
Expand All @@ -617,14 +617,14 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)
_debugSerial->print(F(" Received: "));
printPacket(incomingUBX);

if (packetCfg.valid == true)
if (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
{
if (_printDebug == true)
{
_debugSerial->println(F("packetCfg now valid"));
}
}
if (packetAck.valid == true)
if (packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
{
if (_printDebug == true)
{
Expand All @@ -635,8 +635,10 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)

processUBXpacket(incomingUBX); //We've got a valid packet, now do something with it
}
else
else // Checksum failure
{
incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID;

if (_printDebug == true)
{
//Drive an external pin to allow for easier logic analyzation
Expand Down Expand Up @@ -1062,18 +1064,30 @@ void SFE_UBLOX_GPS::printPacket(ubxPacket *packet)
//"not acknowledge"(UBX-ACK-NAK) message back to the sender, depending on whether or not the message was processed correctly.
//Some messages from other classes also use the same acknowledgement mechanism.

//If the packetCfg len is 1, then we are querying the device for data
//If the packetCfg len is >1, then we are sending a new setting
//When we poll or get a setting, we will receive _both_ a config packet and an ACK
//If the poll or get request is not valid, we will receive _only_ a NACK

//If we are trying to get or poll a setting, then packetCfg.len will be 0 or 1 when the packetCfg is _sent_.
//If we poll the setting for a particular port using UBX-CFG-PRT then .len will be 1 initially
//For all other gets or polls, .len will be 0 initially
//(It would be possible for .len to be 2 _if_ we were using UBX-CFG-MSG to poll the settings for a particular message - but we don't use that (currently))

//If the get or poll _fails_, i.e. is NACK'd, then packetCfg.len could still be 0 or 1 after the NACK is received
//But if the get or poll is ACK'd, then packetCfg.len will have been updated by the incoming data and will always be at least 2

//If we are going to set the value for a setting, then packetCfg.len will be at least 3 when the packetCfg is _sent_.
//(UBX-CFG-MSG appears to have the shortest set length of 3 bytes)

//Returns true if we got the following:
//* If packetCfg len is 1 and we got and ACK and a valid packetCfg (module is responding with register content)
//* If packetCfg len is >1 and we got an ACK (no valid packetCfg needed, module absorbs new register data)
//Returns false if we timed out, got a NACK (command unknown), or had a CLS/ID mismatch
//Returns SFE_UBLOX_STATUS_DATA_RECEIVED if we got an ACK and a valid packetCfg (module is responding with register content)
//Returns SFE_UBLOX_STATUS_DATA_SENT if we got an ACK and no packetCfg (no valid packetCfg needed, module absorbs new register data)
//Returns SFE_UBLOX_STATUS_FAIL if we got an invalid packetCfg (checksum failure)
//Returns SFE_UBLOX_STATUS_COMMAND_UNKNOWN if we got a NACK (command unknown)
//Returns SFE_UBLOX_STATUS_TIMEOUT if we timed out
sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime)
{
commandAck = UBX_ACK_NONE; //Reset flag
packetCfg.valid = false; //This will go true when we receive a response to the packet we sent
packetAck.valid = false;
packetCfg.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent
packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;

unsigned long startTime = millis();
while (millis() - startTime < maxTime)
Expand All @@ -1090,42 +1104,43 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(uint8_t requestedClass, uin
_debugSerial->println(F(" msec"));
}

//Are we expecting data back or just an ACK?
if (packetCfg.len == 1)
//We've got the a valid ACK for this CLS/ID, so is packetCfg valid?
if (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
{
//We are expecting a data response so now we verify the response packet was valid
if (packetCfg.valid == true)
//We've got a valid packetCfg, so does it match the requested Class and ID?
if (packetCfg.cls == requestedClass && packetCfg.id == requestedID)
{
if (packetCfg.cls == requestedClass && packetCfg.id == requestedID)
{
if (_printDebug == true)
{
_debugSerial->print(F("waitForACKResponse: CLS/ID match after "));
_debugSerial->print(millis() - startTime);
_debugSerial->println(F(" msec"));
}
return (SFE_UBLOX_STATUS_DATA_RECEIVED); //Received a data and a correct ACK!
}
else
if (_printDebug == true)
{
//Reset packet and continue checking incoming data for matching cls/id
if (_printDebug == true)
{
_debugSerial->println(F("waitForACKResponse: CLS/ID mismatch, continue to wait..."));
}
packetCfg.valid = false; //This will go true when we receive a response to the packet we sent
_debugSerial->print(F("waitForACKResponse: CLS/ID match after "));
_debugSerial->print(millis() - startTime);
_debugSerial->println(F(" msec"));
}
return (SFE_UBLOX_STATUS_DATA_RECEIVED); //Received a data and a correct ACK!
}
else
// The Class and/or ID don't match the requested ones, so keep trying...
{
//We were expecting data but didn't get a valid config packet
//Reset packet and continue checking incoming data for matching cls/id
if (_printDebug == true)
{
_debugSerial->println(F("waitForACKResponse: Invalid config packet"));
_debugSerial->println(F("waitForACKResponse: CLS/ID mismatch, continue to wait..."));
}
return (SFE_UBLOX_STATUS_FAIL); //We got an ACK, we're never going to get valid config data
packetCfg.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent
}
}
//If we received an invalid packetCfg (checksum failure) then we can't trust it, including its Class and ID
else if (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)
{
//We were expecting data but didn't get a valid config packet
if (_printDebug == true)
{
_debugSerial->println(F("waitForACKResponse: Invalid config packet"));
}
return (SFE_UBLOX_STATUS_FAIL); //We got a checksum failure, we're never going to get valid config data
}
//We didn't receive a valid or invalid packetCfg, so we must have only received the ACK
//Let's hope this was a set?
else
{
//We have sent new data. We expect an ACK but no return config packet.
Expand All @@ -1136,6 +1151,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(uint8_t requestedClass, uin
return (SFE_UBLOX_STATUS_DATA_SENT); //New data successfully sent
}
}
//Did we receive a NACK?
else if (commandAck == UBX_ACK_NACK)
{
if (_printDebug == true)
Expand All @@ -1153,7 +1169,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(uint8_t requestedClass, uin

//TODO add check here if config went valid but we never got the following ack
//Through debug warning, This command might not get an ACK
if (packetCfg.valid == true)
if (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
{
if (_printDebug == true)
{
Expand All @@ -1172,12 +1188,13 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForACKResponse(uint8_t requestedClass, uin
}

//For non-CFG queries no ACK is sent so we use this function
//Returns true if we got a config packet full of response data that has CLS/ID match to our query packet
//Returns false if we timed out
//Returns SFE_UBLOX_STATUS_DATA_RECEIVED if we got a config packet full of response data that has CLS/ID match to our query packet
//Returns SFE_UBLOX_STATUS_CRC_FAIL if we got a corrupt config packet that has CLS/ID match to our query packet
//Returns SFE_UBLOX_STATUS_TIMEOUT if we timed out
sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime)
{
packetCfg.valid = false; //This will go true when we receive a response to the packet we sent
packetAck.valid = false;
packetCfg.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent
packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
packetCfg.cls = 255;
packetCfg.id = 255;

Expand All @@ -1187,10 +1204,10 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(uint8_t requestedClass, u
if (checkUblox() == true) //See if new data is available. Process bytes as they come in.
{
//Did we receive a config packet that matches the cls/id we requested?
if (packetCfg.cls == requestedClass && packetCfg.id == requestedID)
if ((packetCfg.cls == requestedClass) && (packetCfg.id == requestedID) && (packetCfg.valid != SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED))
{
//This packet might be good or it might be CRC corrupt
if (packetCfg.valid == true)
if (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
{
if (_printDebug == true)
{
Expand All @@ -1200,7 +1217,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(uint8_t requestedClass, u
}
return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We have new data to act upon
}
else
else // if (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)
{
if (_printDebug == true)
{
Expand All @@ -1209,8 +1226,9 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(uint8_t requestedClass, u
return (SFE_UBLOX_STATUS_CRC_FAIL); //We got the right packet but it was corrupt
}
}
else if (packetCfg.cls < 255 && packetCfg.id < 255)
else if ((packetCfg.cls < 255) && (packetCfg.id < 255) && (packetCfg.valid != SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED))
{
//We got a valid or invalid packet but it was not the droid we were looking for
//Reset packet and continue checking incoming data for matching cls/id
if (_printDebug == true)
{
Expand All @@ -1222,7 +1240,7 @@ sfe_ublox_status_e SFE_UBLOX_GPS::waitForNoACKResponse(uint8_t requestedClass, u
_debugSerial->println();
}

packetCfg.valid = false; //This will go true when we receive a response to the packet we sent
packetCfg.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; //This will go VALID (or NOT_VALID) when we receive a response to the packet we sent
packetCfg.cls = 255;
packetCfg.id = 255;
}
Expand Down
14 changes: 11 additions & 3 deletions src/SparkFun_Ublox_Arduino_Library.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,14 @@ typedef enum
SFE_UBLOX_STATUS_I2C_COMM_FAILURE,
} sfe_ublox_status_e;

// ubxPacket validity
typedef enum
{
SFE_UBLOX_PACKET_VALIDITY_NOT_VALID,
SFE_UBLOX_PACKET_VALIDITY_VALID,
SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED
} sfe_ublox_packet_validity_e;

//Registers
const uint8_t UBX_SYNCH_1 = 0xB5;
const uint8_t UBX_SYNCH_2 = 0x62;
Expand Down Expand Up @@ -400,7 +408,7 @@ typedef struct
uint8_t *payload;
uint8_t checksumA; //Given to us from module. Checked against the rolling calculated A/B checksums.
uint8_t checksumB;
boolean valid; //Goes true when both checksums pass
sfe_ublox_packet_validity_e valid; //Goes from NOT_DEFINED to VALID or NOT_VALID when checksum is checked
} ubxPacket;

// Struct to hold the results returned by getGeofenceState (returned by UBX-NAV-GEOFENCE)
Expand Down Expand Up @@ -702,8 +710,8 @@ class SFE_UBLOX_GPS
uint8_t payloadCfg[MAX_PAYLOAD_SIZE];

//Init the packet structures and init them with pointers to the payloadAck and payloadCfg arrays
ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, false};
ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, false};
ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};

//Limit checking of new data to every X ms
//If we are expecting an update every X Hz then we should check every half that amount of time
Expand Down