From a92139ca53433a5f8490800b894759690e2410f3 Mon Sep 17 00:00:00 2001
From: Andrew Berridge <aberridg@tibco.com>
Date: Sun, 27 Jun 2021 07:01:56 +0100
Subject: [PATCH 1/9] Improved state management - member functions on packet
 buffers indicate if the class and ID match, rather than using flags that get
 set during the process and processUBX functions. Still some tidy-up required,
 but makes debugging and maintenance easier and helps prevents some race
 conditions/panics/crashes (due to writing data where it shouldn't be
 written)!

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 275 ++++++++++---------
 src/SparkFun_u-blox_GNSS_Arduino_Library.h   |  13 +-
 2 files changed, 152 insertions(+), 136 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index ae80176..fde7f75 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -1084,6 +1084,8 @@ uint16_t SFE_UBLOX_GNSS::getMaxPayloadSize(uint8_t Class, uint8_t ID)
 
 //Processes NMEA and UBX binary sentences one byte at a time
 //Take a given byte and file it into the proper array
+// Note - AJB - we will ALWAYS use packetBuf to receive incoming, and then process them into the
+// appropriate packet structures
 void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID)
 {
   if ((currentSentence == NONE) || (currentSentence == NMEA))
@@ -1096,6 +1098,8 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
       currentSentence = UBX;
       //Reset the packetBuf.counter even though we will need to reset it again when ubxFrameCounter == 2
       packetBuf.counter = 0;
+      packetBuf.cls = 0;
+      packetBuf.id = 0;
       ignoreThisPayload = false; //We should not ignore this payload - yet
       //Store data in packetBuf until we know if we have a requested class and ID match
       activePacketBuffer = SFE_UBLOX_PACKET_PACKETBUF;
@@ -1146,14 +1150,24 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
       if (packetBuf.cls != UBX_CLASS_ACK)
       {
         //This is not an ACK so check for a class and ID match
-        if ((packetBuf.cls == requestedClass) && (packetBuf.id == requestedID))
+        if (packetBuf.isClassAndIdMatch(requestedClass, requestedID))
         {
+          if (_printDebug) _debugSerial->println("got requested class and ID");
           //This is not an ACK and we have a class and ID match
-          //So start diverting data into incomingUBX (usually packetCfg)
-          activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
-          incomingUBX->cls = packetBuf.cls; //Copy the class and ID into incomingUBX (usually packetCfg)
-          incomingUBX->id = packetBuf.id;
-          incomingUBX->counter = packetBuf.counter; //Copy over the .counter too
+          //So start diverting data into the correct buffer
+          if (packetBuf.cls == UBX_CLASS_CFG) 
+          {
+            if (_printDebug) _debugSerial->println("process: activeBuffer PACKETCFG");
+            activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
+            packetCfg.cls = packetBuf.cls;
+            packetCfg.id = packetBuf.id;
+            packetCfg.counter = packetBuf.counter;
+          } 
+          else
+          {
+            if (_printDebug) _debugSerial->println("process: activeBuffer PACKETBUF");
+            activePacketBuffer = SFE_UBLOX_PACKET_PACKETBUF;            
+          }                              
         }
         //This is not an ACK and we do not have a complete class and ID match
         //So let's check if this is an "automatic" message which has its own storage defined
@@ -1196,8 +1210,9 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
               _debugSerial->println(packetBuf.id, HEX);
               _debugSerial->println(F("process: \"automatic\" message could overwrite data"));
             }
-            // The RAM allocation failed so fall back to using incomingUBX (usually packetCfg) even though we risk overwriting data
-            activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
+            // The RAM allocation failed so fall back to using incomingUBX (packetBuf) 
+            // todo - tidy this!
+            activePacketBuffer = SFE_UBLOX_PACKET_PACKETBUF;
             incomingUBX->cls = packetBuf.cls; //Copy the class and ID into incomingUBX (usually packetCfg)
             incomingUBX->id = packetBuf.id;
             incomingUBX->counter = packetBuf.counter; //Copy over the .counter too
@@ -1221,16 +1236,22 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
         }
         else
         {
-          //This is not an ACK and we do not have a class and ID match
+          //This is not an ACK, nor auto and we do not have a class and ID match
           //so we should keep diverting data into packetBuf and ignore the payload
+          if (_printDebug) _debugSerial->println("IGNORING!!!");
           ignoreThisPayload = true;
         }
       }
       else
-      {
-        // This is an ACK so it is to early to do anything with it
-        // We need to wait until we have received the length and data bytes
-        // So we should keep diverting data into packetBuf
+      {        
+        // Ack packet! We can deal with it just like any other packet
+          // Then this is an ACK so copy it into packetAck
+          activePacketBuffer = SFE_UBLOX_PACKET_PACKETACK;
+          if (_printDebug) _debugSerial->println("process: activeBuffer PACKETACK");
+          packetAck.cls = packetBuf.cls;
+          packetAck.id = packetBuf.id;   
+          packetAck.counter = packetBuf.counter;
+          packetAck.startingSpot = packetBuf.startingSpot;
       }
     }
     else if (ubxFrameCounter == 4) //Length LSB
@@ -1278,49 +1299,21 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
       else // Length is >= 2 so this must be a payload byte
       {
         packetBuf.payload[1] = incoming;
-      }
-      // Now that we have received two payload bytes, we can check for a matching ACK/NACK
-      if ((activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF) // If we are not already processing a data packet
-          && (packetBuf.cls == UBX_CLASS_ACK)                // and if this is an ACK/NACK
-          && (packetBuf.payload[0] == requestedClass)        // and if the class matches
-          && (packetBuf.payload[1] == requestedID))          // and if the ID matches
-      {
-        if (packetBuf.len == 2) // Check if .len is 2
-        {
-          // Then this is a matching ACK so copy it into packetAck
-          activePacketBuffer = SFE_UBLOX_PACKET_PACKETACK;
-          packetAck.cls = packetBuf.cls;
-          packetAck.id = packetBuf.id;
-          packetAck.len = packetBuf.len;
-          packetAck.counter = packetBuf.counter;
-          packetAck.payload[0] = packetBuf.payload[0];
-          packetAck.payload[1] = packetBuf.payload[1];
-        }
-        else // Length is not 2 (hopefully this is impossible!)
-        {
-          if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
-          {
-            _debugSerial->print(F("process: ACK received with .len != 2: Class: 0x"));
-            _debugSerial->print(packetBuf.payload[0], HEX);
-            _debugSerial->print(F(" ID: 0x"));
-            _debugSerial->print(packetBuf.payload[1], HEX);
-            _debugSerial->print(F(" len: "));
-            _debugSerial->println(packetBuf.len);
-          }
-        }
-      }
+      }      
     }
 
-    //Divert incoming into the correct buffer
-    if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETACK)
-      processUBX(incoming, &packetAck, requestedClass, requestedID);
-    else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)
-      processUBX(incoming, incomingUBX, requestedClass, requestedID);
-    else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
-      processUBX(incoming, &packetBuf, requestedClass, requestedID);
-    else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
-      processUBX(incoming, &packetAuto, requestedClass, requestedID);
-
+    if (ubxFrameCounter > 1) // we have passed the micro and b characters
+    {
+      //Divert incoming into the correct buffer
+      if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETACK)
+        processUBX(incoming, &packetAck, requestedClass, requestedID);
+      else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)      
+        processUBX(incoming, &packetCfg, requestedClass, requestedID);
+      else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
+        processUBX(incoming, &packetBuf, requestedClass, requestedID);
+      else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
+        processUBX(incoming, &packetAuto, requestedClass, requestedID);
+    }
     //Finally, increment the frame counter
     ubxFrameCounter++;
   }
@@ -1528,24 +1521,26 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
   uint16_t maximum_payload_size;
   if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)
     maximum_payload_size = packetCfgPayloadSize;
