Packet ACK is now checked during sendCommand.

Nathan Seidle · Nathan Seidle · commit 5b51956a67c9 · 2019-12-27T09:46:02.000-07:00
diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp
@@ -237,7 +237,7 @@ boolean SFE_UBLOX_GPS::checkUbloxI2C()
 
     if (bytesAvailable == 0)
     {
-      debugPrintln((char *)"Zero bytes available");
+      debugPrintln((char *)"OK: Zero bytes Zero bytes available");
       lastCheck = millis(); //Put off checking to avoid I2C bus traffic
       return (false);
     }
@@ -368,9 +368,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming)
     else if (ubxFrameCounter == 2)                     //Class
     {
       packetAck.counter = 0;
-      packetAck.valid = false;
       packetCfg.counter = 0;
-      packetCfg.valid = false;
 
       //We can now identify the type of response
       if (incoming == UBX_CLASS_ACK)
@@ -509,14 +507,20 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)
     //Validate this sentence
     if (incomingUBX->checksumA == rollingChecksumA && incomingUBX->checksumB == rollingChecksumB)
     {
+      incomingUBX->valid = true;
+
       if (_printDebug == true)
       {
-        _debugSerial->print(F("Size: "));
+        _debugSerial->print(F("Incoming: Size: "));
         _debugSerial->print(incomingUBX->len);
         _debugSerial->print(F(" Received: "));
         printPacket(incomingUBX);
+        if (packetCfg.valid == true)
+          debugPrintln((char *)"packetCfg now valid");
+        if (packetAck.valid == true)
+          debugPrintln((char *)"packetAck now valid");
       }
-      incomingUBX->valid = true;
+
       processUBXpacket(incomingUBX); //We've got a valid packet, now do something with it
     }
     else
@@ -584,7 +588,7 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
     }
     else if (msg->id == UBX_ACK_NACK && msg->payload[0] == packetCfg.cls && msg->payload[1] == packetCfg.id)
     {
-      //The ack we just received matched the CLS/ID of last packetCfg sent (or received)
+      //The ack we just received matched the CLS/ID of last packetCfg sent
       debugPrintln((char *)"UBX ACK: Not-Acknowledged");
       commandAck = UBX_ACK_NACK;
     }
@@ -700,9 +704,10 @@ boolean SFE_UBLOX_GPS::sendCommand(ubxPacket outgoingUBX, uint16_t maxWait)
 
   if (_printDebug == true)
   {
-    _debugSerial->print(F("Sending: "));
+    _debugSerial->print(F("\nSending: "));
     printPacket(&outgoingUBX);
   }
+
   if (commType == COMM_TYPE_I2C)
   {
     if (!sendI2cCommand(outgoingUBX, maxWait))
@@ -897,6 +902,10 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested
   {
     if (checkUblox() == true) //See if new data is available. Process bytes as they come in.
     {
+      //If we are quering a register the module will send response packet to query as well as a ACK/NACK packet
+      //So first we check that an ACK came through, then the response
+      //If we are sending a new register value, the module will simply ACK/NACK our register value.
+
       if (packetAck.valid == true)
       {
         //If the packet we just sent was a CFG packet then we'll get an ACK
@@ -908,7 +917,7 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested
             _debugSerial->print(millis() - startTime);
             _debugSerial->println(F(" msec"));
           }
-          return (true); //  Received an ACK
+          return (true); //Received an ACK
         }
         else if (commandAck == UBX_ACK_NACK)
         {
@@ -918,14 +927,19 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested
             _debugSerial->print(millis() - startTime);
             _debugSerial->println(F(" msec"));
           }
-          return (false); //  Received a NACK
+          return (false); //Received a NACK
         }
       }
 
       if (packetCfg.valid == true)
       {
-        //Did we receive a config packet that matches the cls/id we requested?
-        if (packetCfg.cls == requestedClass && packetCfg.id == requestedID)
+        if (commandAck == UBX_ACK_NACK)
+        {
+          debugPrintln((char *)"Config valid but command NACK'd");
+          return (false); //Received a NACK. Is this command not known?
+        }
+
+        if (packetCfg.valid == true)
         {
           if (_printDebug == true)
           {
@@ -937,11 +951,11 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested
         }
         else
         {
-          if (_printDebug == true)
-          {
-            _debugSerial->print(F("Packet didn't match CLS/ID"));
-            printPacket(&packetCfg);
-          }
+          //We have an ACK but no valid config packet. We must have
+          //gotten an ACK from sending a new value
+
+          debugPrintln((char *)"New config ACK'd");
+          return (true);
         }
       }
     }
