Make I2C polling dynamic based on update freq. Add error check on bytes avialable.

This allows us to more agressively check for I2C data as the user increases the update rate.

Also added crazy too big error check on bytes available. On the Artemis, there is sometimes the first bit of the bytes available erroneously set to 1. This causes a very large amount of bytes to be read and many valid packets to not be acted upon.

There are still some dropped packets but getting cleaner at 10Hz.

Author: nseidle

src/SparkFun_Ublox_Arduino_Library.cpp

@@ -211,7 +211,7 @@ boolean SFE_UBLOX_GPS::checkUblox()
 //Returns true if new bytes are available
 boolean SFE_UBLOX_GPS::checkUbloxI2C()
 {
-  if (millis() - lastCheck >= I2C_POLLING_WAIT_MS)
+  if (millis() - lastCheck >= i2cPollingWait)
   {
     //Get the number of bytes available from the module
     uint16_t bytesAvailable = 0;
@@ -241,6 +241,31 @@ boolean SFE_UBLOX_GPS::checkUbloxI2C()
       return (false);
     }
 
+    //Check for bit error
+    //This error is rare but if we incorrectly interpret the first bit of the two 'data available' bytes as 1
+    //then we have far too many bytes to check
+    //Correct back down to
+    if (bytesAvailable & ((uint16_t)1 << 15))
+    {
+      //Clear the MSbit
+      bytesAvailable &= ~((uint16_t)1 << 15);
+
+      if (_printDebug == true)
+      {
+        _debugSerial->print("Bytes available error:");
+        _debugSerial->println(bytesAvailable);
+      }
+    }
+
+    if (bytesAvailable > 100)
+    {
+      if (_printDebug == true)
+      {
+        _debugSerial->print("Bytes available:");
+        _debugSerial->println(bytesAvailable);
+      }
+    }
+
     while (bytesAvailable)
     {
       _i2cPort->beginTransmission(_gpsI2Caddress);
@@ -303,16 +328,6 @@ boolean SFE_UBLOX_GPS::checkUbloxSerial()
 //Take a given byte and file it into the proper array
 void SFE_UBLOX_GPS::process(uint8_t incoming)
 {
-
-  if (_printDebug == true)
-  {
-    //if (currentSentence == NONE && incoming == 0xB5) //UBX binary frames start with 0xB5, aka μ
-    //	_debugSerial->println(); //Show new packet start
-
-    //_debugSerial->print(" ");
-    //_debugSerial->print(incoming, HEX);
-  }
-
   if (currentSentence == NONE || currentSentence == NMEA)
   {
     if (incoming == 0xB5) //UBX binary frames start with 0xB5, aka μ
@@ -510,15 +525,15 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX)
         debugPrintln("Checksum failed. Response too big?");
 
         //Drive an external pin to allow for easier logic analyzation
-        // digitalWrite(2, LOW);
-        // delay(10);
-        // digitalWrite(2, HIGH);
-
-        _debugSerial->print("Received: ");
-        printPacket(incomingUBX);
+        digitalWrite(2, LOW);
+        delay(10);
+        digitalWrite(2, HIGH);
 
         _debugSerial->print("Size: ");
         _debugSerial->print(incomingUBX->len);
+        _debugSerial->print(" Received: ");
+        printPacket(incomingUBX);
+
         _debugSerial->print(" checksumA: ");
         _debugSerial->print(incomingUBX->checksumA);
         _debugSerial->print(" checksumB: ");
@@ -891,7 +906,7 @@ boolean SFE_UBLOX_GPS::waitForResponse(uint8_t requestedClass, uint8_t requested
       }
     }
 
-    delay(1);
+    delayMicroseconds(500);
   }
 
   debugPrintln("waitForResponse timeout");
@@ -1251,6 +1266,9 @@ boolean SFE_UBLOX_GPS::setNavigationFrequency(uint8_t navFreq, uint16_t maxWait)
 {
   //if(updateRate > 40) updateRate = 40; //Not needed: module will correct out of bounds values
 
+  //Adjust the I2C polling timeout based on update rate
+  i2cPollingWait = 1000 / (navFreq * 4); //This is the number of ms to wait between checks for new I2C data
+
   //Query the module for the latest lat/long
   packetCfg.cls = UBX_CLASS_CFG;
   packetCfg.id = UBX_CFG_RATE;

src/SparkFun_Ublox_Arduino_Library.h

@@ -428,7 +428,11 @@ class SFE_UBLOX_GPS
 	ubxPacket packetAck = {0, 0, 0, 0, 0, payloadAck, 0, 0, false};
 	ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, false};
 
-	const uint8_t I2C_POLLING_WAIT_MS = 100; //Limit checking of new characters to every X ms
+	//Limit checking of new data to every X ms
+	//If we are expecting an update every X Hz then we should check every half that amount of time
+	//Otherwise we may block ourselves from seeing new data
+	uint8_t i2cPollingWait = 100; //Default to 100ms. Adjusted when user calls setNavigationFrequency()
+
 	unsigned long lastCheck = 0;
 	boolean autoPVT = false;	//Whether autoPVT is enabled or not
 	boolean commandAck = false; //This goes true after we send a command and it's ack'd