AP_Baro: Add in BMP581 Driver

rmackay9 · Jun 16, 2024 · 869ef0e · 869ef0e
1 parent 9135f01
commit 869ef0e
Show file tree

Hide file tree

Showing 8 changed files with 248 additions and 1 deletion.
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
@@ -276,6 +276,7 @@ def __init__(self,
     Feature('Baro', 'BMP085', 'AP_BARO_BMP085_ENABLED', 'Enable BMP085 Barometric Sensor', 1, None),
     Feature('Baro', 'BMP280', 'AP_BARO_BMP280_ENABLED', 'Enable BMP280 Barometric Sensor', 1, None),
     Feature('Baro', 'BMP388', 'AP_BARO_BMP388_ENABLED', 'Enable BMP388 Barometric Sensor', 1, None),
+    Feature('Baro', 'BMP581', 'AP_BARO_BMP581_ENABLED', 'Enable BMP581 Barometric Sensor', 1, None),
     Feature('Baro', 'DPS280', 'AP_BARO_DPS280_ENABLED', 'Enable DPS280/DPS310 Barometric Sensor', 1, None),
     Feature('Baro', 'DUMMY', 'AP_BARO_DUMMY_ENABLED', 'Enable DUMMY Barometric Sensor', 0, None),
     Feature('Baro', 'EXTERNALAHRS', 'AP_BARO_EXTERNALAHRS_ENABLED', 'Enable EXTERNALAHRS Barometric Sensor', 0, 'AHRS_EXT'),

diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py
@@ -126,6 +126,7 @@ def num(s):
     0x12 : "DEVTYPE_BARO_MS5837",
     0x13 : "DEVTYPE_BARO_MS5637",
     0x14 : "DEVTYPE_BARO_BMP390",
+    0x15 : "DEVTYPE_BARO_BMP581",
 }
 
 airspeed_types = {

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
@@ -48,6 +48,7 @@
 #include "AP_Baro_ExternalAHRS.h"
 #include "AP_Baro_ICP101XX.h"
 #include "AP_Baro_ICP201XX.h"
+#include "AP_Baro_BMP581.h"
 
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -177,7 +178,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // @Param: _PROBE_EXT
     // @DisplayName: External barometers to probe
     // @Description: This sets which types of external i2c barometer to look for. It is a bitmask of barometer types. The I2C buses to probe is based on BARO_EXT_BUS. If BARO_EXT_BUS is -1 then it will probe all external buses, otherwise it will probe just the bus number given in BARO_EXT_BUS.
-    // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06,12:MSP
+    // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06,12:MSP,13:BMP581
     // @User: Advanced
     AP_GROUPINFO("_PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT),
 #endif
@@ -781,6 +782,10 @@ void AP_Baro::_probe_i2c_barometers(void)
         { PROBE_BMP388, AP_Baro_BMP388::probe, HAL_BARO_BMP388_I2C_ADDR },
         { PROBE_BMP388, AP_Baro_BMP388::probe, HAL_BARO_BMP388_I2C_ADDR2 },
 #endif
+#if AP_BARO_BMP581_ENABLED
+        { PROBE_BMP581, AP_Baro_BMP581::probe, HAL_BARO_BMP581_I2C_ADDR },
+        { PROBE_BMP581, AP_Baro_BMP581::probe, HAL_BARO_BMP581_I2C_ADDR2 },
+#endif
 #if AP_BARO_MS56XX_ENABLED
         { PROBE_MS5611, AP_Baro_MS56XX::probe_5611, HAL_BARO_MS5611_I2C_ADDR },
         { PROBE_MS5611, AP_Baro_MS56XX::probe_5611, HAL_BARO_MS5611_I2C_ADDR2 },

diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
@@ -266,6 +266,7 @@ class AP_Baro
         PROBE_BMP388=(1<<10),
         PROBE_SPL06 =(1<<11),
         PROBE_MSP   =(1<<12),
+        PROBE_BMP581=(1<<13),
     };
 
 #if HAL_BARO_WIND_COMP_ENABLED

diff --git a/libraries/AP_Baro/AP_Baro_BMP581.cpp b/libraries/AP_Baro/AP_Baro_BMP581.cpp
@@ -0,0 +1,193 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "AP_Baro_BMP581.h"
+
+#if AP_BARO_BMP581_ENABLED
+
+#include <utility>
+#include <AP_Math/AP_Math.h>
+
+extern const AP_HAL::HAL &hal;
+
+#define BMP581_ID                     0x50
+
+#define BMP581_REG_CHIP_ID            0x01
+#define BMP581_REG_REV_ID             0x02
+#define BMP581_REG_CHIP_STATUS        0x11
+#define BMP581_REG_DRIVE_CONFIG       0x13
+#define BMP581_REG_INT_CONFIG         0x14
+#define BMP581_REG_INT_SOURCE         0x15
+#define BMP581_REG_FIFO_CONFIG        0x16
+#define BMP581_REG_FIFO_COUNT         0x17
+#define BMP581_REG_FIFO_SEL           0x18
+#define BMP581_REG_TEMP_DATA_XLSB     0x1D
+#define BMP581_REG_TEMP_DATA_LSB      0x1E
+#define BMP581_REG_TEMP_DATA_MSB      0x1F
+#define BMP581_REG_PRESS_DATA_XLSB    0x20
+#define BMP581_REG_PRESS_DATA_LSB     0x21
+#define BMP581_REG_PRESS_DATA_MSB     0x22
+#define BMP581_REG_INT_STATUS         0x27
+#define BMP581_REG_STATUS             0x28
+#define BMP581_REG_FIFO_DATA          0x29
+#define BMP581_REG_NVM_ADDR           0x2B
+#define BMP581_REG_NVM_DATA_LSB       0x2C
+#define BMP581_REG_NVM_DATA_MSB       0x2D
+#define BMP581_REG_DSP_CONFIG         0x30
+#define BMP581_REG_DSP_IIR            0x31
+#define BMP581_REG_OOR_THR_P_LSB      0x32
+#define BMP581_REG_OOR_THR_P_MSB      0x33
+#define BMP581_REG_OOR_RANGE          0x34
+#define BMP581_REG_OOR_CONFIG         0x35
+#define BMP581_REG_OSR_CONFIG         0x36
+#define BMP581_REG_ODR_CONFIG         0x37
+#define BMP581_REG_OSR_EFF            0x38
+#define BMP581_REG_CMD                0x7E
+
+AP_Baro_BMP581::AP_Baro_BMP581(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
+    : AP_Baro_Backend(baro)
+    , _dev(std::move(dev))
+{
+}
+
+AP_Baro_Backend *AP_Baro_BMP581::probe(AP_Baro &baro,
+                                       AP_HAL::OwnPtr<AP_HAL::Device> dev)
+{
+    if (!dev) {
+        return nullptr;
+    }
+
+    AP_Baro_BMP581 *sensor = NEW_NOTHROW AP_Baro_BMP581(baro, std::move(dev));
+    if (!sensor || !sensor->init()) {
+        delete sensor;
+        return nullptr;
+    }
+    return sensor;
+}
+
+bool AP_Baro_BMP581::init()
+{
+    if (!_dev) {
+        return false;
+    }
+
+    WITH_SEMAPHORE(_dev->get_semaphore());
+
+    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
+
+    uint8_t whoami;
+
+    // setup to allow reads on SPI
+    if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
+        _dev->set_read_flag(0x80);
+
+        if (!_dev->read_registers(BMP581_REG_CHIP_ID, &whoami, 1)) {
+            return false;
+        }
+    }
+
+    if (!_dev->read_registers(BMP581_REG_CHIP_ID, &whoami, 1)) {
+        return false;
+    }
+
+    switch (whoami) {
+    case BMP581_ID:
+        _dev->set_device_type(DEVTYPE_BARO_BMP581);
+        break;
+    default:
+        return false;
+    }
+
+    uint8_t status;
+    if (!_dev->read_registers(BMP581_REG_STATUS, &status, 1)) {
+        return false;
+    }
+
+    if ((status & 0b10) == 0  || (status & 0b100) == 1) {
+        return false;
+    }
+
+    uint8_t int_status;
+    if (!_dev->read_registers(BMP581_REG_INT_STATUS, &int_status, 1)) {
+        return false;
+    }
+
+    if ((int_status & 0x10) == 0) {
+        return false;
+    }
+
+    _dev->setup_checked_registers(4);
+
+    // Standby mode
+    _dev->write_register(BMP581_REG_ODR_CONFIG, 0, true);
+
+    // Press EN | osr_p 64X | osr_t 4X
+    _dev->write_register(BMP581_REG_OSR_CONFIG, 0b01110010, true);
+
+    // ORD 50Hz | Normal Mode
+    _dev->write_register(BMP581_REG_ODR_CONFIG, 0b0111101, true);
+
+    instance = _frontend.register_sensor();
+
+    set_bus_id(instance, _dev->get_bus_id());
+
+    // request 50Hz update
+    _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP581::timer, void));
+
+    return true;
+}
+
+// acumulate a new sensor reading
+void AP_Baro_BMP581::timer(void)
+{
+    uint8_t buf[6];
+
+    if (!_dev->read_registers(BMP581_REG_TEMP_DATA_XLSB, buf, sizeof(buf))) {
+        return;
+    }
+
+    WITH_SEMAPHORE(_sem);
+
+    if (buf[0] != 0x7f || buf[1] != 0x7f || buf[2] != 0x7f) {
+        // we have temperature data
+        temperature = (float)((int32_t)(((uint32_t)buf[2] << 24) | ((uint32_t)buf[1] << 16) | ((uint32_t)buf[0] << 8)) >> 8) * (1.0f / 65536.0f);
+    }
+
+    if (buf[3] != 0x7f || buf[4] != 0x7f || buf[5] != 0x7f) {
+        // we have pressure data
+        pressure_sum += (float)(((uint32_t)buf[5] << 16) | ((uint32_t)buf[4] << 8) | (uint32_t)buf[3]) * (1.0f / 64.0f);
+        pressure_count++;
+    }
+
+    _dev->check_next_register();
+}
+
+// transfer data to the frontend
+void AP_Baro_BMP581::update(void)
+{
+    WITH_SEMAPHORE(_sem);
+
+    if (pressure_count == 0) {
+        return;
+    }
+
+    _copy_to_frontend(instance,
+                      pressure_sum/pressure_count,
+                      temperature);
+
+    pressure_sum = 0;
+    pressure_count = 0;
+}
+
+#endif  // AP_BARO_BMP581_ENABLED
diff --git a/libraries/AP_Baro/AP_Baro_BMP581.h b/libraries/AP_Baro/AP_Baro_BMP581.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "AP_Baro_Backend.h"
+
+#if AP_BARO_BMP581_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Device.h>
+#include <AP_HAL/utility/OwnPtr.h>
+
+#ifndef HAL_BARO_BMP581_I2C_ADDR
+ #define HAL_BARO_BMP581_I2C_ADDR  (0x46)
+#endif
+#ifndef HAL_BARO_BMP581_I2C_ADDR2
+ #define HAL_BARO_BMP581_I2C_ADDR2 (0x47)
+#endif
+
+class AP_Baro_BMP581 : public AP_Baro_Backend
+{
+public:
+    AP_Baro_BMP581(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
+    /* AP_Baro public interface: */
+    void update() override;
+
+    static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
+private:
+
+    bool init(void);
+    void timer(void);
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    uint8_t instance;
+    float pressure_sum;
+    uint32_t pressure_count;
+    float temperature;
+};
+
+#endif  // AP_BARO_BMP581_ENABLED
diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h
@@ -53,6 +53,7 @@ class AP_Baro_Backend
         DEVTYPE_BARO_MS5837   = 0x12,
         DEVTYPE_BARO_MS5637   = 0x13,
         DEVTYPE_BARO_BMP390   = 0x14,
+        DEVTYPE_BARO_BMP581   = 0x15,
     };
 
 protected:

diff --git a/libraries/AP_Baro/AP_Baro_config.h b/libraries/AP_Baro/AP_Baro_config.h
@@ -29,6 +29,10 @@
 #define AP_BARO_BMP388_ENABLED AP_BARO_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_BARO_BMP581_ENABLED
+#define AP_BARO_BMP581_ENABLED AP_BARO_BACKEND_DEFAULT_ENABLED
+#endif
+
 #ifndef AP_BARO_DPS280_ENABLED
 #define AP_BARO_DPS280_ENABLED AP_BARO_BACKEND_DEFAULT_ENABLED
 #endif