@@ -1491,12 +1505,11 @@ boolean SFE_UBLOX_GPS::setPortOutput(uint8_t portID, uint8_t outStreamSettings,
   if (getPortSettings(portID, maxWait) == false)
     return (false); //Something went wrong. Bail.
 
-  // Let's make sure we wait for the ACK too (sendCommand will have returned as soon as the module sent its response)
-  // This is only required because we are doing two sendCommands in quick succession using the same class and ID
-  waitForResponse(UBX_CLASS_CFG, UBX_CFG_PRT, 100); // But we'll only wait for 100msec max
-
-  //Yes, this is the depreciated way to do it but it's still supported on v27 so it
-  //covers both ZED-F9P (v27) and SAM-M8Q (v18)
+  if (commandAck != UBX_ACK_ACK)
+  {
+    debugPrintln((char *)"setPortOutput failed to ACK");
+    return (false);
+  }
 
   packetCfg.cls = UBX_CLASS_CFG;
   packetCfg.id = UBX_CFG_PRT;
diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h
@@ -291,30 +291,30 @@ class SFE_UBLOX_GPS
 
 	boolean waitForResponse(uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime = 250); //Poll the module until and ack is received
 
-  // getPVT will only return data once in each navigation cycle. By default, that is once per second.
-  // Therefore we should set getPVTmaxWait to slightly longer than that.
-  // If you change the navigation frequency to (e.g.) 4Hz using setNavigationFrequency(4)
-  // then you should use a shorter maxWait for getPVT. 300msec would be about right: getPVT(300)
-  // The same is true for getHPPOSLLH.
-  #define getPVTmaxWait 1100 // Default maxWait for getPVT and all functions which call it
-  #define getHPPOSLLHmaxWait 1100  // Default maxWait for getHPPOSLLH and all functions which call it
-
-	boolean assumeAutoPVT(boolean enabled, boolean implicitUpdate = true); //In case no config access to the GPS is possible and PVT is send cyclically already
-	boolean setAutoPVT(boolean enabled, uint16_t maxWait = 250); //Enable/disable automatic PVT reports at the navigation frequency
-	boolean getPVT(uint16_t maxWait = getPVTmaxWait); //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Retruns true if new PVT is available.
+// getPVT will only return data once in each navigation cycle. By default, that is once per second.
+// Therefore we should set getPVTmaxWait to slightly longer than that.
+// If you change the navigation frequency to (e.g.) 4Hz using setNavigationFrequency(4)
+// then you should use a shorter maxWait for getPVT. 300msec would be about right: getPVT(300)
+// The same is true for getHPPOSLLH.
+#define getPVTmaxWait 1100		// Default maxWait for getPVT and all functions which call it
+#define getHPPOSLLHmaxWait 1100 // Default maxWait for getHPPOSLLH and all functions which call it
+
+	boolean assumeAutoPVT(boolean enabled, boolean implicitUpdate = true);				 //In case no config access to the GPS is possible and PVT is send cyclically already
+	boolean setAutoPVT(boolean enabled, uint16_t maxWait = 250);						 //Enable/disable automatic PVT reports at the navigation frequency
+	boolean getPVT(uint16_t maxWait = getPVTmaxWait);									 //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Retruns true if new PVT is available.
 	boolean setAutoPVT(boolean enabled, boolean implicitUpdate, uint16_t maxWait = 250); //Enable/disable automatic PVT reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update
-	boolean getHPPOSLLH(uint16_t maxWait = getHPPOSLLHmaxWait); //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Retruns true if new PVT is available.
-
-	int32_t getLatitude(uint16_t maxWait = getPVTmaxWait); //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module.
-	int32_t getLongitude(uint16_t maxWait = getPVTmaxWait); //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module.
-	int32_t getAltitude(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above ellipsoid
-	int32_t getAltitudeMSL(uint16_t maxWait = getPVTmaxWait); //Returns the current altitude in mm above mean sea level
-	uint8_t getSIV(uint16_t maxWait = getPVTmaxWait); //Returns number of sats used in fix
-	uint8_t getFixType(uint16_t maxWait = getPVTmaxWait); //Returns the type of fix: 0=no, 3=3D, 4=GNSS+Deadreckoning
+	boolean getHPPOSLLH(uint16_t maxWait = getHPPOSLLHmaxWait);							 //Query module for latest group of datums and load global vars: lat, long, alt, speed, SIV, accuracies, etc. If autoPVT is disabled, performs an explicit poll and waits, if enabled does not block. Retruns true if new PVT is available.
+
+	int32_t getLatitude(uint16_t maxWait = getPVTmaxWait);			  //Returns the current latitude in degrees * 10^-7. Auto selects between HighPrecision and Regular depending on ability of module.
+	int32_t getLongitude(uint16_t maxWait = getPVTmaxWait);			  //Returns the current longitude in degrees * 10-7. Auto selects between HighPrecision and Regular depending on ability of module.
+	int32_t getAltitude(uint16_t maxWait = getPVTmaxWait);			  //Returns the current altitude in mm above ellipsoid
+	int32_t getAltitudeMSL(uint16_t maxWait = getPVTmaxWait);		  //Returns the current altitude in mm above mean sea level
+	uint8_t getSIV(uint16_t maxWait = getPVTmaxWait);				  //Returns number of sats used in fix
+	uint8_t getFixType(uint16_t maxWait = getPVTmaxWait);			  //Returns the type of fix: 0=no, 3=3D, 4=GNSS+Deadreckoning
 	uint8_t getCarrierSolutionType(uint16_t maxWait = getPVTmaxWait); //Returns RTK solution: 0=no, 1=float solution, 2=fixed solution
-	int32_t getGroundSpeed(uint16_t maxWait = getPVTmaxWait); //Returns speed in mm/s
-	int32_t getHeading(uint16_t maxWait = getPVTmaxWait); //Returns heading in degrees * 10^-7
-	uint16_t getPDOP(uint16_t maxWait = getPVTmaxWait); //Returns positional dillution of precision * 10^-2
+	int32_t getGroundSpeed(uint16_t maxWait = getPVTmaxWait);		  //Returns speed in mm/s
+	int32_t getHeading(uint16_t maxWait = getPVTmaxWait);			  //Returns heading in degrees * 10^-7
+	uint16_t getPDOP(uint16_t maxWait = getPVTmaxWait);				  //Returns positional dillution of precision * 10^-2
 	uint16_t getYear(uint16_t maxWait = getPVTmaxWait);
 	uint8_t getMonth(uint16_t maxWait = getPVTmaxWait);
 	uint8_t getDay(uint16_t maxWait = getPVTmaxWait);
@@ -393,10 +393,10 @@ class SFE_UBLOX_GPS
 
 	//Support for geofences
 	boolean addGeofence(int32_t latitude, int32_t longitude, uint32_t radius, byte confidence = 0, byte pinPolarity = 0, byte pin = 0, uint16_t maxWait = 1100); // Add a new geofence
-	boolean clearGeofences(uint16_t maxWait = 1100); //Clears all geofences
-	boolean getGeofenceState(geofenceState &currentGeofenceState, uint16_t maxWait = 1100); //Returns the combined geofence state
-	boolean clearAntPIO(uint16_t maxWait = 1100); //Clears the antenna control pin settings to release the PIOs
-	geofenceParams currentGeofenceParams; // Global to store the geofence parameters
+	boolean clearGeofences(uint16_t maxWait = 1100);																											 //Clears all geofences
+	boolean getGeofenceState(geofenceState &currentGeofenceState, uint16_t maxWait = 1100);																		 //Returns the combined geofence state
+	boolean clearAntPIO(uint16_t maxWait = 1100);																												 //Clears the antenna control pin settings to release the PIOs
+	geofenceParams currentGeofenceParams;																														 // Global to store the geofence parameters
 
 	boolean powerSaveMode(bool power_save = true, uint16_t maxWait = 1100);
 

Original file line number	Diff line number	Diff line change
`@@ -237,7 +237,7 @@ boolean SFE_UBLOX_GPS::checkUbloxI2C()`
`237`	`237`
`238`	`238`	`if (bytesAvailable == 0)`
`239`	`239`	`{`
`240`		`- debugPrintln((char *)"Zero bytes available");`
	`240`	`+ debugPrintln((char *)"OK: Zero bytes Zero bytes available");`
`241`	`241`	`lastCheck = millis(); //Put off checking to avoid I2C bus traffic`
`242`	`242`	`return (false);`
`243`	`243`	`}`
`@@ -368,9 +368,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming)`
`368`	`368`	`else if (ubxFrameCounter == 2) //Class`
`369`	`369`	`{`
`370`	`370`	`packetAck.counter = 0;`
`371`		`- packetAck.valid = false;`
`372`	`371`	`packetCfg.counter = 0;`
`373`		`- packetCfg.valid = false;`
`374`	`372`
`375`	`373`	`//We can now identify the type of response`
`376`	`374`	`if (incoming == UBX_CLASS_ACK)`
`@@ -509,14 +507,20 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)`
`509`	`507`	`//Validate this sentence`
`510`	`508`	`if (incomingUBX->checksumA == rollingChecksumA && incomingUBX->checksumB == rollingChecksumB)`
`511`	`509`	`{`
	`510`	`+ incomingUBX->valid = true;`
	`511`	`+`
`512`	`512`	`if (_printDebug == true)`
`513`	`513`	`{`
`514`		`- _debugSerial->print(F("Size: "));`
	`514`	`+ _debugSerial->print(F("Incoming: Size: "));`
`515`	`515`	`_debugSerial->print(incomingUBX->len);`
`516`	`516`	`_debugSerial->print(F(" Received: "));`
`517`	`517`	`printPacket(incomingUBX);`
	`518`	`+ if (packetCfg.valid == true)`
	`519`	`+ debugPrintln((char *)"packetCfg now valid");`
	`520`	`+ if (packetAck.valid == true)`
	`521`	`+ debugPrintln((char *)"packetAck now valid");`
`518`	`522`	`}`
`519`		`- incomingUBX->valid = true;`
	`523`	`+`
`520`	`524`	`processUBXpacket(incomingUBX); //We've got a valid packet, now do something with it`
`521`	`525`	`}`
`522`	`526`	`else`
`@@ -584,7 +588,7 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)`
`584`	`588`	`}`
`585`	`589`	`else if (msg->id == UBX_ACK_NACK && msg->payload[0] == packetCfg.cls && msg->payload[1] == packetCfg.id)`
`586`	`590`	`{`
`587`		`- //The ack we just received matched the CLS/ID of last packetCfg sent (or received)`
	`591`	`+ //The ack we just received matched the CLS/ID of last packetCfg sent`
`588`	`592`	`debugPrintln((char *)"UBX ACK: Not-Acknowledged");`
`589`	`593`	`commandAck = UBX_ACK_NACK;`
`590`	`594`	`}`
`@@ -700,9 +704,10 @@ boolean SFE_UBLOX_GPS::sendCommand(ubxPacket outgoingUBX, uint16_t maxWait)`
`700`	`704`
`701`	`705`	`if (_printDebug == true)`
`702`	`706`	`{`
`703`		`- _debugSerial->print(F("Sending: "));`
	`707`	`+ _debugSerial->print(F("\nSending: "));`
`704`	`708`	`printPacket(&outgoingUBX);`
`705`	`709`	`}`
	`710`	`+`
`706`	`711`	`if (commType == COMM_TYPE_I2C)`
`707`	`712`	`{`
`708`	`713`	`if (!sendI2cCommand(outgoingUBX, maxWait))`
`@@ -897,6 +902,10 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested`
`897`	`902`	`{`
`898`	`903`	`if (checkUblox() == true) //See if new data is available. Process bytes as they come in.`
`899`	`904`	`{`
	`905`	`+ //If we are quering a register the module will send response packet to query as well as a ACK/NACK packet`
	`906`	`+ //So first we check that an ACK came through, then the response`
	`907`	`+ //If we are sending a new register value, the module will simply ACK/NACK our register value.`
	`908`	`+`
`900`	`909`	`if (packetAck.valid == true)`
`901`	`910`	`{`
`902`	`911`	`//If the packet we just sent was a CFG packet then we'll get an ACK`
`@@ -908,7 +917,7 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested`
`908`	`917`	`_debugSerial->print(millis() - startTime);`
`909`	`918`	`_debugSerial->println(F(" msec"));`
`910`	`919`	`}`
`911`		`- return (true); // Received an ACK`
	`920`	`+ return (true); //Received an ACK`
`912`	`921`	`}`
`913`	`922`	`else if (commandAck == UBX_ACK_NACK)`
`914`	`923`	`{`
`@@ -918,14 +927,19 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested`
`918`	`927`	`_debugSerial->print(millis() - startTime);`
`919`	`928`	`_debugSerial->println(F(" msec"));`
`920`	`929`	`}`
`921`		`- return (false); // Received a NACK`
	`930`	`+ return (false); //Received a NACK`
`922`	`931`	`}`
`923`	`932`	`}`
`924`	`933`
`925`	`934`	`if (packetCfg.valid == true)`
`926`	`935`	`{`
`927`		`- //Did we receive a config packet that matches the cls/id we requested?`
`928`		`- if (packetCfg.cls == requestedClass && packetCfg.id == requestedID)`
	`936`	`+ if (commandAck == UBX_ACK_NACK)`
	`937`	`+ {`
	`938`	`+ debugPrintln((char *)"Config valid but command NACK'd");`
	`939`	`+ return (false); //Received a NACK. Is this command not known?`
	`940`	`+ }`
	`941`	`+`
	`942`	`+ if (packetCfg.valid == true)`
`929`	`943`	`{`
`930`	`944`	`if (_printDebug == true)`
`931`	`945`	`{`
`@@ -937,11 +951,11 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested`
`937`	`951`	`}`
`938`	`952`	`else`
`939`	`953`	`{`
`940`		`- if (_printDebug == true)`
`941`		`- {`
`942`		`- _debugSerial->print(F("Packet didn't match CLS/ID"));`
`943`		`- printPacket(&packetCfg);`
`944`		`- }`
	`954`	`+ //We have an ACK but no valid config packet. We must have`
	`955`	`+ //gotten an ACK from sending a new value`
	`956`	`+`
	`957`	`+ debugPrintln((char *)"New config ACK'd");`
	`958`	`+ return (true);`
`945`	`959`	`}`
`946`	`960`	`}`
`947`	`961`	`}`
`@@ -1491,12 +1505,11 @@ boolean SFE_UBLOX_GPS::setPortOutput(uint8_t portID, uint8_t outStreamSettings,`
`1491`	`1505`	`if (getPortSettings(portID, maxWait) == false)`
`1492`	`1506`	`return (false); //Something went wrong. Bail.`
`1493`	`1507`
`1494`		`- // Let's make sure we wait for the ACK too (sendCommand will have returned as soon as the module sent its response)`
`1495`		`- // This is only required because we are doing two sendCommands in quick succession using the same class and ID`
`1496`		`- waitForResponse(UBX_CLASS_CFG, UBX_CFG_PRT, 100); // But we'll only wait for 100msec max`
`1497`		`-`
`1498`		`- //Yes, this is the depreciated way to do it but it's still supported on v27 so it`
`1499`		`- //covers both ZED-F9P (v27) and SAM-M8Q (v18)`
	`1508`	`+ if (commandAck != UBX_ACK_ACK)`
	`1509`	`+ {`
	`1510`	`+ debugPrintln((char *)"setPortOutput failed to ACK");`
	`1511`	`+ return (false);`
	`1512`	`+ }`
`1500`	`1513`
`1501`	`1514`	`packetCfg.cls = UBX_CLASS_CFG;`
`1502`	`1515`	`packetCfg.id = UBX_CFG_PRT;`