From 057eb5e6f61d2a5fe231be0c350bcc99a4d35715 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 Jul 2012 13:44:02 +1000 Subject: [PATCH] Baro: average over all pressure/temperature samples on MS5611 this allows us to use the MS5611 barometer at its full 100Hz sample rate (80Hz for pressure, 20Hz for temperature). The pressure and temperature values are averaged between reads without adding any latency. Previously the driver would throw away values between readings This also fixes a race condition in reading from the SPI bus that could lead to bad values from the barometer --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 80 ++++++++++++++----- libraries/AP_Baro/AP_Baro_MS5611.h | 6 +- .../AP_Baro_MS5611_test.pde | 6 +- 3 files changed, 68 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 3a043e6ade..1e9ccbba72 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -32,6 +32,7 @@ */ +#include #include #include "AP_Baro_MS5611.h" @@ -51,12 +52,14 @@ #define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling) #define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling) -uint32_t AP_Baro_MS5611::_s_D1; -uint32_t AP_Baro_MS5611::_s_D2; +uint32_t volatile AP_Baro_MS5611::_s_D1; +uint32_t volatile AP_Baro_MS5611::_s_D2; +uint8_t volatile AP_Baro_MS5611::_d1_count; +uint8_t volatile AP_Baro_MS5611::_d2_count; uint8_t AP_Baro_MS5611::_state; uint32_t AP_Baro_MS5611::_timer; bool AP_Baro_MS5611::_sync_access; -bool AP_Baro_MS5611::_updated; +bool volatile AP_Baro_MS5611::_updated; uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg) { @@ -132,13 +135,21 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler ) //Send a command to read Temp first _spi_write(CMD_CONVERT_D2_OSR4096); _timer = micros(); - _state = 1; + _state = 0; Temp=0; Press=0; + _s_D1 = 0; + _s_D2 = 0; + _d1_count = 0; + _d2_count = 0; + scheduler->resume_timer(); scheduler->register_process( AP_Baro_MS5611::_update ); + // wait for at least one value to be read + while (!_updated) ; + healthy = true; return true; } @@ -160,20 +171,36 @@ void AP_Baro_MS5611::_update(uint32_t tnow) _timer = tnow; - if (_state == 1) { - _s_D2 = _spi_read_adc(); // On state 1 we read temp + if (_state == 0) { + _s_D2 += _spi_read_adc(); // On state 0 we read temp + _d2_count++; + if (_d2_count == 32) { + // we have summed 128 values. This only happens + // when we stop reading the barometer for a long time + // (more than 1.2 seconds) + _s_D2 >>= 1; + _d2_count = 16; + } _state++; _spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure - } else if (_state == 5) { - _s_D1 = _spi_read_adc(); - _state = 1; // Start again from state = 1 - _spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature - _updated = true; // New pressure reading } else { - _s_D1 = _spi_read_adc(); + _s_D1 += _spi_read_adc(); + _d1_count++; + if (_d1_count == 128) { + // we have summed 128 values. This only happens + // when we stop reading the barometer for a long time + // (more than 1.2 seconds) + _s_D1 >>= 1; + _d1_count = 64; + } _state++; - _spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure - _updated = true; // New pressure reading + _updated = true; // New pressure reading + if (_state == 5) { + _spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature + _state = 0; + } else { + _spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure + } } } @@ -181,10 +208,24 @@ uint8_t AP_Baro_MS5611::read() { _sync_access = true; bool updated = _updated; - _updated = 0; - if (updated > 0) { - D1 = _s_D1; - D2 = _s_D2; + if (updated) { + uint32_t sD1, sD2; + uint8_t d1count, d2count; + // we need to disable interrupts to access + // _s_D1 and _s_D2 as they are not atomic + cli(); + sD1 = _s_D1; _s_D1 = 0; + sD2 = _s_D2; _s_D2 = 0; + d1count = _d1_count; _d1_count = 0; + d2count = _d2_count; _d2_count = 0; + _updated = false; + sei(); + if (d1count != 0) { + D1 = sD1 / d1count; + } + if (d2count != 0) { + D2 = sD2 / d2count; + } _raw_press = D1; _raw_temp = D2; } @@ -209,7 +250,8 @@ void AP_Baro_MS5611::_calculate() // as per data sheet some intermediate results require over 32 bits, therefore // we define parameters as 64 bits to prevent overflow on operations // sub -20c temperature compensation is not included - dT = D2-((long)C5*256); + // Serial.printf("D1=%lu D2=%lu\n", (unsigned long)D1, (unsigned long)D2); + dT = D2-((int32_t)C5*256); TEMP = 2000 + ((int64_t)dT * C6)/8388608; OFF = (int64_t)C2 * 65536 + ((int64_t)C4 * dT ) / 128; SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256; diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 2da8430bc0..b045071c41 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -23,8 +23,10 @@ class AP_Baro_MS5611 : public AP_Baro /* Asynchronous handler functions: */ static void _update(uint32_t ); /* Asynchronous state: */ - static bool _updated; - static uint32_t _s_D1, _s_D2; + static volatile bool _updated; + static volatile uint8_t _d1_count; + static volatile uint8_t _d2_count; + static volatile uint32_t _s_D1, _s_D2; static uint8_t _state; static uint32_t _timer; /* Gates access to asynchronous state: */ diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde index 902bb0102d..52dd030a6c 100644 --- a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde @@ -34,7 +34,7 @@ void setup() SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later baro.init(&scheduler); - timer = micros(); + timer = millis(); } void loop() @@ -42,8 +42,8 @@ void loop() float tmp_float; float Altitude; - if((micros()- timer) > 50000L){ - timer = micros(); + if((millis() - timer) > 100){ + timer = millis(); baro.read(); unsigned long read_time = micros() - timer; if (!baro.healthy) {