Merge pull request #59 from sparkfun/BetterNACKHandling

Faster version polling

Author: nseidle

src/SparkFun_Ublox_Arduino_Library.cpp

@@ -42,6 +42,7 @@ SFE_UBLOX_GPS::SFE_UBLOX_GPS(void)
 {
   // Constructor
   currentGeofenceParams.numFences = 0; // Zero the number of geofences currently in use
+  moduleQueried.versionNumber = false;
 }
 
 //Initialize the Serial port
@@ -575,8 +576,14 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
     if (msg->id == UBX_ACK_ACK && msg->payload[0] == packetCfg.cls && msg->payload[1] == packetCfg.id)
     {
       //The ack we just received matched the CLS/ID of last packetCfg sent
-      debugPrintln((char *)"Command sent/ack'd successfully");
-      commandAck = true;
+      debugPrintln((char *)"UBX ACK: Command sent/ack'd successfully");
+      commandAck = UBX_ACK_ACK;
+    }
+    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
+      debugPrintln((char *)"UBX ACK: Not-Acknowledged");
+      commandAck = UBX_ACK_NACK;
     }
     break;
 
@@ -685,7 +692,7 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
 //Given a packet and payload, send everything including CRC bytes via I2C port
 boolean SFE_UBLOX_GPS::sendCommand(ubxPacket outgoingUBX, uint16_t maxWait)
 {
-  commandAck = false;         //We're about to send a command. Begin waiting for ack.
+  commandAck = UBX_ACK_NONE;  //We're about to send a command. Begin waiting for ack.
   calcChecksum(&outgoingUBX); //Sets checksum A and B bytes of the packet
 
   if (_printDebug == true)
@@ -878,16 +885,24 @@ void SFE_UBLOX_GPS::printPacket(ubxPacket *packet)
 //Poll the module until and ack is received
 boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requestedID, uint16_t maxTime)
 {
-  commandAck = false;      //Reset flag
-  packetCfg.valid = false; //This will go true when we receive a response to the packet we sent
+  commandAck = UBX_ACK_NONE; //Reset flag
+  packetCfg.valid = false;   //This will go true when we receive a response to the packet we sent
+  packetAck.valid = false;
 
   unsigned long startTime = millis();
   while (millis() - startTime < maxTime)
   {
     if (checkUblox() == true) //See if new data is available. Process bytes as they come in.
     {
-      if (commandAck == true)
-        return (true); //If the packet we just sent was a CFG packet then we'll get an ACK
+      if (packetAck.valid == true)
+      {
+        //If the packet we just sent was a CFG packet then we'll get an ACK
+        if (commandAck == UBX_ACK_ACK)
+          return (true); //  Received an ACK
+        else if (commandAck == UBX_ACK_NACK)
+          return (false); //  Received a NACK
+      }
+
       if (packetCfg.valid == true)
       {
         //Did we receive a config packet that matches the cls/id we requested?
@@ -2252,7 +2267,6 @@ uint8_t SFE_UBLOX_GPS::getProtocolVersionHigh(uint16_t maxWait)
 {
   if (moduleQueried.versionNumber == false)
     getProtocolVersion(maxWait);
-  moduleQueried.versionNumber = false;
   return (versionHigh);
 }
 
@@ -2262,7 +2276,6 @@ uint8_t SFE_UBLOX_GPS::getProtocolVersionLow(uint16_t maxWait)
 {
   if (moduleQueried.versionNumber == false)
     getProtocolVersion(maxWait);
-  moduleQueried.versionNumber = false;
   return (versionLow);
 }
 
@@ -2274,46 +2287,44 @@ boolean SFE_UBLOX_GPS::getProtocolVersion(uint16_t maxWait)
   packetCfg.cls = UBX_CLASS_MON;
   packetCfg.id = UBX_MON_VER;
 
-  //We will send the command repeatedly, increasing the startingSpot as we go
-  //Then we look at each extension field of 30 bytes
-  for (uint8_t extensionNumber = 0; extensionNumber < 10; extensionNumber++)
-  {
-    packetCfg.len = 0;
-    packetCfg.startingSpot = 40 + (30 * extensionNumber);
+  packetCfg.len = 0;
+  packetCfg.startingSpot = 40; //Start at first "extended software information" string
+
+  if (sendCommand(packetCfg, maxWait) == false)
+    return (false); //If command send fails then bail
 
-    if (sendCommand(packetCfg, maxWait) == false)
-      return (false); //If command send fails then 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 multiple sendCommands in quick succession using the same class and ID
+  waitForResponse(UBX_CLASS_MON, UBX_MON_VER, 100); // But we'll only wait for 100msec max
 
-    // 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 multiple sendCommands in quick succession using the same class and ID
-    waitForResponse(UBX_CLASS_MON, UBX_MON_VER, 100); // But we'll only wait for 100msec max
+  //Payload should now contain ~220 characters (depends on module type)
 
-    if (_printDebug == true)
+  if (_printDebug == true)
+  {
+    _debugSerial->print(F("MON VER Payload:"));
+    for (int location = 0; location < packetCfg.len; location++)
     {
-      _debugSerial->print(F("Extension "));
-      _debugSerial->print(extensionNumber);
-      _debugSerial->print(F(": "));
-      for (int location = 0; location < MAX_PAYLOAD_SIZE; location++)
-      {
-        if (payloadCfg[location] == '\0')
-          break;
-        _debugSerial->write(payloadCfg[location]);
-      }
-      _debugSerial->println();
+      if (location % 30 == 0)
+        _debugSerial->println();
+      _debugSerial->write(payloadCfg[location]);
     }
+    _debugSerial->println();
+  }
 
+  //We will step through the payload looking at each extension field of 30 bytes
+  for (uint8_t extensionNumber = 0; extensionNumber < 10; extensionNumber++)
+  {
     //Now we need to find "PROTVER=18.00" in the incoming byte stream
-    if (payloadCfg[0] == 'P' && payloadCfg[6] == 'R')
+    if (payloadCfg[(30 * extensionNumber) + 0] == 'P' && payloadCfg[(30 * extensionNumber) + 6] == 'R')
     {
-      versionHigh = (payloadCfg[8] - '0') * 10 + (payloadCfg[9] - '0');  //Convert '18' to 18
-      versionLow = (payloadCfg[11] - '0') * 10 + (payloadCfg[12] - '0'); //Convert '00' to 00
-      return (true);                                                     // This function returns a boolean (so we can't return versionLow)
+      versionHigh = (payloadCfg[(30 * extensionNumber) + 8] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 9] - '0');  //Convert '18' to 18
+      versionLow = (payloadCfg[(30 * extensionNumber) + 11] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 12] - '0'); //Convert '00' to 00
+      moduleQueried.versionNumber = true;                                                                                  //Mark this data as new
+      return (true);                                                                                                       //Success!
     }
   }
 
-  moduleQueried.versionNumber = true; //Mark this data as new
-
-  return (true);
+  return (false); //We failed
 }
 
 //Relative Positioning Information in NED frame

src/SparkFun_Ublox_Arduino_Library.h

@@ -149,6 +149,7 @@ const uint8_t UBX_RTCM_1230 = 0xE6; //GLONASS code-phase biases, set to once eve
 
 const uint8_t UBX_ACK_NACK = 0x00;
 const uint8_t UBX_ACK_ACK = 0x01;
+const uint8_t UBX_ACK_NONE = 0x02; //Not a real value
 
 //The following consts are used to configure the various ports and streams for those ports. See -CFG-PRT.
 const uint8_t COM_PORT_I2C = 0;
@@ -204,7 +205,7 @@ enum dynModel // Possible values for the dynamic platform model
 
 #ifndef MAX_PAYLOAD_SIZE
 
-#define MAX_PAYLOAD_SIZE 128
+#define MAX_PAYLOAD_SIZE 256 //We need ~220 bytes for getProtocolVersion on most ublox modules
 //#define MAX_PAYLOAD_SIZE 768 //Worst case: UBX_CFG_VALSET packet with 64 keyIDs each with 64 bit values
 
 #endif
@@ -367,9 +368,9 @@ class SFE_UBLOX_GPS
 
 	uint32_t getPositionAccuracy(uint16_t maxWait = 500); //Returns the 3D accuracy of the current high-precision fix, in mm. Supported on NEO-M8P, ZED-F9P,
 
-	uint8_t getProtocolVersionHigh(uint16_t maxWait = 1000); //Returns the PROTVER XX.00 from UBX-MON-VER register
-	uint8_t getProtocolVersionLow(uint16_t maxWait = 1000);  //Returns the PROTVER 00.XX from UBX-MON-VER register
-	boolean getProtocolVersion(uint16_t maxWait = 1000);	 //Queries module, loads low/high bytes
+	uint8_t getProtocolVersionHigh(uint16_t maxWait = 500); //Returns the PROTVER XX.00 from UBX-MON-VER register
+	uint8_t getProtocolVersionLow(uint16_t maxWait = 500);  //Returns the PROTVER 00.XX from UBX-MON-VER register
+	boolean getProtocolVersion(uint16_t maxWait = 500);		//Queries module, loads low/high bytes
 
 	boolean getRELPOSNED(uint16_t maxWait = 1000); //Get Relative Positioning Information of the NED frame
 
@@ -521,7 +522,7 @@ class SFE_UBLOX_GPS
 	unsigned long lastCheck = 0;
 	boolean autoPVT = false;			  //Whether autoPVT is enabled or not
 	boolean autoPVTImplicitUpdate = true; // Whether autoPVT is triggered by accessing stale data (=true) or by a call to checkUblox (=false)
-	boolean commandAck = false;			  //This goes true after we send a command and it's ack'd
+	uint8_t commandAck = UBX_ACK_NONE;	//This goes to UBX_ACK_ACK after we send a command and it's ack'd
 	uint16_t ubxFrameCounter;			  //It counts all UBX frame. [Fixed header(2bytes), CLS(1byte), ID(1byte), length(2bytes), payload(x bytes), checksums(2bytes)]
 
 	uint8_t rollingChecksumA; //Rolls forward as we receive incoming bytes. Checked against the last two A/B checksum bytes