Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/system.pde

@@ -108,7 +108,9 @@ static void init_ardupilot()
 	//
 #ifndef DESKTOP_BUILD
     I2c.begin();
-    I2c.timeOut(20);
+    I2c.timeOut(5);
+    // initially set a fast I2c speed, and drop it on first failures
+    I2c.setSpeed(true);
 #endif
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

ArduPlane/system.pde

@@ -108,7 +108,9 @@ static void init_ardupilot()
 	//
 #ifndef DESKTOP_BUILD
     I2c.begin();
-    I2c.timeOut(20);
+    I2c.timeOut(5);
+    // initially set a fast I2c speed, and drop it on first failures
+    I2c.setSpeed(true);
 #endif
     SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -152,7 +152,13 @@ void AP_Baro_BMP085::ReadPress()
 {
 	uint8_t buf[3];
 
+    if (!healthy && millis() < _retry_time) {
+        return;
+    }
+
 	if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
+        _retry_time = millis() + 1000;
+        I2c.setSpeed(false);
 		healthy = false;
 		return;
 	}
@@ -197,7 +203,13 @@ void AP_Baro_BMP085::ReadTemp()
 {
 	uint8_t buf[2];
 
+    if (!healthy && millis() < _retry_time) {
+        return;
+    }
+
 	if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
+        _retry_time = millis() + 1000;
+        I2c.setSpeed(false);
 		healthy = false;
 		return;
 	}

libraries/AP_Baro/AP_Baro_BMP085.h

@@ -50,6 +50,7 @@ class AP_Baro_BMP085 : public AP_Baro
 
 	uint8_t	_temp_index;
 	uint8_t	_press_index;
+    uint32_t _retry_time;
 
 	void Command_ReadPress();
 	void Command_ReadTemp();

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -246,11 +246,20 @@ AP_Compass_HMC5843::init()
 // Read Sensor data
 bool AP_Compass_HMC5843::read()
 {
-   if (!healthy && !re_initialise()) {
+   if (!healthy) {
+	  if (millis() < _retry_time) {
 		 return false;
 	  }
+	  if (!re_initialise()) {
+		 _retry_time = millis() + 1000;
+		 return false;
+	  }
+   }
 
    if (!read_raw()) {
+	  // try again in 1 second, and set I2c clock speed slower
+	  _retry_time = millis() + 1000;
+	  I2c.setSpeed(false);
 	  return false;
    }
 

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -53,6 +53,7 @@ class AP_Compass_HMC5843 : public Compass
 	virtual bool re_initialise(void);
     bool read_register(uint8_t address, uint8_t *value);
     bool write_register(uint8_t address, byte value);
+    uint32_t _retry_time; // when unhealthy the millis() value to retry at
 
   public:
 	AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}

libraries/DataFlash/DataFlash_APM2.cpp

@@ -53,8 +53,6 @@ extern "C" {
 	# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
 #endif
 
-#define DF_LAST_PAGE 8192
-
 // AT45DB321D Commands (from Datasheet)
 #define DF_TRANSFER_PAGE_TO_BUFFER_1   0x53
 #define DF_TRANSFER_PAGE_TO_BUFFER_2   0x55
@@ -133,8 +131,23 @@ void DataFlash_APM2::Init(void)
   // get page size: 512 or 528  (by default: 528)
   df_PageSize=PageSize();
 
-  // the last page is reserved for config information
+  ReadManufacturerID();
-  df_NumPages = DF_LAST_PAGE - 1;
+
+  // see page 22 of the spec for the density code
+  uint8_t density_code = (df_device >> 8) & 0x1F;
+  
+  // note that we set df_NumPages to one lower than the highest, as
+  // the last page is reserved for a config page
+  if (density_code == 0x7) {
+      // 32 Mbit
+      df_NumPages = 8191;
+  } else if (density_code == 0x6) {
+      // 16 Mbit
+      df_NumPages = 4095;
+  } else {
+      // what is this??? card not inserted perhaps?
+      df_NumPages = 0;
+  }
 }
 
 // This function is mainly to test the device
@@ -155,7 +168,7 @@ void DataFlash_APM2::ReadManufacturerID()
 // This function return 1 if Card is inserted on SD slot
 bool DataFlash_APM2::CardInserted()
 {
-    return (digitalRead(DF_CARDDETECT) == 0);
+    return (df_NumPages >= 4095 && digitalRead(DF_CARDDETECT) == 0);
 }
 
 // Read the status register