-  else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
+  else if (incomingUBX->counter > 1 // wait for class and ID
+    && (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO || activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF))
   {
     // Calculate maximum payload size once Class and ID have been received
     // (This check is probably redundant as activePacketBuffer can only be SFE_UBLOX_PACKET_PACKETAUTO
     //  when ubxFrameCounter >= 3)
     //if (incomingUBX->counter >= 2)
     //{
-      maximum_payload_size = getMaxPayloadSize(incomingUBX->cls, incomingUBX->id);
-      if (maximum_payload_size == 0)
+    maximum_payload_size = getMaxPayloadSize(incomingUBX->cls, incomingUBX->id);
+    if (maximum_payload_size == 0)
+    {
+      if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
       {
-        if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
-        {
-          _debugSerial->print(F("processUBX: getMaxPayloadSize returned ZERO!! Class: 0x"));
-          _debugSerial->print(incomingUBX->cls);
-          _debugSerial->print(F(" ID: 0x"));
-          _debugSerial->println(incomingUBX->id);
-        }
+        _debugSerial->print(F("processUBX: getMaxPayloadSize returned ZERO!! Class: 0x"));
+        _debugSerial->printf("%x", incomingUBX->cls);
+        _debugSerial->print(F(" ID: 0x"));
+        _debugSerial->printf("%x\n", incomingUBX->id);
+      
       }
+    }
     //}
     //else
     //  maximum_payload_size = 2;
@@ -1590,27 +1585,35 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
     if ((incomingUBX->checksumA == rollingChecksumA) && (incomingUBX->checksumB == rollingChecksumB))
     {
       incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_VALID; // Flag the packet as valid
+      if (_printDebug) _debugSerial->println("processUBX packet is valid");
 
-      // Let's check if the class and ID match the requestedClass and requestedID
-      // Remember - this could be a data packet or an ACK packet
-      if ((incomingUBX->cls == requestedClass) && (incomingUBX->id == requestedID))
-      {
-        incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_VALID; // If we have a match, set the classAndIDmatch flag to valid
+      // ACK or NACK???
+      if (incomingUBX->isAckForClassAndId(requestedClass, requestedID)) {
+        if (_printDebug)
+        {
+          _debugSerial->print(F("processUBX: ACK received: Requested Class: 0x"));
+          _debugSerial->print(incomingUBX->payload[0], HEX);
+          _debugSerial->print(F(" Requested ID: 0x"));
+          _debugSerial->println(incomingUBX->payload[1], HEX);
+        }
       }
 
-      // If this is an ACK then let's check if the class and ID match the requestedClass and requestedID
-      else if ((incomingUBX->cls == UBX_CLASS_ACK) && (incomingUBX->id == UBX_ACK_ACK) && (incomingUBX->payload[0] == requestedClass) && (incomingUBX->payload[1] == requestedID))
-      {
-        incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_VALID; // If we have a match, set the classAndIDmatch flag to valid
+      else if (incomingUBX->isAckForClassAndId(requestedClass, requestedID))
+      {        
+        if (_printDebug == true)
+        {
+          _debugSerial->print(F("processUBX: NACK received: Requested Class: 0x"));
+          _debugSerial->print(incomingUBX->payload[0], HEX);
+          _debugSerial->print(F(" Requested ID: 0x"));
+          _debugSerial->println(incomingUBX->payload[1], HEX);
+        }
       }
 
-      // If this is a NACK then let's check if the class and ID match the requestedClass and requestedID
-      else if ((incomingUBX->cls == UBX_CLASS_ACK) && (incomingUBX->id == UBX_ACK_NACK) && (incomingUBX->payload[0] == requestedClass) && (incomingUBX->payload[1] == requestedID))
+      else if (incomingUBX->isClassAndIdMatch(requestedClass, requestedID)) 
       {
-        incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_NOTACKNOWLEDGED; // If we have a match, set the classAndIDmatch flag to NOTACKNOWLEDGED
         if (_printDebug == true)
         {
-          _debugSerial->print(F("processUBX: NACK received: Requested Class: 0x"));
+          _debugSerial->print(F("processUBX: Message received for requested Class: 0x"));
           _debugSerial->print(incomingUBX->payload[0], HEX);
           _debugSerial->print(F(" Requested ID: 0x"));
           _debugSerial->println(incomingUBX->payload[1], HEX);
@@ -1639,22 +1642,6 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
         _debugSerial->print(F(" Received: "));
         printPacket(incomingUBX);
 
-        if (incomingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
-        {
-          _debugSerial->println(F("packetCfg now valid"));
-        }
-        if (packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
-        {
-          _debugSerial->println(F("packetAck now valid"));
-        }
-        if (incomingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID)
-        {
-          _debugSerial->println(F("packetCfg classAndIDmatch"));
-        }
-        if (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID)
-        {
-          _debugSerial->println(F("packetAck classAndIDmatch"));
-        }
       }
 
       //We've got a valid packet, now do something with it but only if ignoreThisPayload is false
@@ -1718,11 +1705,16 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
   {
     //If an automatic packet comes in asynchronously, we need to fudge the startingSpot
     uint16_t startingSpot = incomingUBX->startingSpot;
-    if (checkAutomatic(incomingUBX->cls, incomingUBX->id))
+    if (!incomingUBX->isClassAndIdMatch(requestedClass, requestedID) && checkAutomatic(incomingUBX->cls, incomingUBX->id))
+    {
+      //_debugSerial->println("processUBX: incoming is automatic");
       startingSpot = 0;
+    }
+
     // Check if this is payload data which should be ignored
     if (ignoreThisPayload == false)
     {
+      //_debugSerial->printf("incomingUBX->counter: %d, startingSpot: %d, maximum_payload_size: %d", incomingUBX->counter, startingSpot, maximum_payload_size);
       //Begin recording if counter goes past startingSpot
       if ((incomingUBX->counter - 4) >= startingSpot)
       {
@@ -2756,7 +2748,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::sendCommand(ubxPacket *outgoingUBX, uint16_t
       {
         _debugSerial->println(F("sendCommand: Waiting for ACK response"));
       }
-      retVal = waitForACKResponse(outgoingUBX, outgoingUBX->cls, outgoingUBX->id, maxWait); //Wait for Ack response
+      retVal = waitForACKResponse(outgoingUBX, outgoingUBX->cls, outgoingUBX->id, maxWait, expectACKonly); //Wait for Ack response
     }
     else
     {
@@ -2989,7 +2981,7 @@ void SFE_UBLOX_GNSS::printPacket(ubxPacket *packet, boolean alwaysPrintPayload)
 //Returns SFE_UBLOX_STATUS_TIMEOUT if we timed out
 //Returns SFE_UBLOX_STATUS_DATA_OVERWRITTEN if we got an ACK and a valid packetCfg but that the packetCfg has been
 // or is currently being overwritten (remember that Serial data can arrive very slowly)
-sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime)
+sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime, bool expectACKonly)
 {
   outgoingUBX->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;
@@ -3005,10 +2997,22 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   {
     if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
     {
-      // If both the outgoingUBX->classAndIDmatch and packetAck.classAndIDmatch are VALID
-      // and outgoingUBX->valid is _still_ VALID and the class and ID _still_ match
-      // then we can be confident that the data in outgoingUBX is valid
-      if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID))
+
+      // If we are expecting an ACK only, return as soon as we have it!
+      if (expectACKonly && packetAck.isAckForClassAndId(requestedClass, requestedID) && packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
+      {
+        if (_printDebug == true)
+        {
+          _debugSerial->print(F("waitForACKResponse: expecting ACK only, and valid ACK received after "));
+          _debugSerial->print(millis() - startTime);
+          _debugSerial->println(F(" msec"));
+        }
+        return (SFE_UBLOX_STATUS_DATA_SENT); //todo - AJB - I'd prefer another flag - ACK_RECEIVED or something like that!
+      }
+
+      // If both the packetCfg->isClassAndIdMatch and packetAck.isClassAndIdMatch
+      // then we can be confident that the data in packetCfg is valid
+      if (packetAck.isAckForClassAndId(requestedClass, requestedID) && packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID && packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID && packetCfg.isClassAndIdMatch(requestedClass, requestedID))
       {
         if (_printDebug == true)
         {
@@ -3024,7 +3028,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
       // then outgoingUBX->classAndIDmatch will be NOT_DEFINED and the packetAck.classAndIDmatch will VALID.
       // We should not check outgoingUBX->valid, outgoingUBX->cls or outgoingUBX->id
       // as these may have been changed by an automatic packet.
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID))
+      else if (!packetCfg.isClassAndIdMatch(requestedClass, requestedID) && packetAck.isClassAndIdMatch(requestedClass, requestedID))
       {
         if (_printDebug == true)
         {
@@ -3073,7 +3077,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
       // So I think this is telling us we need a special state for packetAck.classAndIDmatch to tell us
       // the packet was definitely NACK'd otherwise we are possibly just guessing...
       // Note: the addition of packetBuf changes the logic of this, but we'll leave the code as is for now.
-      else if (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_NOTACKNOWLEDGED)
+      else if (packetAck.isNackForClassAndId(requestedClass, requestedID))
       {
         if (_printDebug == true)
         {
@@ -3131,7 +3135,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   // We have timed out...
   // If the outgoingUBX->classAndIDmatch is VALID then we can take a gamble and return DATA_RECEIVED
   // even though we did not get an ACK
-  if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID))
+  if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) && !packetAck.isClassAndIdMatch(requestedClass, requestedID)) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID))
   {
     if (_printDebug == true)
     {
@@ -3172,13 +3176,13 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForNoACKResponse(ubxPacket *outgoingUBX,
   unsigned long startTime = millis();
   while (millis() - startTime < maxTime)
   {
-    if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
+    if (checkUbloxInternal(&packetBuf, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
     {
 
       // If outgoingUBX->classAndIDmatch is VALID
       // and outgoingUBX->valid is _still_ VALID and the class and ID _still_ match
       // then we can be confident that the data in outgoingUBX is valid
-      if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID))
+      if (packetBuf.isClassAndIdMatch(requestedClass, requestedID) && packetBuf.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
       {
         if (_printDebug == true)
         {
@@ -3223,7 +3227,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForNoACKResponse(ubxPacket *outgoingUBX,
       }
 
       // If the outgoingUBX->classAndIDmatch is NOT_VALID then we return CRC failure
-      else if (outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)
+      /*else if (outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)
       {
         if (_printDebug == true)
         {
@@ -3232,7 +3236,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForNoACKResponse(ubxPacket *outgoingUBX,
           _debugSerial->println(F(" msec"));
         }
         return (SFE_UBLOX_STATUS_CRC_FAIL); //We received invalid data
-      }
+      }*/
     }
 
     delayMicroseconds(500);
@@ -5410,7 +5414,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVPOSECEFrate(uint8_t rate, boolean implicitUpda
   payloadCfg[1] = UBX_NAV_POSECEF;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVPOSECEF->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -5567,7 +5571,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVSTATUSrate(uint8_t rate, boolean implicitUpdat
   payloadCfg[1] = UBX_NAV_STATUS;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVSTATUS->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -5744,7 +5748,7 @@ boolean SFE_UBLOX_GNSS::setAutoDOPrate(uint8_t rate, boolean implicitUpdate, uin
   payloadCfg[1] = UBX_NAV_DOP;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVDOP->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -5907,7 +5911,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVATTrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_NAV_ATT;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVATT->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -6086,8 +6090,11 @@ boolean SFE_UBLOX_GNSS::setAutoPVTrate(uint8_t rate, boolean implicitUpdate, uin
   payloadCfg[0] = UBX_CLASS_NAV;
   payloadCfg[1] = UBX_NAV_PVT;
   payloadCfg[2] = rate; // rate relative to navigation freq.
+  packetCfg.payload = payloadCfg;
+  
+  // Payload is working here but again we get no ack!
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVPVT->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -6246,7 +6253,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVODOrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_NAV_ODO;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVODO->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -6402,7 +6409,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVVELECEFrate(uint8_t rate, boolean implicitUpda
   payloadCfg[1] = UBX_NAV_VELECEF;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVVELECEF->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -6556,7 +6563,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVVELNEDrate(uint8_t rate, boolean implicitUpdat
   payloadCfg[1] = UBX_NAV_VELNED;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVVELNED->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -6712,7 +6719,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVHPPOSECEFrate(uint8_t rate, boolean implicitUp
   payloadCfg[1] = UBX_NAV_HPPOSECEF;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVHPPOSECEF->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -6890,7 +6897,7 @@ boolean SFE_UBLOX_GNSS::setAutoHPPOSLLHrate(uint8_t rate, boolean implicitUpdate
   payloadCfg[1] = UBX_NAV_HPPOSLLH;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVHPPOSLLH->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -7046,7 +7053,7 @@ boolean SFE_UBLOX_GNSS::setAutoNAVCLOCKrate(uint8_t rate, boolean implicitUpdate
   payloadCfg[1] = UBX_NAV_CLOCK;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVCLOCK->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -7301,7 +7308,7 @@ boolean SFE_UBLOX_GNSS::setAutoRELPOSNEDrate(uint8_t rate, boolean implicitUpdat
   payloadCfg[1] = UBX_NAV_RELPOSNED;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXNAVRELPOSNED->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -7457,7 +7464,7 @@ boolean SFE_UBLOX_GNSS::setAutoRXMSFRBXrate(uint8_t rate, boolean implicitUpdate
   payloadCfg[1] = UBX_RXM_SFRBX;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXRXMSFRBX->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -7613,7 +7620,7 @@ boolean SFE_UBLOX_GNSS::setAutoRXMRAWXrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_RXM_RAWX;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXRXMRAWX->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -7816,7 +7823,7 @@ boolean SFE_UBLOX_GNSS::setAutoTIMTM2rate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_TIM_TM2;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXTIMTM2->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -8001,7 +8008,7 @@ boolean SFE_UBLOX_GNSS::setAutoESFALGrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_ESF_ALG;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXESFALG->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -8186,7 +8193,7 @@ boolean SFE_UBLOX_GNSS::setAutoESFSTATUSrate(uint8_t rate, boolean implicitUpdat
   payloadCfg[1] = UBX_ESF_STATUS;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXESFSTATUS->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -8372,7 +8379,7 @@ boolean SFE_UBLOX_GNSS::setAutoESFINSrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_ESF_INS;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXESFINS->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -8557,7 +8564,7 @@ boolean SFE_UBLOX_GNSS::setAutoESFMEASrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_ESF_MEAS;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXESFMEAS->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -8742,7 +8749,7 @@ boolean SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_ESF_RAW;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXESFRAW->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -8932,7 +8939,7 @@ boolean SFE_UBLOX_GNSS::setAutoHNRATTrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_HNR_ATT;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXHNRATT->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -9123,7 +9130,7 @@ boolean SFE_UBLOX_GNSS::setAutoHNRINSrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_HNR_INS;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXHNRINS->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -9308,7 +9315,7 @@ boolean SFE_UBLOX_GNSS::setAutoHNRPVTrate(uint8_t rate, boolean implicitUpdate,
   payloadCfg[1] = UBX_HNR_PVT;
   payloadCfg[2] = rate; // rate relative to navigation freq.
 
-  boolean ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean ok = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
   if (ok)
   {
     packetUBXHNRPVT->automaticFlags.flags.bits.automatic = (rate > 0);
@@ -9441,7 +9448,7 @@ boolean SFE_UBLOX_GNSS::setNavigationFrequency(uint8_t navFreq, uint16_t maxWait
   payloadCfg[0] = measurementRate & 0xFF; //measRate LSB
   payloadCfg[1] = measurementRate >> 8;   //measRate MSB
 
-  boolean result = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean result = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 
   flushCFGRATE(); // Mark the polled measurement and navigation rate data as stale
 
@@ -9486,7 +9493,7 @@ boolean SFE_UBLOX_GNSS::setMeasurementRate(uint16_t rate, uint16_t maxWait)
   payloadCfg[0] = rate & 0xFF; //measRate LSB
   payloadCfg[1] = rate >> 8;   //measRate MSB
 
-  boolean result = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean result = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 
   flushCFGRATE(); // Mark the polled measurement and navigation rate data as stale
 
@@ -9525,7 +9532,7 @@ boolean SFE_UBLOX_GNSS::setNavigationRate(uint16_t rate, uint16_t maxWait)
   payloadCfg[2] = rate & 0xFF; //navRate LSB
   payloadCfg[3] = rate >> 8;   //navRate MSB
 
-  boolean result = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  boolean result = ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 
   flushCFGRATE(); // Mark the polled measurement and navigation rate data as stale
 
@@ -10753,7 +10760,7 @@ boolean SFE_UBLOX_GNSS::setHNRNavigationRate(uint8_t rate, uint16_t maxWait)
   payloadCfg[0] = rate;
 
   //Update the navigation rate
-  sfe_ublox_status_e result = sendCommand(&packetCfg, maxWait); // We are only expecting an ACK
+  sfe_ublox_status_e result = sendCommand(&packetCfg, maxWait, true); // We are only expecting an ACK
 
   //Adjust the I2C polling timeout based on update rate
   if (result == SFE_UBLOX_STATUS_DATA_SENT)
diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
index 015ba3f..86ae99c 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
@@ -507,6 +507,15 @@ struct ubxPacket
 	uint8_t checksumB;
 	sfe_ublox_packet_validity_e valid;			 //Goes from NOT_DEFINED to VALID or NOT_VALID when checksum is checked
 	sfe_ublox_packet_validity_e classAndIDmatch; // Goes from NOT_DEFINED to VALID or NOT_VALID when the Class and ID match the requestedClass and requestedID
+	bool isClassAndIdMatch(uint8_t theClass, uint8_t theId) {
+		return cls == theClass && theId == id;
+	}
+	bool isAckForClassAndId(uint8_t theClass, uint8_t theId) {
+		return isClassAndIdMatch(UBX_CLASS_ACK, UBX_ACK_ACK) && payload[0] == theClass && payload[1] == theId;
+	}
+	bool isNackForClassAndId(uint8_t theClass, uint8_t theId) {
+		return isClassAndIdMatch(UBX_CLASS_ACK, UBX_ACK_NACK) && payload[0] == theClass && payload[1] == theId;
+	}
 };
 
 // Struct to hold the results returned by getGeofenceState (returned by UBX-NAV-GEOFENCE)
@@ -639,7 +648,7 @@ class SFE_UBLOX_GNSS
 
 	// After sending a message to the module, wait for the expected response (data+ACK or just data)
 
-	sfe_ublox_status_e waitForACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime = defaultMaxWait);	 //Poll the module until a config packet and an ACK is received, or just an ACK
+	sfe_ublox_status_e waitForACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime = defaultMaxWait, bool expectACKonly = false);	 //Poll the module until a config packet and an ACK is received, or just an ACK
 	sfe_ublox_status_e waitForNoACKResponse(ubxPacket *outgoingUBX, uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime = defaultMaxWait); //Poll the module until a config packet is received
 
 	// Check if any callbacks need to be called
@@ -1300,7 +1309,7 @@ class SFE_UBLOX_GNSS
 	//The packet buffers
 	//These are pointed at from within the ubxPacket
 	uint8_t payloadAck[2];				  // Holds the requested ACK/NACK
-	uint8_t payloadBuf[2];				  // Temporary buffer used to screen incoming packets or dump unrequested packets
+	uint8_t payloadBuf[MAX_PAYLOAD_SIZE];				  // Temporary buffer used to screen incoming packets or dump unrequested packets aberridg TODO: figure out if we can reduce memory usage by not using the whole buffer - needs some clean-up
 	size_t packetCfgPayloadSize = 0; // Size for the packetCfg payload. .begin will set this to MAX_PAYLOAD_SIZE if necessary. User can change with setPacketCfgPayloadSize
 	uint8_t *payloadCfg = NULL;
 	uint8_t *payloadAuto = NULL;

From a1ca3d4608e4998910cb29797baba985ece85e41 Mon Sep 17 00:00:00 2001
From: Andrew Berridge <aberridg@tibco.com>
Date: Sun, 27 Jun 2021 07:36:52 +0100
Subject: [PATCH 2/9] Removed classAndIdMatch flag on packet buffers. Gradually
 moving state management out of the process and processUBX functions.

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 126 ++-----------------
 src/SparkFun_u-blox_GNSS_Arduino_Library.h   |   9 +-
 2 files changed, 17 insertions(+), 118 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index fde7f75..1b615a9 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -1625,7 +1625,7 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
       else if (checkAutomatic(incomingUBX->cls, incomingUBX->id))
       {
         // This isn't the message we are looking for...
-        // Let's say so and leave incomingUBX->classAndIDmatch _unchanged_
+        // Let's say so...
         if (_printDebug == true)
         {
           _debugSerial->print(F("processUBX: incoming \"automatic\" message: Class: 0x"));
@@ -1654,20 +1654,6 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
     {
       incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID;
 
-      // Let's check if the class and ID match the requestedClass and requestedID.
-      // This is potentially risky as we are saying that we saw the requested Class and ID
-      // but that the packet checksum failed. Potentially it could be the class or ID bytes
-      // that caused the checksum error!
-      if ((incomingUBX->cls == requestedClass) && (incomingUBX->id == requestedID))
-      {
-        incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID; // If we have a match, set the classAndIDmatch flag to not valid
-      }
-      // If this is an ACK then let's check if the class and ID match the requestedClass and requestedID
-      else if ((incomingUBX->cls == UBX_CLASS_ACK) && (incomingUBX->payload[0] == requestedClass) && (incomingUBX->payload[1] == requestedID))
-      {
-        incomingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_VALID; // If we have a match, set the classAndIDmatch flag to not valid
-      }
-
       if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
       {
         //Drive an external pin to allow for easier logic analyzation
@@ -2987,10 +2973,6 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
   packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
   packetAuto.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-  outgoingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // This will go VALID (or NOT_VALID) when we receive a packet that matches the requested class and ID
-  packetAck.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-  packetBuf.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-  packetAuto.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
 
   unsigned long startTime = millis();
   while (millis() - startTime < maxTime)
@@ -3022,12 +3004,6 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         }
         return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data and a correct ACK!
       }
-
-      // We can be confident that the data packet (if we are going to get one) will always arrive
-      // before the matching ACK. So if we sent a config packet which only produces an ACK
-      // then outgoingUBX->classAndIDmatch will be NOT_DEFINED and the packetAck.classAndIDmatch will VALID.
-      // We should not check outgoingUBX->valid, outgoingUBX->cls or outgoingUBX->id
-      // as these may have been changed by an automatic packet.
       else if (!packetCfg.isClassAndIdMatch(requestedClass, requestedID) && packetAck.isClassAndIdMatch(requestedClass, requestedID))
       {
         if (_printDebug == true)
@@ -3039,27 +3015,9 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         return (SFE_UBLOX_STATUS_DATA_SENT); //We got an ACK but no data...
       }
 
-      // If both the outgoingUBX->classAndIDmatch and packetAck.classAndIDmatch are VALID
-      // but the outgoingUBX->cls or ID no longer match then we can be confident that we had
-      // valid data but it has been or is currently being overwritten by an automatic packet (e.g. PVT).
-      // If (e.g.) a PVT packet is _being_ received: outgoingUBX->valid will be NOT_DEFINED
-      // If (e.g.) a PVT packet _has been_ received: outgoingUBX->valid will be VALID (or just possibly NOT_VALID)
-      // So we cannot use outgoingUBX->valid as part of this check.
-      // Note: the addition of packetBuf should make this check redundant!
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && ((outgoingUBX->cls != requestedClass) || (outgoingUBX->id != requestedID)))
-      {
-        if (_printDebug == true)
-        {
-          _debugSerial->print(F("waitForACKResponse: data being OVERWRITTEN after "));
-          _debugSerial->print(millis() - startTime);
-          _debugSerial->println(F(" msec"));
-        }
-        return (SFE_UBLOX_STATUS_DATA_OVERWRITTEN); // Data was valid but has been or is being overwritten
-      }
-
-      // If packetAck.classAndIDmatch is VALID but both outgoingUBX->valid and outgoingUBX->classAndIDmatch
+      // If packetAck is for requested class/ID but both outgoingUBX->valid and packetCfg isClassAndIDmatch
       // are NOT_VALID then we can be confident we have had a checksum failure on the data packet
-      else if ((packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID))
+      else if ((packetAck.isAckForClassAndId(requestedClass, requestedID) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)))
       {
         if (_printDebug == true)
         {
@@ -3069,14 +3027,6 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         }
         return (SFE_UBLOX_STATUS_CRC_FAIL); //Checksum fail
       }
-
-      // If our packet was not-acknowledged (NACK) we do not receive a data packet - we only get the NACK.
-      // So you would expect outgoingUBX->valid and outgoingUBX->classAndIDmatch to still be NOT_DEFINED
-      // But if a full PVT packet arrives afterwards outgoingUBX->valid could be VALID (or just possibly NOT_VALID)
-      // but outgoingUBX->cls and outgoingUBX->id would not match...
-      // So I think this is telling us we need a special state for packetAck.classAndIDmatch to tell us
-      // the packet was definitely NACK'd otherwise we are possibly just guessing...
-      // Note: the addition of packetBuf changes the logic of this, but we'll leave the code as is for now.
       else if (packetAck.isNackForClassAndId(requestedClass, requestedID))
       {
         if (_printDebug == true)
@@ -3088,10 +3038,10 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         return (SFE_UBLOX_STATUS_COMMAND_NACK); //We received a NACK!
       }
 
-      // If the outgoingUBX->classAndIDmatch is VALID but the packetAck.classAndIDmatch is NOT_VALID
+      // If the packetCfg.isClassAndIdMatch but the packetAck is not for this class/ID
       // then the ack probably had a checksum error. We will take a gamble and return DATA_RECEIVED.
       // If we were playing safe, we should return FAIL instead
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID) && (outgoingUBX->cls == requestedClass) && (outgoingUBX->id == requestedID))
+      else if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID)  && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)))
       {
         if (_printDebug == true)
         {
@@ -3102,9 +3052,9 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data and an invalid ACK!
       }
 
-      // If the outgoingUBX->classAndIDmatch is NOT_VALID and the packetAck.classAndIDmatch is NOT_VALID
+      // If classes of ack and data do not match
       // then we return a FAIL. This must be a double checksum failure?
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID))
+      else if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) == false) && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false))
       {
         if (_printDebug == true)
         {
@@ -3115,9 +3065,9 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         return (SFE_UBLOX_STATUS_FAIL); //We received invalid data and an invalid ACK!
       }
 
-      // If the outgoingUBX->classAndIDmatch is VALID and the packetAck.classAndIDmatch is NOT_DEFINED
+      // If the config packet is VALID and packetAck does not match the requested class/ID 
       // then the ACK has not yet been received and we should keep waiting for it
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && (packetAck.classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED))
+      else if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) == true) && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false))
       {
         // if (_printDebug == true)
         // {
@@ -3133,7 +3083,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   } //while (millis() - startTime < maxTime)
 
   // We have timed out...
-  // If the outgoingUBX->classAndIDmatch is VALID then we can take a gamble and return DATA_RECEIVED
+  // If the packetCfg matches the requested class/ID then we can take a gamble and return DATA_RECEIVED
   // even though we did not get an ACK
   if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) && !packetAck.isClassAndIdMatch(requestedClass, requestedID)) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID))
   {
@@ -3168,20 +3118,16 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForNoACKResponse(ubxPacket *outgoingUBX,
   packetAck.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
   packetBuf.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
   packetAuto.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-  outgoingUBX->classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED; // This will go VALID (or NOT_VALID) when we receive a packet that matches the requested class and ID
-  packetAck.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-  packetBuf.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-  packetAuto.classAndIDmatch = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
-
+ 
   unsigned long startTime = millis();
   while (millis() - startTime < maxTime)
   {
     if (checkUbloxInternal(&packetBuf, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
     {
 
-      // If outgoingUBX->classAndIDmatch is VALID
+      // If packetBuf matches
       // and outgoingUBX->valid is _still_ VALID and the class and ID _still_ match
-      // then we can be confident that the data in outgoingUBX is valid
+      // then we can be confident that the data in packetBuf is valid
       if (packetBuf.isClassAndIdMatch(requestedClass, requestedID) && packetBuf.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
       {
         if (_printDebug == true)
@@ -3192,53 +3138,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForNoACKResponse(ubxPacket *outgoingUBX,
         }
         return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data!
       }
-
-      // If the outgoingUBX->classAndIDmatch is VALID
-      // but the outgoingUBX->cls or ID no longer match then we can be confident that we had
-      // valid data but it has been or is currently being overwritten by another packet (e.g. PVT).
-      // If (e.g.) a PVT packet is _being_ received: outgoingUBX->valid will be NOT_DEFINED
-      // If (e.g.) a PVT packet _has been_ received: outgoingUBX->valid will be VALID (or just possibly NOT_VALID)
-      // So we cannot use outgoingUBX->valid as part of this check.
-      // Note: the addition of packetBuf should make this check redundant!
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_VALID) && ((outgoingUBX->cls != requestedClass) || (outgoingUBX->id != requestedID)))
-      {
-        if (_printDebug == true)
-        {
-          _debugSerial->print(F("waitForNoACKResponse: data being OVERWRITTEN after "));
-          _debugSerial->print(millis() - startTime);
-          _debugSerial->println(F(" msec"));
-        }
-        return (SFE_UBLOX_STATUS_DATA_OVERWRITTEN); // Data was valid but has been or is being overwritten
-      }
-
-      // If outgoingUBX->classAndIDmatch is NOT_DEFINED
-      // and outgoingUBX->valid is VALID then this must be (e.g.) a PVT packet
-      else if ((outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID))
-      {
-        // if (_printDebug == true)
-        // {
-        //   _debugSerial->print(F("waitForNoACKResponse: valid but UNWANTED data after "));
-        //   _debugSerial->print(millis() - startTime);
-        //   _debugSerial->print(F(" msec. Class: "));
-        //   _debugSerial->print(outgoingUBX->cls);
-        //   _debugSerial->print(F(" ID: "));
-        //   _debugSerial->print(outgoingUBX->id);
-        // }
-      }
-
-      // If the outgoingUBX->classAndIDmatch is NOT_VALID then we return CRC failure
-      /*else if (outgoingUBX->classAndIDmatch == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)
-      {
-        if (_printDebug == true)
-        {
-          _debugSerial->print(F("waitForNoACKResponse: CLS/ID match but failed CRC after "));
-          _debugSerial->print(millis() - startTime);
-          _debugSerial->println(F(" msec"));
-        }
-        return (SFE_UBLOX_STATUS_CRC_FAIL); //We received invalid data
-      }*/
     }
-
     delayMicroseconds(500);
   }
 
diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
index 86ae99c..2aa85a0 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
@@ -506,7 +506,6 @@ struct ubxPacket
 	uint8_t checksumA; //Given to us from module. Checked against the rolling calculated A/B checksums.
 	uint8_t checksumB;
 	sfe_ublox_packet_validity_e valid;			 //Goes from NOT_DEFINED to VALID or NOT_VALID when checksum is checked
-	sfe_ublox_packet_validity_e classAndIDmatch; // Goes from NOT_DEFINED to VALID or NOT_VALID when the Class and ID match the requestedClass and requestedID
 	bool isClassAndIdMatch(uint8_t theClass, uint8_t theId) {
 		return cls == theClass && theId == id;
 	}
@@ -1318,10 +1317,10 @@ class SFE_UBLOX_GNSS
 	uint8_t spiBufferIndex = 0;			// The index into the SPI buffer
 
 	//Init the packet structures and init them with pointers to the payloadAck, payloadCfg, payloadBuf and payloadAuto arrays
-	ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
-	ubxPacket packetBuf = {0, 0, 0, 0, 0, payloadBuf, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
-	ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
-	ubxPacket packetAuto = {0, 0, 0, 0, 0, payloadAuto, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+	ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+	ubxPacket packetBuf = {0, 0, 0, 0, 0, payloadBuf, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+	ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+	ubxPacket packetAuto = {0, 0, 0, 0, 0, payloadAuto, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
 
 	//Flag if this packet is unrequested (and so should be ignored and not copied into packetCfg or packetAck)
 	boolean ignoreThisPayload = false;

From 87b641cd4270b9c450e82c7f85ddac930bf0fbdc Mon Sep 17 00:00:00 2001
From: Andrew Berridge <aberridg@tibco.com>
Date: Sun, 27 Jun 2021 08:16:41 +0100
Subject: [PATCH 3/9] Removing class/ID match checking from processUBX. All
 checking now done much higher up the call chain

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 81 +++-----------------
 src/SparkFun_u-blox_GNSS_Arduino_Library.h   |  2 +-
 2 files changed, 12 insertions(+), 71 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index 1b615a9..d0372c8 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -1306,13 +1306,13 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
     {
       //Divert incoming into the correct buffer
       if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETACK)
-        processUBX(incoming, &packetAck, requestedClass, requestedID);
+        processUBX(incoming, &packetAck);
       else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)      
-        processUBX(incoming, &packetCfg, requestedClass, requestedID);
+        processUBX(incoming, &packetCfg);
       else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
-        processUBX(incoming, &packetBuf, requestedClass, requestedID);
+        processUBX(incoming, &packetBuf);
       else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
-        processUBX(incoming, &packetAuto, requestedClass, requestedID);
+        processUBX(incoming, &packetAuto);
     }
     //Finally, increment the frame counter
     ubxFrameCounter++;
@@ -1513,7 +1513,7 @@ void SFE_UBLOX_GNSS::processRTCM(uint8_t incoming) // IGNORE COMPILER WARNING un
 //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 packetCfgPayloadSize bytes.
 //startingSpot can be set so we only record a subset of bytes within a larger packet.
-void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID)
+void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)
 {
     //If incomingUBX is a user-defined custom packet, then the payload size could be different to packetCfgPayloadSize.
     //TO DO: update this to prevent an overrun when receiving an automatic message
@@ -1587,54 +1587,6 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
       incomingUBX->valid = SFE_UBLOX_PACKET_VALIDITY_VALID; // Flag the packet as valid
       if (_printDebug) _debugSerial->println("processUBX packet is valid");
 
-      // ACK or NACK???
-      if (incomingUBX->isAckForClassAndId(requestedClass, requestedID)) {
-        if (_printDebug)
-        {
-          _debugSerial->print(F("processUBX: ACK received: Requested Class: 0x"));
-          _debugSerial->print(incomingUBX->payload[0], HEX);
-          _debugSerial->print(F(" Requested ID: 0x"));
-          _debugSerial->println(incomingUBX->payload[1], HEX);
-        }
-      }
-
-      else if (incomingUBX->isAckForClassAndId(requestedClass, requestedID))
-      {        
-        if (_printDebug == true)
-        {
-          _debugSerial->print(F("processUBX: NACK received: Requested Class: 0x"));
-          _debugSerial->print(incomingUBX->payload[0], HEX);
-          _debugSerial->print(F(" Requested ID: 0x"));
-          _debugSerial->println(incomingUBX->payload[1], HEX);
-        }
-      }
-
-      else if (incomingUBX->isClassAndIdMatch(requestedClass, requestedID)) 
-      {
-        if (_printDebug == true)
-        {
-          _debugSerial->print(F("processUBX: Message received for requested Class: 0x"));
-          _debugSerial->print(incomingUBX->payload[0], HEX);
-          _debugSerial->print(F(" Requested ID: 0x"));
-          _debugSerial->println(incomingUBX->payload[1], HEX);
-        }
-      }
-
-      //This is not an ACK and we do not have a complete class and ID match
-      //So let's check for an "automatic" message arriving
-      else if (checkAutomatic(incomingUBX->cls, incomingUBX->id))
-      {
-        // This isn't the message we are looking for...
-        // Let's say so...
-        if (_printDebug == true)
-        {
-          _debugSerial->print(F("processUBX: incoming \"automatic\" message: Class: 0x"));
-          _debugSerial->print(incomingUBX->cls, HEX);
-          _debugSerial->print(F(" ID: 0x"));
-          _debugSerial->println(incomingUBX->id, HEX);
-        }
-      }
-
       if (_printDebug == true)
       {
         _debugSerial->print(F("Incoming: Size: "));
@@ -1643,7 +1595,6 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
         printPacket(incomingUBX);
 
       }
-
       //We've got a valid packet, now do something with it but only if ignoreThisPayload is false
       if (ignoreThisPayload == false)
       {
@@ -1691,9 +1642,13 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_
   {
     //If an automatic packet comes in asynchronously, we need to fudge the startingSpot
     uint16_t startingSpot = incomingUBX->startingSpot;
-    if (!incomingUBX->isClassAndIdMatch(requestedClass, requestedID) && checkAutomatic(incomingUBX->cls, incomingUBX->id))
+    if (checkAutomatic(incomingUBX->cls, incomingUBX->id))
     {
-      //_debugSerial->println("processUBX: incoming is automatic");
+      if(_printDebug == true) 
+      {
+        //_debugSerial->println("processUBX: incoming is automatic");
+      }
+      
       startingSpot = 0;
     }
 
@@ -2979,7 +2934,6 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   {
     if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
     {
-
       // If we are expecting an ACK only, return as soon as we have it!
       if (expectACKonly && packetAck.isAckForClassAndId(requestedClass, requestedID) && packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
       {
@@ -3064,19 +3018,6 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         }
         return (SFE_UBLOX_STATUS_FAIL); //We received invalid data and an invalid ACK!
       }
-
-      // If the config packet is VALID and packetAck does not match the requested class/ID 
-      // then the ACK has not yet been received and we should keep waiting for it
-      else if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) == true) && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false))
-      {
-        // if (_printDebug == true)
-        // {
-        //   _debugSerial->print(F("waitForACKResponse: valid data after "));
-        //   _debugSerial->print(millis() - startTime);
-        //   _debugSerial->println(F(" msec. Waiting for ACK."));
-        // }
-      }
-
     } //checkUbloxInternal == true
 
     delayMicroseconds(500);
diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
index 2aa85a0..e7fc142 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
@@ -632,7 +632,7 @@ class SFE_UBLOX_GNSS
 	void processNMEA(char incoming) __attribute__((weak)); //Given a NMEA character, do something with it. User can overwrite if desired to use something like tinyGPS or MicroNMEA libraries
 	void processRTCMframe(uint8_t incoming); //Monitor the incoming bytes for start and length bytes
 	void processRTCM(uint8_t incoming) __attribute__((weak)); //Given rtcm byte, do something with it. User can overwrite if desired to pipe bytes to radio, internet, etc.
-	void processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID); //Given a character, file it away into the uxb packet structure
+	void processUBX(uint8_t incoming, ubxPacket *incomingUBX); //Given a character, file it away into the uxb packet structure
 	void processUBXpacket(ubxPacket *msg); //Once a packet has been received and validated, identify this packet's class/id and update internal flags
 
 	// Send I2C/Serial/SPI commands to the module

From 4f962e11686c4b7c9b42fe6aa30cde85db10e34f Mon Sep 17 00:00:00 2001
From: Andrew Berridge <aberridg@tibco.com>
Date: Thu, 1 Jul 2021 21:58:44 +0100
Subject: [PATCH 4/9] Fixes for checking for ack packet only

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 30 ++++++++++++++------
 1 file changed, 21 insertions(+), 9 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index e27eb78..014c56d 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -2756,6 +2756,11 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::sendCommand(ubxPacket *outgoingUBX, uint16_t
       if (_printDebug == true)
       {
         _debugSerial->println(F("sendCommand: Waiting for ACK response"));
+        _debugSerial->print(F("expecting cls and id: 0x"));
+        _debugSerial->print(outgoingUBX->cls, HEX);
+        _debugSerial->print(F(" 0x"));
+        _debugSerial->println(outgoingUBX->id, HEX);
+
       }
       retVal = waitForACKResponse(outgoingUBX, outgoingUBX->cls, outgoingUBX->id, maxWait, expectACKonly); //Wait for Ack response
     }
@@ -3029,6 +3034,13 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   packetAuto.valid = SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED;
 
   unsigned long startTime = millis();
+  if (_printDebug == true)
+  {
+    _debugSerial->print(F("waitForACKResponse: requested class and id: "));    
+    _debugSerial->print(requestedClass, HEX);
+    _debugSerial->print(F(" "));
+    _debugSerial->println(requestedID, HEX);
+  }
   while (millis() - startTime < maxTime)
   {
     if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
@@ -3793,7 +3805,7 @@ boolean SFE_UBLOX_GNSS::setPortOutput(uint8_t portID, uint8_t outStreamSettings,
   //payloadCfg is now loaded with current bytes. Change only the ones we need to
   payloadCfg[14] = outStreamSettings; //OutProtocolMask LSB - Set outStream bits
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Configure a given port to input UBX, NMEA, RTCM3 or a combination thereof
@@ -3814,7 +3826,7 @@ boolean SFE_UBLOX_GNSS::setPortInput(uint8_t portID, uint8_t inStreamSettings, u
   //payloadCfg is now loaded with current bytes. Change only the ones we need to
   payloadCfg[12] = inStreamSettings; //InProtocolMask LSB - Set inStream bits
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Changes the I2C address that the u-blox module responds to
@@ -4028,7 +4040,7 @@ boolean SFE_UBLOX_GNSS::configureMessage(uint8_t msgClass, uint8_t msgID, uint8_
   //payloadCfg is now loaded with current bytes. Change only the ones we need to
   payloadCfg[2 + portID] = sendRate; //Send rate is relative to the event a message is registered on. For example, if the rate of a navigation message is set to 2, the message is sent every 2nd navigation solution.
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Enable a given message type, default of 1 per update rate (usually 1 per second)
@@ -4117,7 +4129,7 @@ boolean SFE_UBLOX_GNSS::setSurveyMode(uint8_t mode, uint16_t observationTime, fl
   payloadCfg[30] = svinAccLimit >> 16;
   payloadCfg[31] = svinAccLimit >> 24;
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Begin Survey-In for NEO-M8P
@@ -4179,7 +4191,7 @@ bool SFE_UBLOX_GNSS::setStaticPosition(int32_t ecefXOrLat, int8_t ecefXOrLatHP,
   payloadCfg[17] = ecefYOrLonHP;
   payloadCfg[18] = ecefZOrAltHP;
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 bool SFE_UBLOX_GNSS::setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, int32_t ecefZOrAlt, bool latlong, uint16_t maxWait)
@@ -4382,7 +4394,7 @@ boolean SFE_UBLOX_GNSS::addGeofence(int32_t latitude, int32_t longitude, uint32_
     payloadCfg[54] = currentGeofenceParams->rads[3] >> 16;
     payloadCfg[55] = currentGeofenceParams->rads[3] >> 24;
   }
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Clear all geofences using UBX-CFG-GEOFENCE
@@ -4408,7 +4420,7 @@ boolean SFE_UBLOX_GNSS::clearGeofences(uint16_t maxWait)
 
   currentGeofenceParams->numFences = 0; // Zero the number of geofences currently in use
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Clear the antenna control settings using UBX-CFG-ANT
@@ -4426,7 +4438,7 @@ boolean SFE_UBLOX_GNSS::clearAntPIO(uint16_t maxWait)
   payloadCfg[2] = 0xFF; // Antenna pin configuration: set pinSwitch and pinSCD to 31
   payloadCfg[3] = 0xFF; // Antenna pin configuration: set pinOCD to 31, set reconfig bit
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Returns the combined geofence state using UBX-NAV-GEOFENCE
@@ -4702,7 +4714,7 @@ boolean SFE_UBLOX_GNSS::setDynamicModel(dynModel newDynamicModel, uint16_t maxWa
   packetCfg.len = 36;
   packetCfg.startingSpot = 0;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Get the dynamic platform model using UBX-CFG-NAV5

From d46bd509c5ec66bc1bdcc538fab61a211a4eaf71 Mon Sep 17 00:00:00 2001
From: Andrew Berridge <aberridg@tibco.com>
Date: Fri, 2 Jul 2021 10:12:37 +0100
Subject: [PATCH 5/9] Fixing to support new state management paradigm

---
 .../Example20_SendCustomCommand.ino                           | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino b/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino
index 45c259b..e513d71 100644
--- a/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino
+++ b/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino
@@ -64,7 +64,7 @@ void setup()
   // Let's create our custom packet
   uint8_t customPayload[MAX_PAYLOAD_SIZE]; // This array holds the payload data bytes
   // The next line creates and initialises the packet information which wraps around the payload
-  ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+  ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
 
   // The structure of ubxPacket is:
   // uint8_t cls           : The message Class
@@ -126,7 +126,7 @@ void setup()
   // when sendCommand read the data
 
   // Now we write the custom packet back again to change the setting
-  if (myGNSS.sendCommand(&customCfg, maxWait) != SFE_UBLOX_STATUS_DATA_SENT) // This time we are only expecting an ACK
+  if (myGNSS.sendCommand(&customCfg, maxWait, true) != SFE_UBLOX_STATUS_DATA_SENT) // This time we are only expecting an ACK
   {
     Serial.println(F("sendCommand (set) failed! Freezing."));
     while (1)

From 8c2fbf6082c7f2782c51043dd4daf2cddf89d788 Mon Sep 17 00:00:00 2001
From: PaulZC <pc235603@gmail.com>
Date: Sat, 3 Jul 2021 12:13:26 +0100
Subject: [PATCH 6/9] Add more expectACKonly = true

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 48 ++++++++++----------
 1 file changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index e27eb78..8cb3361 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -3793,7 +3793,7 @@ boolean SFE_UBLOX_GNSS::setPortOutput(uint8_t portID, uint8_t outStreamSettings,
   //payloadCfg is now loaded with current bytes. Change only the ones we need to
   payloadCfg[14] = outStreamSettings; //OutProtocolMask LSB - Set outStream bits
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Configure a given port to input UBX, NMEA, RTCM3 or a combination thereof
@@ -3834,7 +3834,7 @@ boolean SFE_UBLOX_GNSS::setI2CAddress(uint8_t deviceAddress, uint16_t maxWait)
   //payloadCfg is now loaded with current bytes. Change only the ones we need to
   payloadCfg[4] = deviceAddress << 1; //DDC mode LSB
 
-  if (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT) // We are only expecting an ACK
+  if (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT) // We are only expecting an ACK
   {
     //Success! Now change our internal global.
     _gpsI2Caddress = deviceAddress; //Store the I2C address from user
@@ -3875,7 +3875,7 @@ void SFE_UBLOX_GNSS::setSerialRate(uint32_t baudrate, uint8_t uartPort, uint16_t
     _debugSerial->println(((uint32_t)payloadCfg[10] << 16) | ((uint32_t)payloadCfg[9] << 8) | payloadCfg[8]);
   }
 
-  sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait);
+  sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait, true); // We are only expecting an ACK <- TO DO: check this!
   if (_printDebug == true)
   {
     _debugSerial->print(F("setSerialRate: sendCommand returned: "));
@@ -3963,7 +3963,7 @@ boolean SFE_UBLOX_GNSS::factoryDefault(uint16_t maxWait)
   packetCfg.payload[8] = 0xFF; //Set any bit in the loadMask field to discard current config and rebuild from lower non-volatile memory layers
   packetCfg.payload[9] = 0xFF;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Save configuration to BBR / Flash
@@ -3983,7 +3983,7 @@ boolean SFE_UBLOX_GNSS::saveConfiguration(uint16_t maxWait)
   packetCfg.payload[4] = 0xFF; //Set any bit in the saveMask field to save current config to Flash and BBR
   packetCfg.payload[5] = 0xFF;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Save the selected configuration sub-sections to flash and BBR (battery backed RAM)
@@ -4003,7 +4003,7 @@ boolean SFE_UBLOX_GNSS::saveConfigSelective(uint32_t configMask, uint16_t maxWai
   packetCfg.payload[6] = (configMask >> 16) & 0xFF;
   packetCfg.payload[7] = (configMask >> 24) & 0xFF;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Configure a given message type for a given port (UART1, I2C, SPI, etc)
@@ -4028,7 +4028,7 @@ boolean SFE_UBLOX_GNSS::configureMessage(uint8_t msgClass, uint8_t msgID, uint8_
   //payloadCfg is now loaded with current bytes. Change only the ones we need to
   payloadCfg[2 + portID] = sendRate; //Send rate is relative to the event a message is registered on. For example, if the rate of a navigation message is set to 2, the message is sent every 2nd navigation solution.
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Enable a given message type, default of 1 per update rate (usually 1 per second)
@@ -4117,7 +4117,7 @@ boolean SFE_UBLOX_GNSS::setSurveyMode(uint8_t mode, uint16_t observationTime, fl
   payloadCfg[30] = svinAccLimit >> 16;
   payloadCfg[31] = svinAccLimit >> 24;
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Begin Survey-In for NEO-M8P
@@ -4179,7 +4179,7 @@ bool SFE_UBLOX_GNSS::setStaticPosition(int32_t ecefXOrLat, int8_t ecefXOrLatHP,
   payloadCfg[17] = ecefYOrLonHP;
   payloadCfg[18] = ecefZOrAltHP;
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 bool SFE_UBLOX_GNSS::setStaticPosition(int32_t ecefXOrLat, int32_t ecefYOrLon, int32_t ecefZOrAlt, bool latlong, uint16_t maxWait)
@@ -4382,7 +4382,7 @@ boolean SFE_UBLOX_GNSS::addGeofence(int32_t latitude, int32_t longitude, uint32_
     payloadCfg[54] = currentGeofenceParams->rads[3] >> 16;
     payloadCfg[55] = currentGeofenceParams->rads[3] >> 24;
   }
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Clear all geofences using UBX-CFG-GEOFENCE
@@ -4408,7 +4408,7 @@ boolean SFE_UBLOX_GNSS::clearGeofences(uint16_t maxWait)
 
   currentGeofenceParams->numFences = 0; // Zero the number of geofences currently in use
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Clear the antenna control settings using UBX-CFG-ANT
@@ -4426,7 +4426,7 @@ boolean SFE_UBLOX_GNSS::clearAntPIO(uint16_t maxWait)
   payloadCfg[2] = 0xFF; // Antenna pin configuration: set pinSwitch and pinSCD to 31
   payloadCfg[3] = 0xFF; // Antenna pin configuration: set pinOCD to 31, set reconfig bit
 
-  return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return ((sendCommand(&packetCfg, maxWait, true)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Returns the combined geofence state using UBX-NAV-GEOFENCE
@@ -4514,7 +4514,7 @@ boolean SFE_UBLOX_GNSS::powerSaveMode(bool power_save, uint16_t maxWait)
   packetCfg.len = 2;
   packetCfg.startingSpot = 0;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 // Get Power Save Mode
@@ -4588,7 +4588,7 @@ boolean SFE_UBLOX_GNSS::powerOff(uint32_t durationInMs, uint16_t maxWait)
   if (maxWait != 0)
   {
     // check for "not acknowledged" command
-    return (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_COMMAND_NACK);
+    return (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_COMMAND_NACK); // TO DO: check if expectACKonly should be true
   }
   else
   {
@@ -4667,7 +4667,7 @@ boolean SFE_UBLOX_GNSS::powerOffWithInterrupt(uint32_t durationInMs, uint32_t wa
   if (maxWait != 0)
   {
     // check for "not acknowledged" command
-    return (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_COMMAND_NACK);
+    return (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_COMMAND_NACK); // TO DO: check if expectACKonly should be true
   }
   else
   {
@@ -4702,7 +4702,7 @@ boolean SFE_UBLOX_GNSS::setDynamicModel(dynModel newDynamicModel, uint16_t maxWa
   packetCfg.len = 36;
   packetCfg.startingSpot = 0;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Get the dynamic platform model using UBX-CFG-NAV5
@@ -4758,7 +4758,7 @@ boolean SFE_UBLOX_GNSS::enableGNSS(boolean enable, sfe_ublox_gnss_ids_e id, uint
     }
   }
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Check if an individual GNSS system is enabled
@@ -4873,7 +4873,7 @@ boolean SFE_UBLOX_GNSS::setTimePulseParameters(UBX_CFG_TP5_data_t *data, uint16_
   payloadCfg[30] = (data->flags.all >> 16) & 0xFF;
   payloadCfg[31] = (data->flags.all >> 24) & 0xFF;
 
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 // CONFIGURATION INTERFACE (protocol v27 and above)
@@ -5028,7 +5028,7 @@ uint8_t SFE_UBLOX_GNSS::setVal16(uint32_t key, uint16_t value, uint8_t layer, ui
   payloadCfg[9] = value >> 8 * 1;
 
   //Send VALSET command with this key and value
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Given a key, set an 8-bit value
@@ -5058,7 +5058,7 @@ uint8_t SFE_UBLOX_GNSS::setVal8(uint32_t key, uint8_t value, uint8_t layer, uint
   payloadCfg[8] = value; //Value
 
   //Send VALSET command with this key and value
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Given a key, set a 32-bit value
@@ -5091,7 +5091,7 @@ uint8_t SFE_UBLOX_GNSS::setVal32(uint32_t key, uint32_t value, uint8_t layer, ui
   payloadCfg[11] = value >> 8 * 3;
 
   //Send VALSET command with this key and value
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Start defining a new UBX-CFG-VALSET ubxPacket
@@ -5260,7 +5260,7 @@ uint8_t SFE_UBLOX_GNSS::sendCfgValset32(uint32_t key, uint32_t value, uint16_t m
   addCfgValset32(key, value);
 
   //Send VALSET command with this key and value
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Add a final keyID and value to an existing UBX-CFG-VALSET ubxPacket and send it
@@ -5271,7 +5271,7 @@ uint8_t SFE_UBLOX_GNSS::sendCfgValset16(uint32_t key, uint16_t value, uint16_t m
   addCfgValset16(key, value);
 
   //Send VALSET command with this key and value
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //Add a final keyID and value to an existing UBX-CFG-VALSET ubxPacket and send it
@@ -5282,7 +5282,7 @@ uint8_t SFE_UBLOX_GNSS::sendCfgValset8(uint32_t key, uint8_t value, uint16_t max
   addCfgValset8(key, value);
 
   //Send VALSET command with this key and value
-  return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
+  return (sendCommand(&packetCfg, maxWait, true) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
 //=-=-=-=-=-=-=-= "Automatic" Messages =-=-=-=-=-=-=-==-=-=-=-=-=-=-=

From ac07f9e768e9c3baf722e6117aff28e1a3272e77 Mon Sep 17 00:00:00 2001
From: PaulZC <pc235603@gmail.com>
Date: Sat, 3 Jul 2021 13:11:52 +0100
Subject: [PATCH 7/9] Changes to allow custom commands to run correctly

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 26 +++++++++++---------
 src/SparkFun_u-blox_GNSS_Arduino_Library.h   |  2 +-
 2 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index 3af0559..5d99bae 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -1154,6 +1154,8 @@ uint16_t SFE_UBLOX_GNSS::getMaxPayloadSize(uint8_t Class, uint8_t ID)
 //Take a given byte and file it into the proper array
 // Note - AJB - we will ALWAYS use packetBuf to receive incoming, and then process them into the
 // appropriate packet structures
+// Note - PC - we need to make sure that users can still use custom packets (passed via incomingUBX).
+// We can't hardwire this code to packetCfg...
 void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t requestedClass, uint8_t requestedID)
 {
   if ((currentSentence == NONE) || (currentSentence == NMEA))
@@ -1227,9 +1229,9 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
           {
             if (_printDebug) _debugSerial->println("process: activeBuffer PACKETCFG");
             activePacketBuffer = SFE_UBLOX_PACKET_PACKETCFG;
-            packetCfg.cls = packetBuf.cls;
-            packetCfg.id = packetBuf.id;
-            packetCfg.counter = packetBuf.counter;
+            incomingUBX->cls = packetBuf.cls;
+            incomingUBX->id = packetBuf.id;
+            incomingUBX->counter = packetBuf.counter;
           }
           else
           {
@@ -1376,7 +1378,7 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
       if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETACK)
         processUBX(incoming, &packetAck);
       else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETCFG)
-        processUBX(incoming, &packetCfg);
+        processUBX(incoming, incomingUBX);
       else if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETBUF)
         processUBX(incoming, &packetBuf);
       else // if (activePacketBuffer == SFE_UBLOX_PACKET_PACKETAUTO)
@@ -3059,7 +3061,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
 
       // If both the packetCfg->isClassAndIdMatch and packetAck.isClassAndIdMatch
       // then we can be confident that the data in packetCfg is valid
-      if (packetAck.isAckForClassAndId(requestedClass, requestedID) && packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID && packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID && packetCfg.isClassAndIdMatch(requestedClass, requestedID))
+      if (packetAck.isAckForClassAndId(requestedClass, requestedID) && outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID && packetAck.valid == SFE_UBLOX_PACKET_VALIDITY_VALID && outgoingUBX->isClassAndIdMatch(requestedClass, requestedID))
       {
         if (_printDebug == true)
         {
@@ -3069,7 +3071,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
         }
         return (SFE_UBLOX_STATUS_DATA_RECEIVED); //We received valid data and a correct ACK!
       }
-      else if (!packetCfg.isClassAndIdMatch(requestedClass, requestedID) && packetAck.isClassAndIdMatch(requestedClass, requestedID))
+      else if (!outgoingUBX->isClassAndIdMatch(requestedClass, requestedID) && packetAck.isClassAndIdMatch(requestedClass, requestedID))
       {
         if (_printDebug == true)
         {
@@ -3082,7 +3084,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
 
       // If packetAck is for requested class/ID but both outgoingUBX->valid and packetCfg isClassAndIDmatch
       // are NOT_VALID then we can be confident we have had a checksum failure on the data packet
-      else if ((packetAck.isAckForClassAndId(requestedClass, requestedID) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)))
+      else if ((packetAck.isAckForClassAndId(requestedClass, requestedID) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_NOT_VALID)))
       {
         if (_printDebug == true)
         {
@@ -3106,7 +3108,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
       // If the packetCfg.isClassAndIdMatch but the packetAck is not for this class/ID
       // then the ack probably had a checksum error. We will take a gamble and return DATA_RECEIVED.
       // If we were playing safe, we should return FAIL instead
-      else if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID)  && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)))
+      else if ((outgoingUBX->isClassAndIdMatch(requestedClass, requestedID)  && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID)))
       {
         if (_printDebug == true)
         {
@@ -3119,7 +3121,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
 
       // If classes of ack and data do not match
       // then we return a FAIL. This must be a double checksum failure?
-      else if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) == false) && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false))
+      else if ((outgoingUBX->isClassAndIdMatch(requestedClass, requestedID) == false) && (packetAck.isClassAndIdMatch(requestedClass, requestedID) == false))
       {
         if (_printDebug == true)
         {
@@ -3137,7 +3139,7 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForACKResponse(ubxPacket *outgoingUBX, ui
   // We have timed out...
   // If the packetCfg matches the requested class/ID then we can take a gamble and return DATA_RECEIVED
   // even though we did not get an ACK
-  if ((packetCfg.isClassAndIdMatch(requestedClass, requestedID) && !packetAck.isClassAndIdMatch(requestedClass, requestedID)) && (packetCfg.valid == SFE_UBLOX_PACKET_VALIDITY_VALID))
+  if ((outgoingUBX->isClassAndIdMatch(requestedClass, requestedID) && !packetAck.isClassAndIdMatch(requestedClass, requestedID)) && (outgoingUBX->valid == SFE_UBLOX_PACKET_VALIDITY_VALID))
   {
     if (_printDebug == true)
     {
@@ -3174,13 +3176,13 @@ sfe_ublox_status_e SFE_UBLOX_GNSS::waitForNoACKResponse(ubxPacket *outgoingUBX,
   unsigned long startTime = millis();
   while (millis() - startTime < maxTime)
   {
-    if (checkUbloxInternal(&packetBuf, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
+    if (checkUbloxInternal(outgoingUBX, requestedClass, requestedID) == true) //See if new data is available. Process bytes as they come in.
     {
 
       // If packetBuf matches
       // and outgoingUBX->valid is _still_ VALID and the class and ID _still_ match
       // then we can be confident that the data in packetBuf is valid
-      if (packetBuf.isClassAndIdMatch(requestedClass, requestedID) && packetBuf.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
+      if (outgoingUBX->isClassAndIdMatch(requestedClass, requestedID) && packetBuf.valid == SFE_UBLOX_PACKET_VALIDITY_VALID)
       {
         if (_printDebug == true)
         {
diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
index 3137a6a..a7cb79e 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h
@@ -1328,7 +1328,7 @@ class SFE_UBLOX_GNSS
 	//The packet buffers
 	//These are pointed at from within the ubxPacket
 	uint8_t payloadAck[2];				  // Holds the requested ACK/NACK
-	uint8_t payloadBuf[MAX_PAYLOAD_SIZE];				  // Temporary buffer used to screen incoming packets or dump unrequested packets aberridg TODO: figure out if we can reduce memory usage by not using the whole buffer - needs some clean-up
+	uint8_t payloadBuf[MAX_PAYLOAD_SIZE]; // Temporary buffer used to screen incoming packets or dump unrequested packets aberridg TODO: figure out if we can reduce memory usage by not using the whole buffer - needs some clean-up
 	size_t packetCfgPayloadSize = 0; // Size for the packetCfg payload. .begin will set this to MAX_PAYLOAD_SIZE if necessary. User can change with setPacketCfgPayloadSize
 	uint8_t *payloadCfg = NULL;
 	uint8_t *payloadAuto = NULL;

From 4fdb585d1b837f3c8e1a8dc503dc14770fa7fa71 Mon Sep 17 00:00:00 2001
From: PaulZC <pc235603@gmail.com>
Date: Sat, 3 Jul 2021 13:29:03 +0100
Subject: [PATCH 8/9] Update Example20 - also uses custom command

---
 .../Example20_SendCustomCommand.ino                          | 2 +-
 examples/Example21_ModuleInfo/Example21_ModuleInfo.ino       | 5 ++---
 examples/Example3_GetPosition/Example3_GetPosition.ino       | 2 ++
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp                 | 2 +-
 4 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino b/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino
index e513d71..6230c71 100644
--- a/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino
+++ b/examples/Example20_SendCustomCommand/Example20_SendCustomCommand.ino
@@ -76,7 +76,6 @@ void setup()
   // uint8_t checksumA     : Given to us by the module. Checked against the rolling calculated A/B checksums.
   // uint8_t checksumB
   // sfe_ublox_packet_validity_e valid            : Goes from NOT_DEFINED to VALID or NOT_VALID when checksum is checked
-  // sfe_ublox_packet_validity_e classAndIDmatch  : Goes from NOT_DEFINED to VALID or NOT_VALID when the Class and ID match the requestedClass and requestedID
 
   // sendCommand will return:
   // SFE_UBLOX_STATUS_DATA_RECEIVED if the data we requested was read / polled successfully
@@ -124,6 +123,7 @@ void setup()
 
   // We don't need to update customCfg.len as it will have been set to 6
   // when sendCommand read the data
+  customCfg.len = 6;
 
   // Now we write the custom packet back again to change the setting
   if (myGNSS.sendCommand(&customCfg, maxWait, true) != SFE_UBLOX_STATUS_DATA_SENT) // This time we are only expecting an ACK
diff --git a/examples/Example21_ModuleInfo/Example21_ModuleInfo.ino b/examples/Example21_ModuleInfo/Example21_ModuleInfo.ino
index 9349eca..d1a47cb 100644
--- a/examples/Example21_ModuleInfo/Example21_ModuleInfo.ino
+++ b/examples/Example21_ModuleInfo/Example21_ModuleInfo.ino
@@ -67,7 +67,7 @@ void setup()
 
     Wire.begin();
 
-    //myGNSS.enableDebugging(); // Uncomment this line to enable debug messages
+    myGNSS.enableDebugging(); // Uncomment this line to enable debug messages
 
     if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
     {
@@ -120,7 +120,7 @@ boolean SFE_UBLOX_GPS_ADD::getModuleInfo(uint16_t maxWait)
     uint8_t customPayload[MAX_PAYLOAD_SIZE]; // This array holds the payload data bytes
 
     // The next line creates and initialises the packet information which wraps around the payload
-    ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+    ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
 
     // The structure of ubxPacket is:
     // uint8_t cls           : The message Class
@@ -132,7 +132,6 @@ boolean SFE_UBLOX_GPS_ADD::getModuleInfo(uint16_t maxWait)
     // uint8_t checksumA     : Given to us by the module. Checked against the rolling calculated A/B checksums.
     // uint8_t checksumB
     // sfe_ublox_packet_validity_e valid            : Goes from NOT_DEFINED to VALID or NOT_VALID when checksum is checked
-    // sfe_ublox_packet_validity_e classAndIDmatch  : Goes from NOT_DEFINED to VALID or NOT_VALID when the Class and ID match the requestedClass and requestedID
 
     // sendCommand will return:
     // SFE_UBLOX_STATUS_DATA_RECEIVED if the data we requested was read / polled successfully
diff --git a/examples/Example3_GetPosition/Example3_GetPosition.ino b/examples/Example3_GetPosition/Example3_GetPosition.ino
index be605f3..489cc2c 100644
--- a/examples/Example3_GetPosition/Example3_GetPosition.ino
+++ b/examples/Example3_GetPosition/Example3_GetPosition.ino
@@ -43,6 +43,8 @@ void setup()
 
   Wire.begin();
 
+  //myGNSS.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+
   if (myGNSS.begin() == false) //Connect to the u-blox module using Wire port
   {
     Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index 5d99bae..1f91ae4 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -1308,7 +1308,7 @@ void SFE_UBLOX_GNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t r
         {
           //This is not an ACK, nor auto and we do not have a class and ID match
           //so we should keep diverting data into packetBuf and ignore the payload
-          if (_printDebug) _debugSerial->println("IGNORING!!!");
+          if (_printDebug) _debugSerial->println("process: ignoring!");
           ignoreThisPayload = true;
         }
       }

From cdb93d19052ac13b462f4493a83fc97256fb7abf Mon Sep 17 00:00:00 2001
From: PaulZC <pc235603@gmail.com>
Date: Sat, 3 Jul 2021 13:30:29 +0100
Subject: [PATCH 9/9] Change debug printf's to print - ATmega328 does not
 support printf

---
 src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
index 1f91ae4..804f2ff 100644
--- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
@@ -1605,10 +1605,9 @@ void SFE_UBLOX_GNSS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)
       if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
       {
         _debugSerial->print(F("processUBX: getMaxPayloadSize returned ZERO!! Class: 0x"));
-        _debugSerial->printf("%x", incomingUBX->cls);
+        _debugSerial->print(incomingUBX->cls, HEX);
         _debugSerial->print(F(" ID: 0x"));
-        _debugSerial->printf("%x\n", incomingUBX->id);
-
+        _debugSerial->println(incomingUBX->id, HEX);
       }
     }
     //}