From d9931b5f34b4015e6b5811852d103431cb381f68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 28 Sep 2015 15:31:12 -0300 Subject: [PATCH] AP_Baro: Add support to MS5637 As AVR2560 is not supported anymore and do integer operations is usually faster than float-point the _calculate() implementation was done using only integer operations and as more close to what datasheet says. --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 39 ++++++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_MS5611.h | 8 ++++++ 2 files changed, 47 insertions(+) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 04d225bde5..2facc5e6a2 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -449,6 +449,45 @@ void AP_Baro_MS5607::_calculate() _copy_to_frontend(_instance, pressure, temperature); } +/* MS563 Class */ +AP_Baro_MS5637::AP_Baro_MS5637(AP_Baro &baro, AP_SerialBus *serial, bool use_timer) + : AP_Baro_MS56XX(baro, serial, use_timer) +{ +} + +// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100). +void AP_Baro_MS5637::_calculate() +{ + int32_t dT, TEMP; + int64_t OFF, SENS; + int32_t raw_pressure = _D1; + int32_t raw_temperature = _D2; + + // Formulas from manufacturer datasheet + // sub -15c temperature compensation is not included + + dT = raw_temperature - (((uint32_t)_C5) << 8); + TEMP = 2000 + ((int64_t)dT * (int64_t)_C6) / 8388608; + OFF = (int64_t)_C2 * (int64_t)131072 + ((int64_t)_C4 * (int64_t)dT) / (int64_t)64; + SENS = (int64_t)_C1 * (int64_t)65536 + ((int64_t)_C3 * (int64_t)dT) / (int64_t)128; + + if (TEMP < 2000) { + // second order temperature compensation when under 20 degrees C + int32_t T2 = ((int64_t)3 * ((int64_t)dT * (int64_t)dT) / (int64_t)8589934592); + int64_t aux = (TEMP - 2000) * (TEMP - 2000); + int64_t OFF2 = 61 * aux / 16; + int64_t SENS2 = 29 * aux / 16; + + TEMP = TEMP - T2; + OFF = OFF - OFF2; + SENS = SENS - SENS2; + } + + int32_t pressure = ((int64_t)raw_pressure * SENS / (int64_t)2097152 - OFF) / (int64_t)32768; + float temperature = TEMP * 0.01f; + _copy_to_frontend(_instance, (float)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 diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 629f7fc0ee..9f11d80785 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -119,4 +119,12 @@ public: private: void _calculate(); }; + +class AP_Baro_MS5637 : public AP_Baro_MS56XX +{ +public: + AP_Baro_MS5637(AP_Baro &baro, AP_SerialBus *serial, bool use_timer); +private: + void _calculate(); +}; #endif // __AP_BARO_MS5611_H__