From 8359c082ca62d30372ff36a8c6cc573b82ad70fe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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;