diff --git a/libraries/I2C/I2C.cpp b/libraries/I2C/I2C.cpp
index 13fb0c53bd..5b3f052f55 100644
--- a/libraries/I2C/I2C.cpp
+++ b/libraries/I2C/I2C.cpp
@@ -31,13 +31,8 @@
  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
-#include <inttypes.h>
+#include <AP_Common.h>
 #include "I2C.h"
-#if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
-#else
- #include "WProgram.h"
-#endif
 
 
 
@@ -392,32 +387,56 @@ uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes,
     nack = numberBytes - 1;
     returnStatus = 0;
     returnStatus = start();
-    if(returnStatus) {return(returnStatus); }
+    if (returnStatus) {
+	    _lockup_count++;
+	    return(returnStatus); 
+    }
     returnStatus = sendAddress(SLA_W(address));
-    if(returnStatus) {return(returnStatus); }
+    if(returnStatus) {
+	    _lockup_count++;
+	    return(returnStatus);
+    }
     returnStatus = sendByte(registerAddress);
-    if(returnStatus) {return(returnStatus); }
+    if(returnStatus) {
+	    _lockup_count++;
+	    return(returnStatus); 
+    }
     returnStatus = start();
-    if(returnStatus) {return(returnStatus); }
+    if(returnStatus) {
+	    _lockup_count++;
+	    return(returnStatus); 
+    }
     returnStatus = sendAddress(SLA_R(address));
-    if(returnStatus) {return(returnStatus); }
+    if(returnStatus) {
+	    _lockup_count++;
+	    return(returnStatus); 
+    }
     for(uint8_t i = 0; i < numberBytes; i++)
     {
         if( i == nack )
         {
             returnStatus = receiveByte(0);
-            if(returnStatus != MR_DATA_NACK) {return(returnStatus); }
+            if (returnStatus != MR_DATA_NACK) {
+		    _lockup_count++;
+		    return(returnStatus); 
+	    }
         }
         else
         {
             returnStatus = receiveByte(1);
-            if(returnStatus != MR_DATA_ACK) {return(returnStatus); }
+            if (returnStatus != MR_DATA_ACK) {
+		    _lockup_count++;
+		    return(returnStatus); 
+	    }
         }
         dataBuffer[i] = TWDR;
         bytesAvailable = i+1;
         totalBytes = i+1;
     }
     returnStatus = stop();
+    if (returnStatus) {
+	    _lockup_count++;
+    }
     return(returnStatus);
 }