AP_HAL: Add semaphores to I2C driver.

- Guard I2C transactions with this semaphore in the MS5611 and
  HMC5843 drivers.

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -143,6 +143,12 @@ void AP_Baro_MS5611_SPI::sem_give()
 
 void AP_Baro_MS5611_I2C::init()
 {
+    _i2c_sem = hal.i2c->get_semaphore();
+    if (_i2c_sem == NULL) {
+        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get "
+                                  "valid I2C semaphroe!"));
+        return; /* never reached */
+    }
 }
 
 uint16_t AP_Baro_MS5611_I2C::read_16bits(uint8_t reg)
@@ -170,6 +176,36 @@ void AP_Baro_MS5611_I2C::write(uint8_t reg)
     hal.i2c->write(MS5611_ADDR, 1, &reg);
 }
 
+bool AP_Baro_MS5611_I2C::sem_take_blocking() {
+    return _i2c_sem->take(10);
+}
+
+bool AP_Baro_MS5611_I2C::sem_take_nonblocking()
+{
+    /**
+     * Take nonblocking from a TimerProcess context &
+     * monitor for bad failures
+     */
+    static int semfail_ctr = 0;
+    bool got = _i2c_sem->take_nonblocking();
+    if (!got) {
+        semfail_ctr++;
+        if (semfail_ctr > 100) {
+            hal.scheduler->panic(PSTR("PANIC: failed to take _i2c_sem "
+                        "100 times in a row, in AP_Baro_MS5611::_update"));
+        }
+        return false; /* never reached */
+    } else {
+        semfail_ctr = 0;
+    }
+    return got;
+}
+
+void AP_Baro_MS5611_I2C::sem_give()
+{
+    _i2c_sem->give();
+}
+
 // Public Methods //////////////////////////////////////////////////////////////
 
 // SPI should be initialized externally

libraries/AP_Baro/AP_Baro_MS5611.h

@@ -57,6 +57,12 @@ public:
     virtual uint16_t read_16bits(uint8_t reg);
     virtual uint32_t read_adc();
     virtual void write(uint8_t reg);
+    virtual bool sem_take_nonblocking();
+    virtual bool sem_take_blocking();
+    virtual void sem_give();
+
+private:
+    AP_HAL::Semaphore *_i2c_sem;
 };
 
 class AP_Baro_MS5611 : public AP_Baro

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -77,6 +77,7 @@ bool AP_Compass_HMC5843::read_raw()
 			hal.i2c->setHighSpeed(false);
         }
         healthy = false;
+        _i2c_sem->give();
         return false;
     }
 
@@ -110,7 +111,12 @@ void AP_Compass_HMC5843::accumulate(void)
 	  // the compass gets new data at 75Hz
 	  return;
    }
-   if (read_raw()) {
+
+   _i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+   bool result = read_raw();
+   _i2c_sem->give();
+
+   if (result) {
 	  // the _mag_N values are in the range -2048 to 2047, so we can
 	  // accumulate up to 15 of them in an int16_t. Let's make it 14
 	  // for ease of calculation. We expect to do reads at 10Hz, and
@@ -157,10 +163,14 @@ AP_Compass_HMC5843::init()
 
     hal.scheduler->delay(10);
 
+    _i2c_sem = hal.i2c->get_semaphore();
+    _i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER);
+
     // determine if we are using 5843 or 5883L
     if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
         !read_register(ConfigRegA, &_base_config)) {
         healthy = false;
+        _i2c_sem->give();
         return false;
     }
     if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
@@ -174,6 +184,7 @@ AP_Compass_HMC5843::init()
         product_id = AP_COMPASS_TYPE_HMC5843;
     } else {
         // not behaving like either supported compass type
+        _i2c_sem->give();
         return false;
     }
 
@@ -249,9 +260,11 @@ AP_Compass_HMC5843::init()
 
     // leave test mode
     if (!re_initialise()) {
+        _i2c_sem->give();
         return false;
     }
 
+    _i2c_sem->give();
     _initialised = true;
 
 	// perform an initial read

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -2,6 +2,7 @@
 #ifndef AP_Compass_HMC5843_H
 #define AP_Compass_HMC5843_H
 
+#include <AP_HAL.h>
 #include "../AP_Common/AP_Common.h"
 #include "../AP_Math/AP_Math.h"
 
@@ -55,6 +56,7 @@ private:
     bool                read_register(uint8_t address, uint8_t *value);
     bool                write_register(uint8_t address, uint8_t value);
     uint32_t            _retry_time; // when unhealthy the millis() value to retry at
+    AP_HAL::Semaphore*  _i2c_sem;
 
     int16_t			    _mag_x;
     int16_t			    _mag_y;

libraries/AP_HAL/I2CDriver.h

@@ -32,6 +32,7 @@ public:
                                   uint8_t len, uint8_t* data) = 0;
 
     virtual uint8_t lockup_count() = 0;
+    virtual AP_HAL::Semaphore* get_semaphore() = 0;
 };
 
 #endif // __AP_HAL_I2C_DRIVER_H__

libraries/AP_HAL_SMACCM/I2CDriver.cpp

@@ -29,6 +29,7 @@ using namespace SMACCM;
 void SMACCMI2CDriver::begin()
 {
   i2c_init(I2C_BUS, I2C_SDA, I2C_SCL);
+  _semaphore.init();
 }
 
 // XXX hwf4 doesn't support de-initialization
@@ -46,6 +47,11 @@ void SMACCMI2CDriver::setHighSpeed(bool active)
 {
 }
 
+AP_HAL::Semaphore* SMACCMI2CDriver::get_semaphore()
+{
+  return &_semaphore;
+}
+
 uint8_t SMACCMI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
 {
   return i2c_transfer(I2C_BUS, addr, data, len, NULL, 0) ? 0 : 1;

libraries/AP_HAL_SMACCM/I2CDriver.h

@@ -15,6 +15,7 @@
 #define __AP_HAL_SMACCM_I2CDRIVER_H__
 
 #include <AP_HAL_SMACCM.h>
+#include "Semaphores.h"
 
 class SMACCM::SMACCMI2CDriver : public AP_HAL::I2CDriver {
 public:
@@ -42,7 +43,10 @@ public:
                                   uint8_t len, uint8_t* data);
 
     uint8_t lockup_count();
+    AP_HAL::Semaphore* get_semaphore();
 
+private:
+    SMACCMSemaphore _semaphore;
 };
 
 #endif // __AP_HAL_SMACCM_I2CDRIVER_H__