From e217faacaba357b4c5e8e3563b4ddbbdd9435b68 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 25 Jul 2016 13:52:33 -0300 Subject: [PATCH] AP_Baro: MS56XX: cosmetic changes to members - reorder and document members. - remove tentative of vertical alignement - like was done for accumulated values, move the calibration values to a struct --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 36 ++++++++++++++-------------- libraries/AP_Baro/AP_Baro_MS5611.h | 16 ++++++++----- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 809e3c30c8..ad97232490 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -90,12 +90,12 @@ void AP_Baro_MS56XX::_init() } // Save factory calibration coefficients - _c1 = prom[1]; - _c2 = prom[2]; - _c3 = prom[3]; - _c4 = prom[4]; - _c5 = prom[5]; - _c6 = prom[6]; + _cal_reg.c1 = prom[1]; + _cal_reg.c2 = prom[2]; + _cal_reg.c3 = prom[3]; + _cal_reg.c4 = prom[4]; + _cal_reg.c5 = prom[5]; + _cal_reg.c6 = prom[6]; // Send a command to read temperature first _dev->transfer(&ADDR_CMD_CONVERT_TEMPERATURE, 1, nullptr, 0); @@ -328,10 +328,10 @@ void AP_Baro_MS5611::_calculate() // we do the calculations using floating point allows us to take advantage // of the averaging of D1 and D1 over multiple samples, giving us more // precision - dT = _D2-(((uint32_t)_c5)<<8); - TEMP = (dT * _c6)/8388608; - OFF = _c2 * 65536.0f + (_c4 * dT) / 128; - SENS = _c1 * 32768.0f + (_c3 * dT) / 256; + dT = _D2-(((uint32_t)_cal_reg.c5)<<8); + TEMP = (dT * _cal_reg.c6)/8388608; + OFF = _cal_reg.c2 * 65536.0f + (_cal_reg.c4 * dT) / 128; + SENS = _cal_reg.c1 * 32768.0f + (_cal_reg.c3 * dT) / 256; if (TEMP < 0) { // second order temperature compensation when under 20 degrees C @@ -370,10 +370,10 @@ void AP_Baro_MS5607::_calculate() // we do the calculations using floating point allows us to take advantage // of the averaging of D1 and D1 over multiple samples, giving us more // precision - dT = _D2-(((uint32_t)_c5)<<8); - TEMP = (dT * _c6)/8388608; - OFF = _c2 * 131072.0f + (_c4 * dT) / 64; - SENS = _c1 * 65536.0f + (_c3 * dT) / 128; + dT = _D2-(((uint32_t)_cal_reg.c5)<<8); + TEMP = (dT * _cal_reg.c6)/8388608; + OFF = _cal_reg.c2 * 131072.0f + (_cal_reg.c4 * dT) / 64; + SENS = _cal_reg.c1 * 65536.0f + (_cal_reg.c3 * dT) / 128; if (TEMP < 0) { // second order temperature compensation when under 20 degrees C @@ -409,10 +409,10 @@ void AP_Baro_MS5637::_calculate() // 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; + dT = raw_temperature - (((uint32_t)_cal_reg.c5) << 8); + TEMP = 2000 + ((int64_t)dT * (int64_t)_cal_reg.c6) / 8388608; + OFF = (int64_t)_cal_reg.c2 * (int64_t)131072 + ((int64_t)_cal_reg.c4 * (int64_t)dT) / (int64_t)64; + SENS = (int64_t)_cal_reg.c1 * (int64_t)65536 + ((int64_t)_cal_reg.c3 * (int64_t)dT) / (int64_t)128; if (TEMP < 2000) { // second order temperature compensation when under 20 degrees C diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 448b3fe516..acd790c8cf 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -49,12 +49,16 @@ protected: } _accum; uint8_t _state; + uint8_t _instance; uint32_t _last_cmd_usec; + /* Last compensated values from accumulated sample */ + float _D1, _D2; + // Internal calibration registers - uint16_t _c1,_c2,_c3,_c4,_c5,_c6; - float _D1, _D2; - uint8_t _instance; + struct { + uint16_t c1, c2, c3, c4, c5, c6; + } _cal_reg; }; class AP_Baro_MS5611 : public AP_Baro_MS56XX @@ -62,7 +66,7 @@ class AP_Baro_MS5611 : public AP_Baro_MS56XX public: AP_Baro_MS5611(AP_Baro &baro, AP_HAL::OwnPtr dev); protected: - void _calculate(); + void _calculate() override; }; class AP_Baro_MS5607 : public AP_Baro_MS56XX @@ -70,7 +74,7 @@ class AP_Baro_MS5607 : public AP_Baro_MS56XX public: AP_Baro_MS5607(AP_Baro &baro, AP_HAL::OwnPtr dev); protected: - void _calculate(); + void _calculate() override; }; class AP_Baro_MS5637 : public AP_Baro_MS56XX @@ -78,6 +82,6 @@ class AP_Baro_MS5637 : public AP_Baro_MS56XX public: AP_Baro_MS5637(AP_Baro &baro, AP_HAL::OwnPtr dev); protected: - void _calculate(); + void _calculate() override; bool _read_prom(uint16_t prom[8]) override; };