From 3cccafd35d42d0a880d3ead1050920d03b02eee9 Mon Sep 17 00:00:00 2001 From: cuav-chen2 Date: Thu, 6 Jun 2024 10:31:11 +0800 Subject: [PATCH] AP_Baro: Add in BMP581 Driver --- Tools/scripts/build_options.py | 1 + Tools/scripts/decode_devid.py | 1 + libraries/AP_Baro/AP_Baro.cpp | 7 +- libraries/AP_Baro/AP_Baro.h | 1 + libraries/AP_Baro/AP_Baro_BMP581.cpp | 193 +++++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_BMP581.h | 41 ++++++ libraries/AP_Baro/AP_Baro_Backend.h | 1 + libraries/AP_Baro/AP_Baro_config.h | 4 + 8 files changed, 248 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Baro/AP_Baro_BMP581.cpp create mode 100644 libraries/AP_Baro/AP_Baro_BMP581.h diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index ebb2248eaf..55eea3e84c 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -269,6 +269,7 @@ BUILD_OPTIONS = [ 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 index 04cf8b1738..2ff1b999b6 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -125,6 +125,7 @@ baro_types = { 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 index 84bfbadc9f..6910faf21e 100644 --- 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 #include @@ -173,7 +174,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 @@ -835,6 +836,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 index 4b1055fff9..5e50597ec8 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -244,6 +244,7 @@ private: 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 new file mode 100644 index 0000000000..aa8ff86695 --- /dev/null +++ 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 . + */ +#include "AP_Baro_BMP581.h" + +#if AP_BARO_BMP581_ENABLED + +#include +#include + +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 dev) + : AP_Baro_Backend(baro) + , _dev(std::move(dev)) +{ +} + +AP_Baro_Backend *AP_Baro_BMP581::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + + AP_Baro_BMP581 *sensor = new 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 new file mode 100644 index 0000000000..0c50f1ab98 --- /dev/null +++ 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 +#include +#include + +#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 dev); + + /* AP_Baro public interface: */ + void update() override; + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); + +private: + + bool init(void); + void timer(void); + + AP_HAL::OwnPtr _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 index 94b155e483..2652f8f556 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -58,6 +58,7 @@ public: 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 index 0da6757a73..a5c9e0db2f 100644 --- 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