AP_Baro: rework SPL06 to do background updates compliant with the spec.

Pull in some bug fixes from betaflight
ArduPilot · Jan 1, 2025 · 855f8ad · 855f8ad
1 parent 5726c03
commit 855f8ad
Showing 1 changed file with 62 additions and 15 deletions.
diff --git 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<AP_HAL::Device> 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,29 +167,66 @@ 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);
 
 #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