AP_Baro: added BMP388 driver

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
+    // @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
+    // @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,

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 {

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 <http://www.gnu.org/licenses/>.
+ */
+#include "AP_Baro_BMP388.h"
+
+#include <utility>
+
+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<AP_HAL::Device> _dev)
+    : AP_Baro_Backend(baro)
+    , dev(std::move(_dev))
+{
+}
+
+AP_Baro_Backend *AP_Baro_BMP388::probe(AP_Baro &baro,
+                                       AP_HAL::OwnPtr<AP_HAL::Device> _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;
+}

libraries/AP_Baro/AP_Baro_BMP388.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Device.h>
+#include <AP_HAL/utility/OwnPtr.h>
+
+#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<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);
+    void update_temperature(uint32_t);
+    void update_pressure(uint32_t);
+
+    AP_HAL::OwnPtr<AP_HAL::Device> 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);
+};