From 855f8ad69a54cf48918cd2bb0b73239d54859d11 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 1 Jan 2025 16:51:21 +0000 Subject: [PATCH] AP_Baro: rework SPL06 to do background updates compliant with the spec. Pull in some bug fixes from betaflight --- libraries/AP_Baro/AP_Baro_SPL06.cpp | 77 +++++++++++++++++++++++------ 1 file changed, 62 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index 6ca52a2f0286e..e13c9bc8d020b 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -79,7 +79,7 @@ extern const AP_HAL::HAL &hal; // enable Background Mode for continuous measurement #ifndef AP_BARO_SPL06_BACKGROUND_ENABLE -#define AP_BARO_SPL06_BACKGROUND_ENABLE 0 +#define AP_BARO_SPL06_BACKGROUND_ENABLE 1 #endif AP_Baro_SPL06::AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr dev) @@ -107,6 +107,16 @@ AP_Baro_Backend *AP_Baro_SPL06::probe(AP_Baro &baro, return sensor; } +static int32_t get_twos_complement(uint32_t raw, uint8_t length) +{ + if (raw & ((int)1 << (length - 1))) { + return ((int32_t)raw) - ((int32_t)1 << length); + } + else { + return raw; + } +} + bool AP_Baro_SPL06::_init() { if (!_dev) { @@ -157,21 +167,58 @@ bool AP_Baro_SPL06::_init() break; } + bool ready = false; + for (uint8_t i=0; i<5; i++) { + uint8_t status = 0; + if (_dev->read_registers(SPL06_REG_MODE_AND_STATUS, &status, 1)) { + if ((status & 1<<7U) && (status & 1<<6U)) { + ready = true; + break; + } + } + hal.scheduler->delay_microseconds(100); + } + + if (!ready) { + return false; + } + uint8_t buf[SPL06_CALIB_COEFFS_LEN]; - _dev->read_registers(SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf)); - - _c0 = (buf[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[0] << 4) | (((uint16_t)buf[1] & 0xF0) >> 4); - _c1 = ((buf[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[1] & 0x0F) << 8) | (uint16_t)buf[2]; - _c00 = (buf[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] & 0xF0) >> 4); - _c10 = (buf[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7]; - _c01 = ((uint16_t)buf[8] << 8) | ((uint16_t)buf[9]); - _c11 = ((uint16_t)buf[10] << 8) | (uint16_t)buf[11]; - _c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13]; - _c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15]; - _c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17]; + +#define READ_LENGTH 9 + + for (uint8_t i = 0; i < SPL06_CALIB_COEFFS_LEN; ) { + ssize_t chunk = MIN(READ_LENGTH, SPL06_CALIB_COEFFS_LEN - i); + if (!_dev->read_registers(SPL06_REG_CALIB_COEFFS_START + i, buf + i, chunk)) { + return false; + } + i += chunk; + } + + // 0x11 c0 [3:0] + 0x10 c0 [11:4] + _c0 = get_twos_complement(((uint32_t)buf[0] << 4) | (((uint32_t)buf[1] >> 4) & 0x0F), 12); + // 0x11 c1 [11:8] + 0x12 c1 [7:0] + _c1 = get_twos_complement((((uint32_t)buf[1] & 0x0F) << 8) | (uint32_t)buf[2], 12); + // 0x13 c00 [19:12] + 0x14 c00 [11:4] + 0x15 c00 [3:0] + _c00 = get_twos_complement(((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] >> 4) & 0x0F), 20); + // 0x15 c10 [19:16] + 0x16 c10 [15:8] + 0x17 c10 [7:0] + _c10 = get_twos_complement((((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7], 20); + // 0x18 c01 [15:8] + 0x19 c01 [7:0] + _c01 = get_twos_complement(((uint32_t)buf[8] << 8) | (uint32_t)buf[9], 16); + // 0x1A c11 [15:8] + 0x1B c11 [7:0] + _c11 = get_twos_complement(((uint32_t)buf[10] << 8) | (uint32_t)buf[11], 16); + // 0x1C c20 [15:8] + 0x1D c20 [7:0] + _c20 = get_twos_complement(((uint32_t)buf[12] << 8) | (uint32_t)buf[13], 16); + // 0x1E c21 [15:8] + 0x1F c21 [7:0] + _c21 = get_twos_complement(((uint32_t)buf[14] << 8) | (uint32_t)buf[15], 16); + // 0x20 c30 [15:8] + 0x21 c30 [7:0] + _c30 = get_twos_complement(((uint32_t)buf[16] << 8) | (uint32_t)buf[17], 16); + if(type == Type::SPA06) { - _c31 = (buf[18] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[18] << 4) | (((uint16_t)buf[19] & 0xF0) >> 4); - _c40 = ((buf[19] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[19] & 0x0F) << 8) | (uint16_t)buf[20]; + // 0x23 c31 [3:0] + 0x22 c31 [11:4] + _c31 = get_twos_complement(((uint32_t)buf[18] << 4) | (((uint32_t)buf[19] >> 4) & 0x0F), 12); + // 0x23 c40 [11:8] + 0x24 c40 [7:0] + _c40 = get_twos_complement((((uint32_t)buf[19] & 0x0F) << 8) | (uint32_t)buf[20], 12); } // setup temperature and pressure measurements @@ -179,7 +226,7 @@ bool AP_Baro_SPL06::_init() #if AP_BARO_SPL06_BACKGROUND_ENABLE //set rate and oversampling - _dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); + _dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_USE_EXT_SENSOR | SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_PRES_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true); //enable background mode