From 8359c082ca62d30372ff36a8c6cc573b82ad70fe Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 6 Jan 2015 16:28:11 +1100
Subject: [PATCH] AP_Baro: fixed baro on NavIO

don't use the 1kHz timer as it conflicts with other I2C devices
---
 libraries/AP_Baro/AP_Baro.cpp        |  5 +++--
 libraries/AP_Baro/AP_Baro_MS5611.cpp | 29 +++++++++++++++++++++++++---
 libraries/AP_Baro/AP_Baro_MS5611.h   |  5 ++++-
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 65d50197e3..9e63ea2527 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -248,14 +248,15 @@ void AP_Baro::init(void)
     }
 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611
     {
-        drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(MS5611_I2C_ADDR));
+        drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(MS5611_I2C_ADDR), false);
         _num_drivers = 1;
     }
 #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI
     {
         drivers[0] = new AP_Baro_MS5611(*this, 
                                         new AP_SerialBus_SPI(AP_HAL::SPIDevice_MS5611, 
-                                                             AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH));
+                                                             AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH),
+                                        true);
         _num_drivers = 1;
     }
 #endif    
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp
index b314e7ba6e..123d1d9331 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.cpp
+++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp
@@ -154,12 +154,13 @@ void AP_SerialBus_I2C::sem_give()
 /*
   constructor
  */
-AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial) :
+AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) :
     AP_Baro_Backend(baro),
     _serial(serial),
     _updated(false),
     _state(0),
-    _last_timer(0)
+    _last_timer(0),
+    _use_timer(use_timer)
 {
     _instance = _frontend.register_sensor();
     _serial->init();
@@ -195,7 +196,9 @@ AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial) :
 
     _serial->sem_give();
 
-    hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_timer));
+    if (_use_timer) {
+        hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_timer));
+    }
 }
 
 /**
@@ -310,6 +313,12 @@ void AP_Baro_MS5611::_timer(void)
 
 void AP_Baro_MS5611::update()
 {
+    if (!_use_timer) {
+        // if we're not using the timer then accumulate one more time
+        // to cope with the calibration loop and minimise lag
+        accumulate();
+    }
+
     if (!_updated) {
         return;
     }
@@ -370,3 +379,17 @@ void AP_Baro_MS5611::_calculate()
     float temperature = (TEMP + 2000) * 0.01f;
     _copy_to_frontend(_instance, pressure, temperature);
 }
+
+/*
+  Read the sensor from main code. This is only used for I2C MS5611 to
+  avoid conflicts on the semaphore from calling it in a timer, which
+  conflicts with the compass driver use of I2C
+*/
+void AP_Baro_MS5611::accumulate(void)
+{
+    if (!_use_timer) {
+        // the timer isn't being called as a timer, so we need to call
+        // it in accumulate()
+        _timer();
+    }
+}
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h
index abebe2d519..bbfbbb1edb 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.h
+++ b/libraries/AP_Baro/AP_Baro_MS5611.h
@@ -76,8 +76,9 @@ private:
 class AP_Baro_MS5611 : public AP_Baro_Backend
 {
 public:
-    AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial);
+    AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial, bool use_timer);
     void update();
+    void accumulate();
 
 private:
     AP_SerialBus *_serial;
@@ -96,6 +97,8 @@ private:
     uint8_t                  _state;
     uint32_t                 _last_timer;
 
+    bool _use_timer;
+
     // Internal calibration registers
     uint16_t                 C1,C2,C3,C4,C5,C6;
     float                    D1,D2;