From 600df16604f055a67f1f5176a9fab5abea1e2df6 Mon Sep 17 00:00:00 2001
From: ES-Alexander <sandman.esalexander@gmail.com>
Date: Tue, 13 Jul 2021 17:25:51 +1000
Subject: [PATCH] Add pressure offset based on device P-mode
---
libraries/AP_Baro/AP_Baro_KellerLD.cpp | 25 +++++++++++++++++++++++--
1 file changed, 23 insertions(+), 2 deletions(-)
diff --git a/libraries/AP_Baro/AP_Baro_KellerLD.cpp b/libraries/AP_Baro/AP_Baro_KellerLD.cpp
index 9893d80c26..5ba6bb7218 100644
--- a/libraries/AP_Baro/AP_Baro_KellerLD.cpp
+++ b/libraries/AP_Baro/AP_Baro_KellerLD.cpp
@@ -30,6 +30,8 @@
extern const AP_HAL::HAL &hal;
+// sensor metadata register
+static const uint8_t CMD_METADATA_PMODE = 0x12;
// Measurement range registers
static const uint8_t CMD_PRANGE_MIN_MSB = 0x13;
static const uint8_t CMD_PRANGE_MIN_LSB = 0x14;
@@ -81,6 +83,26 @@ bool AP_Baro_KellerLD::_init()
// measurement range, but for some reason this isn't an issue when requesting measurements.
// This is why we need to split the transfers with delays like this.
// (Using AP_HAL::I2CDevice::set_split_transfers will not work with these sensors)
+
+ // Read out sensor P-mode and select relevant pressure offset
+ cal_read_ok &= _dev->transfer(&CMD_METADATA_PMODE, 1, nullptr, 0);
+ hal.scheduler->delay(1);
+ cal_read_ok &= _dev->transfer(nullptr, 0, &data[0], 3);
+ hal.scheduler->delay(1);
+
+ switch (data[2] & 0b11) {
+ case 0:
+ // PR-mode Vented Gauge sensor (zero at 1 atm)
+ _p_mode_offset = 1.01325;
+ break;
+ case 1:
+ // PA-mode Sealed Gauge sensor (zero at 1 bar)
+ _p_mode_offset = 1.0;
+ break;
+ default:
+ // PAA-mode Absolute sensor (zero at vacuum), or undefined mode
+ _p_mode_offset = 0.0;
+ }
// Read out pressure measurement range
cal_read_ok &= _dev->transfer(&CMD_PRANGE_MIN_MSB, 1, nullptr, 0);
@@ -248,9 +270,8 @@ void AP_Baro_KellerLD::update()
uint16_t raw_temperature_avg = sum_temperature / num_samples;
// per datasheet
- float pressure = (raw_pressure_avg - 16384) * (_p_max - _p_min) / 32768 + _p_min;
+ float pressure = (raw_pressure_avg - 16384) * (_p_max - _p_min) / 32768 + _p_min + _p_mode_offset;
pressure *= 100000; // bar -> Pascal
- pressure += 101300; // MSL pressure offset
float temperature = ((raw_temperature_avg >> 4) - 24) * 0.05f - 50;
_copy_to_frontend(_instance, pressure, temperature);