From 7b3d707d23f884e77ce1286e95d6655d53f3aa96 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 1 Jan 2025 16:51:21 +0000 Subject: [PATCH 1/5] 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 | 80 +++++++++++++++++++++++------ 1 file changed, 64 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index 6ca52a2f0286e3..712ec85a81470c 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,35 +167,73 @@ 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 _dev->setup_checked_registers(3, 20); + uint8_t tmp_sensor = (type == Type::SPA06 ? 0 : SPL06_TEMP_USE_EXT_SENSOR); #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, tmp_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 _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_CON_PRE_TEM, true); #else - _dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_USE_EXT_SENSOR | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); + _dev->write_register(SPL06_REG_TEMPERATURE_CFG, tmp_sensor | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true); #endif //AP_BARO_SPL06_BACKGROUND_ENABLE From 8810a7ab90064513dab4498c42a29f607f48bab5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Feb 2025 15:12:10 +0000 Subject: [PATCH 2/5] AP_Baro: identify SPA06 in devid --- libraries/AP_Baro/AP_Baro_Backend.h | 1 + libraries/AP_Baro/AP_Baro_SPL06.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 6b661bb1ec6ae7..5325fb9356de7f 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -54,6 +54,7 @@ class AP_Baro_Backend DEVTYPE_BARO_MS5637 = 0x13, DEVTYPE_BARO_BMP390 = 0x14, DEVTYPE_BARO_BMP581 = 0x15, + DEVTYPE_BARO_SPA06 = 0x16, }; protected: diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index 712ec85a81470c..13365c4023ba90 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -248,7 +248,12 @@ bool AP_Baro_SPL06::_init() _instance = _frontend.register_sensor(); - _dev->set_device_type(DEVTYPE_BARO_SPL06); + if(type == Type::SPA06) { + _dev->set_device_type(DEVTYPE_BARO_SPA06); + } else { + _dev->set_device_type(DEVTYPE_BARO_SPL06); + } + set_bus_id(_instance, _dev->get_bus_id()); // request 50Hz update From 2b19b6b8e05f8f1f68cad803dd468ab04c4b93b3 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Feb 2025 15:12:32 +0000 Subject: [PATCH 3/5] scripts: decode SPA06 --- Tools/scripts/decode_devid.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py index 8841d06d8769e5..a714c2a923f83a 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -129,6 +129,7 @@ def num(s): 0x13 : "DEVTYPE_BARO_MS5637", 0x14 : "DEVTYPE_BARO_BMP390", 0x15 : "DEVTYPE_BARO_BMP581", + 0x16 : "DEVTYPE_BARO_SPA06", } airspeed_types = { From 75a8cb7b0bc679b12e9e3c96a6c9e4c32c8e349d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Feb 2025 15:22:16 +0000 Subject: [PATCH 4/5] AP_Baro: move get_twos_complement() to AP_Math --- libraries/AP_Baro/AP_Baro_SPL06.cpp | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index 13365c4023ba90..cfcc60dfddf40c 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -107,16 +107,6 @@ 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) { @@ -187,7 +177,7 @@ bool AP_Baro_SPL06::_init() #define READ_LENGTH 9 - for (uint8_t i = 0; i < SPL06_CALIB_COEFFS_LEN; ) { + for (uint8_t i = 0; i < ARRAY_SIZE(buf); ) { 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; @@ -224,7 +214,7 @@ bool AP_Baro_SPL06::_init() // setup temperature and pressure measurements _dev->setup_checked_registers(3, 20); - uint8_t tmp_sensor = (type == Type::SPA06 ? 0 : SPL06_TEMP_USE_EXT_SENSOR); + const uint8_t tmp_sensor = (type == Type::SPA06 ? 0 : SPL06_TEMP_USE_EXT_SENSOR); #if AP_BARO_SPL06_BACKGROUND_ENABLE //set rate and oversampling _dev->write_register(SPL06_REG_TEMPERATURE_CFG, tmp_sensor | SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); From 6c076b330eb7101fcb6179a72e0eb3db5ba4d006 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Feb 2025 15:22:33 +0000 Subject: [PATCH 5/5] AP_Math: add get_twos_complement() from betaflight --- libraries/AP_Math/AP_Math.cpp | 12 ++++++++++++ libraries/AP_Math/AP_Math.h | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index e553ade4d941ab..29548d52a0dd75 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -563,3 +563,15 @@ double uint64_to_double_le(const uint64_t& value) memcpy(&out, &value, sizeof(out)); return out; } + +/* + get a twos-complement value from the first 'length' bits of a uint32_t + With thanks to betaflight + */ +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); + } + return raw; +} diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 87e2914831f56c..b3e4acf7348924 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -391,3 +391,9 @@ float int32_to_float_le(const uint32_t& value) WARN_IF_UNUSED; Convert from uint64_t to double without breaking Wstrict-aliasing due to type punning */ double uint64_to_double_le(const uint64_t& value) WARN_IF_UNUSED; + +/* + get a twos-complement value from the first 'length' bits of a uint32_t + With thanks to betaflight + */ +int32_t get_twos_complement(uint32_t raw, uint8_t length) WARN_IF_UNUSED;