From a19e55e83c18daa172b4e101fc7e24235423fc68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Jul 2019 16:55:30 +1000 Subject: [PATCH] AP_Baro: added BMP388 driver --- libraries/AP_Baro/AP_Baro.cpp | 13 +- libraries/AP_Baro/AP_Baro.h | 1 + libraries/AP_Baro/AP_Baro_BMP388.cpp | 214 +++++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_BMP388.h | 81 ++++++++++ 4 files changed, 307 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_Baro/AP_Baro_BMP388.cpp create mode 100644 libraries/AP_Baro/AP_Baro_BMP388.h diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 7055200811..8b0198d8fc 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -40,6 +40,7 @@ #include "AP_Baro_LPS2XH.h" #include "AP_Baro_FBM320.h" #include "AP_Baro_DPS280.h" +#include "AP_Baro_BMP388.h" #if HAL_WITH_UAVCAN #include "AP_Baro_UAVCAN.h" #endif @@ -157,8 +158,8 @@ 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 GND_EXT_BUS. If GND_EXT_BUS is -1 then it will probe all external buses, otherwise it will probe just the bus number given in GND_EXT_BUS. - // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller - // @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller + // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,10:MS5837,11:BMP388 + // @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388 // @User: Advanced AP_GROUPINFO("PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT), #endif @@ -666,6 +667,14 @@ void AP_Baro::_probe_i2c_barometers(void) std::move(hal.i2c_mgr->get_device(i, HAL_BARO_BMP280_I2C_ADDR2)))); } } + if (probe & PROBE_BMP388) { + FOREACH_I2C_MASK(i,mask) { + ADD_BACKEND(AP_Baro_BMP388::probe(*this, + std::move(hal.i2c_mgr->get_device(i, HAL_BARO_BMP388_I2C_ADDR)))); + ADD_BACKEND(AP_Baro_BMP388::probe(*this, + std::move(hal.i2c_mgr->get_device(i, HAL_BARO_BMP388_I2C_ADDR2)))); + } + } if (probe & PROBE_MS5611) { FOREACH_I2C_MASK(i,mask) { ADD_BACKEND(AP_Baro_MS56XX::probe(*this, diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 9268064b18..fd3cda7e01 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -214,6 +214,7 @@ private: PROBE_LPS25H=(1<<7), PROBE_KELLER=(1<<8), PROBE_MS5837=(1<<9), + PROBE_BMP388=(1<<10), }; struct sensor { diff --git a/libraries/AP_Baro/AP_Baro_BMP388.cpp b/libraries/AP_Baro/AP_Baro_BMP388.cpp new file mode 100644 index 0000000000..b32ab9329c --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_BMP388.cpp @@ -0,0 +1,214 @@ +/* + 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_BMP388.h" + +#include + +extern const AP_HAL::HAL &hal; + +#define BMP388_MODE_SLEEP 0 +#define BMP388_MODE_FORCED 1 +#define BMP388_MODE_NORMAL 3 +#define BMP388_MODE BMP388_MODE_NORMAL + +#define BMP388_ID 0x50 + +#define BMP388_REG_ID 0x00 +#define BMP388_REG_ERR 0x02 +#define BMP388_REG_STATUS 0x03 +#define BMP388_REG_PRESS 0x04 // 24 bit +#define BMP388_REG_TEMP 0x07 // 24 bit +#define BMP388_REG_TIME 0x0C // 24 bit +#define BMP388_REG_EVENT 0x10 +#define BMP388_REG_INT_STS 0x11 +#define BMP388_REG_FIFO_LEN 0x12 // 9 bit +#define BMP388_REG_FIFO_DATA 0x14 +#define BMP388_REG_FIFO_WTMK 0x15 // 9 bit +#define BMP388_REG_FIFO_CNF1 0x17 +#define BMP388_REG_FIFO_CNF2 0x18 +#define BMP388_REG_INT_CTRL 0x19 +#define BMP388_REG_PWR_CTRL 0x1B +#define BMP388_REG_OSR 0x1C +#define BMP388_REG_ODR 0x1D +#define BMP388_REG_CONFIG 0x1F +#define BMP388_REG_CMD 0x7E + +#define BMP388_REG_CAL_P 0x36 +#define BMP388_REG_CAL_T 0x31 + +AP_Baro_BMP388::AP_Baro_BMP388(AP_Baro &baro, AP_HAL::OwnPtr _dev) + : AP_Baro_Backend(baro) + , dev(std::move(_dev)) +{ +} + +AP_Baro_Backend *AP_Baro_BMP388::probe(AP_Baro &baro, + AP_HAL::OwnPtr _dev) +{ + if (!_dev) { + return nullptr; + } + + AP_Baro_BMP388 *sensor = new AP_Baro_BMP388(baro, std::move(_dev)); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_BMP388::init() +{ + if (!dev) { + return false; + } + WITH_SEMAPHORE(dev->get_semaphore()); + + has_sample = false; + + dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + // setup to allow reads on SPI + if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { + dev->set_read_flag(0x80); + } + + // normal mode, temp and pressure + dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true); + + uint8_t whoami; + if (!dev->read_registers(BMP388_REG_ID, &whoami, 1) || + whoami != BMP388_ID) { + // not a BMP388 + return false; + } + + // read the calibration data + dev->read_registers(BMP388_REG_CAL_P, (uint8_t *)&calib_p, sizeof(calib_p)); + dev->read_registers(BMP388_REG_CAL_T, (uint8_t *)&calib_t, sizeof(calib_t)); + + scale_calibration_data(); + + dev->setup_checked_registers(4); + + // normal mode, temp and pressure + dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true); + + instance = _frontend.register_sensor(); + + // request 50Hz update + dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP388::timer, void)); + + return true; +} + + + +// acumulate a new sensor reading +void AP_Baro_BMP388::timer(void) +{ + uint8_t buf[7]; + + if (!dev->read_registers(BMP388_REG_STATUS, buf, sizeof(buf))) { + return; + } + const uint8_t status = buf[0]; + if ((status & 0x20) != 0) { + // we have pressure data + update_pressure((buf[3] << 16) | (buf[2] << 8) | buf[1]); + } + if ((status & 0x40) != 0) { + // we have temperature data + update_temperature((buf[6] << 16) | (buf[5] << 8) | buf[4]); + } + + dev->check_next_register(); +} + +// transfer data to the frontend +void AP_Baro_BMP388::update(void) +{ + WITH_SEMAPHORE(_sem); + + if (!has_sample) { + return; + } + + _copy_to_frontend(instance, pressure, temperature); + has_sample = false; +} + +/* + convert calibration data from NVM values to values ready for + compensation calculations + */ +void AP_Baro_BMP388::scale_calibration_data(void) +{ + // note that this assumes little-endian MCU + calib.par_t1 = calib_t.nvm_par_t1 * 256.0; + calib.par_t2 = calib_t.nvm_par_t2 / 1073741824.0f; + calib.par_t3 = calib_t.nvm_par_t3 / 281474976710656.0f; + + calib.par_p1 = (calib_p.nvm_par_p1 - 16384) / 1048576.0f; + calib.par_p2 = (calib_p.nvm_par_p2 - 16384) / 536870912.0f; + calib.par_p3 = calib_p.nvm_par_p3 / 4294967296.0f; + calib.par_p4 = calib_p.nvm_par_p4 / 137438953472.0; + calib.par_p5 = calib_p.nvm_par_p5 * 8.0f; + calib.par_p6 = calib_p.nvm_par_p6 / 64.0; + calib.par_p7 = calib_p.nvm_par_p7 / 256.0f; + calib.par_p8 = calib_p.nvm_par_p8 / 32768.0f; + calib.par_p9 = calib_p.nvm_par_p9 / 281474976710656.0f; + calib.par_p10 = calib_p.nvm_par_p10 / 281474976710656.0f; + calib.par_p11 = calib_p.nvm_par_p11 / 36893488147419103232.0f; +} + +/* + update temperature from raw sample + */ +void AP_Baro_BMP388::update_temperature(uint32_t data) +{ + float partial1 = data - calib.par_t1; + float partial2 = partial1 * calib.par_t2; + + WITH_SEMAPHORE(_sem); + temperature = partial2 + sq(partial1) * calib.par_t3; +} + +/* + update pressure from raw pressure data + */ +void AP_Baro_BMP388::update_pressure(uint32_t data) +{ + float partial1 = calib.par_p6 * temperature; + float partial2 = calib.par_p7 * powf(temperature, 2); + float partial3 = calib.par_p8 * powf(temperature, 3); + float partial_out1 = calib.par_p5 + partial1 + partial2 + partial3; + + partial1 = calib.par_p2 * temperature; + partial2 = calib.par_p3 * powf(temperature, 2); + partial3 = calib.par_p4 * powf(temperature, 3); + float partial_out2 = data * (calib.par_p1 + partial1 + partial2 + partial3); + + partial1 = powf(data, 2); + partial2 = calib.par_p9 + calib.par_p10 * temperature; + partial3 = partial1 * partial2; + float partial4 = partial3 + powf(data, 3) * calib.par_p11; + float press = partial_out1 + partial_out2 + partial4; + + WITH_SEMAPHORE(_sem); + + pressure = press; + has_sample = true; +} diff --git a/libraries/AP_Baro/AP_Baro_BMP388.h b/libraries/AP_Baro/AP_Baro_BMP388.h new file mode 100644 index 0000000000..edba1d5b52 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_BMP388.h @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include + +#include "AP_Baro_Backend.h" + +#ifndef HAL_BARO_BMP388_I2C_ADDR + #define HAL_BARO_BMP388_I2C_ADDR (0x76) +#endif +#ifndef HAL_BARO_BMP388_I2C_ADDR2 + #define HAL_BARO_BMP388_I2C_ADDR2 (0x77) +#endif + +class AP_Baro_BMP388 : public AP_Baro_Backend +{ +public: + AP_Baro_BMP388(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); + void update_temperature(uint32_t); + void update_pressure(uint32_t); + + AP_HAL::OwnPtr dev; + + bool has_sample; + uint8_t instance; + float pressure; + float temperature; + + // Internal calibration registers + struct PACKED { + int16_t nvm_par_p1; // at 0x36 + int16_t nvm_par_p2; + int8_t nvm_par_p3; + int8_t nvm_par_p4; + int16_t nvm_par_p5; + int16_t nvm_par_p6; + int8_t nvm_par_p7; + int8_t nvm_par_p8; + int16_t nvm_par_p9; + int8_t nvm_par_p10; + int8_t nvm_par_p11; + } calib_p; + + struct PACKED { + uint16_t nvm_par_t1; // at 0x31 + uint16_t nvm_par_t2; + int8_t nvm_par_t3; + } calib_t; + + // scaled calibration data + struct { + float par_t1; + float par_t2; + float par_t3; + float par_p1; + float par_p2; + float par_p3; + float par_p4; + float par_p5; + float par_p6; + float par_p7; + float par_p8; + float par_p9; + float par_p10; + float par_p11; + float t_lin; + } calib; + + void scale_calibration_data(void); +};