AP_Compass: when I2c fails, don't retry for 1s

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()) {
-	  return false;
+   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) {}