diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 1599ab4d13..358f9d0e95 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -135,7 +135,7 @@ const char * const lookupTableGyroHardware[] = {
#if defined(USE_SENSOR_NAMES) || defined(USE_BARO)
// sync with baroSensor_e
const char * const lookupTableBaroHardware[] = {
- "AUTO", "NONE", "BMP085", "MS5611", "BMP280", "LPS", "QMP6988"
+ "AUTO", "NONE", "BMP085", "MS5611", "BMP280", "LPS", "QMP6988", "BMP388"
};
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
diff --git a/src/main/drivers/barometer/barometer.h b/src/main/drivers/barometer/barometer.h
index 164e225e09..a447f8bc4b 100644
--- a/src/main/drivers/barometer/barometer.h
+++ b/src/main/drivers/barometer/barometer.h
@@ -21,14 +21,19 @@
#pragma once
#include "drivers/bus.h" // XXX
+#include "drivers/exti.h"
struct baroDev_s;
typedef void (*baroOpFuncPtr)(struct baroDev_s *baro); // baro start operation
typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
+// the 'u' in these variable names means 'uncompensated', 't' is temperature, 'p' pressure.
typedef struct baroDev_s {
busDevice_t busdev;
+#ifdef USE_EXTI
+ extiCallbackRec_t exti;
+#endif
uint16_t ut_delay;
uint16_t up_delay;
baroOpFuncPtr start_ut;
diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c
index ac627f00b4..b2c1434553 100644
--- a/src/main/drivers/barometer/barometer_bmp085.c
+++ b/src/main/drivers/barometer/barometer_bmp085.c
@@ -46,7 +46,7 @@ static bool isEOCConnected = false;
static IO_t eocIO;
static extiCallbackRec_t exti;
-static void bmp085_extiHandler(extiCallbackRec_t* cb)
+static void bmp085ExtiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
isConversionComplete = true;
@@ -120,14 +120,14 @@ static bool bmp085InitDone = false;
STATIC_UNIT_TESTED uint16_t bmp085_ut; // static result of temperature measurement
STATIC_UNIT_TESTED uint32_t bmp085_up; // static result of pressure measurement
-static void bmp085_get_cal_param(busDevice_t *busdev);
-static void bmp085_start_ut(baroDev_t *baro);
-static void bmp085_get_ut(baroDev_t *baro);
-static void bmp085_start_up(baroDev_t *baro);
-static void bmp085_get_up(baroDev_t *baro);
-static int32_t bmp085_get_temperature(uint32_t ut);
-static int32_t bmp085_get_pressure(uint32_t up);
-STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature);
+static void bmp085ReadCalibrarionParameters(busDevice_t *busdev);
+static void bmp085StartUT(baroDev_t *baro);
+static void bmp085GetUT(baroDev_t *baro);
+static void bmp085StartUP(baroDev_t *baro);
+static void bmp085GetUP(baroDev_t *baro);
+static int32_t bmp085GetTemperature(uint32_t ut);
+static int32_t bmp085GetPressure(uint32_t up);
+STATIC_UNIT_TESTED void bmp085Calculate(int32_t *pressure, int32_t *temperature);
static bool bmp085TestEOCConnected(baroDev_t *baro, const bmp085Config_t *config);
@@ -159,7 +159,7 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
eocIO = IOGetByTag(config->eocTag);
IOInit(eocIO, OWNER_BARO_EOC, 0);
- EXTIHandlerInit(&exti, bmp085_extiHandler);
+ EXTIHandlerInit(&exti, bmp085ExtiHandler);
EXTIConfig(eocIO, &exti, NVIC_PRIO_BARO_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING);
EXTIEnable(eocIO, true);
#else
@@ -185,14 +185,14 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
busReadRegisterBuffer(busdev, BMP085_VERSION_REG, &data, 1); /* read Version reg */
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
- bmp085_get_cal_param(busdev); /* readout bmp085 calibparam structure */
+ bmp085ReadCalibrarionParameters(busdev); /* readout bmp085 calibparam structure */
baro->ut_delay = UT_DELAY;
baro->up_delay = UP_DELAY;
- baro->start_ut = bmp085_start_ut;
- baro->get_ut = bmp085_get_ut;
- baro->start_up = bmp085_start_up;
- baro->get_up = bmp085_get_up;
- baro->calculate = bmp085_calculate;
+ baro->start_ut = bmp085StartUT;
+ baro->get_ut = bmp085GetUT;
+ baro->start_up = bmp085StartUP;
+ baro->get_up = bmp085GetUP;
+ baro->calculate = bmp085Calculate;
isEOCConnected = bmp085TestEOCConnected(baro, config);
@@ -217,7 +217,7 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
return false;
}
-static int32_t bmp085_get_temperature(uint32_t ut)
+static int32_t bmp085GetTemperature(uint32_t ut)
{
int32_t temperature;
int32_t x1, x2;
@@ -230,7 +230,7 @@ static int32_t bmp085_get_temperature(uint32_t ut)
return temperature;
}
-static int32_t bmp085_get_pressure(uint32_t up)
+static int32_t bmp085GetPressure(uint32_t up)
{
int32_t pressure, x1, x2, x3, b3, b6;
uint32_t b4, b7;
@@ -270,14 +270,14 @@ static int32_t bmp085_get_pressure(uint32_t up)
return pressure;
}
-static void bmp085_start_ut(baroDev_t *baro)
+static void bmp085StartUT(baroDev_t *baro)
{
isConversionComplete = false;
busWriteRegister(&baro->busdev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
}
-static void bmp085_get_ut(baroDev_t *baro)
+static void bmp085GetUT(baroDev_t *baro)
{
uint8_t data[2];
@@ -290,7 +290,7 @@ static void bmp085_get_ut(baroDev_t *baro)
bmp085_ut = (data[0] << 8) | data[1];
}
-static void bmp085_start_up(baroDev_t *baro)
+static void bmp085StartUP(baroDev_t *baro)
{
uint8_t ctrl_reg_data;
@@ -305,7 +305,7 @@ static void bmp085_start_up(baroDev_t *baro)
depending on the oversampling ratio setting up can be 16 to 19 bit
\return up parameter that represents the uncompensated pressure value
*/
-static void bmp085_get_up(baroDev_t *baro)
+static void bmp085GetUP(baroDev_t *baro)
{
uint8_t data[3];
@@ -319,19 +319,19 @@ static void bmp085_get_up(baroDev_t *baro)
>> (8 - bmp085.oversampling_setting);
}
-STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void bmp085Calculate(int32_t *pressure, int32_t *temperature)
{
int32_t temp, press;
- temp = bmp085_get_temperature(bmp085_ut);
- press = bmp085_get_pressure(bmp085_up);
+ temp = bmp085GetTemperature(bmp085_ut);
+ press = bmp085GetPressure(bmp085_up);
if (pressure)
*pressure = press;
if (temperature)
*temperature = temp;
}
-static void bmp085_get_cal_param(busDevice_t *busdev)
+static void bmp085ReadCalibrarionParameters(busDevice_t *busdev)
{
uint8_t data[22];
busReadRegisterBuffer(busdev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN);
@@ -365,7 +365,7 @@ static bool bmp085TestEOCConnected(baroDev_t *baro, const bmp085Config_t *config
return false;
}
- bmp085_start_ut(baro);
+ bmp085StartUT(baro);
delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure
// conversion should have finished now so check if EOC is high
diff --git a/src/main/drivers/barometer/barometer_bmp085.h b/src/main/drivers/barometer/barometer_bmp085.h
index d338b4c6c7..9cb82fd507 100644
--- a/src/main/drivers/barometer/barometer_bmp085.h
+++ b/src/main/drivers/barometer/barometer_bmp085.h
@@ -22,8 +22,6 @@
#include "drivers/io_types.h"
-#define BMP085_I2C_ADDR 0x77
-
typedef struct bmp085Config_s {
ioTag_t xclrTag;
ioTag_t eocTag;
diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c
index 989a48639b..fb858d7713 100644
--- a/src/main/drivers/barometer/barometer_bmp280.c
+++ b/src/main/drivers/barometer/barometer_bmp280.c
@@ -39,6 +39,46 @@
#if defined(USE_BARO) && (defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280))
+
+#define BMP280_I2C_ADDR (0x76)
+#define BMP280_DEFAULT_CHIP_ID (0x58)
+
+#define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */
+#define BMP280_RST_REG (0xE0) /* Softreset Register */
+#define BMP280_STAT_REG (0xF3) /* Status Register */
+#define BMP280_CTRL_MEAS_REG (0xF4) /* Ctrl Measure Register */
+#define BMP280_CONFIG_REG (0xF5) /* Configuration Register */
+#define BMP280_PRESSURE_MSB_REG (0xF7) /* Pressure MSB Register */
+#define BMP280_PRESSURE_LSB_REG (0xF8) /* Pressure LSB Register */
+#define BMP280_PRESSURE_XLSB_REG (0xF9) /* Pressure XLSB Register */
+#define BMP280_TEMPERATURE_MSB_REG (0xFA) /* Temperature MSB Reg */
+#define BMP280_TEMPERATURE_LSB_REG (0xFB) /* Temperature LSB Reg */
+#define BMP280_TEMPERATURE_XLSB_REG (0xFC) /* Temperature XLSB Reg */
+#define BMP280_FORCED_MODE (0x01)
+
+#define BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG (0x88)
+#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH (24)
+#define BMP280_DATA_FRAME_SIZE (6)
+
+#define BMP280_OVERSAMP_SKIPPED (0x00)
+#define BMP280_OVERSAMP_1X (0x01)
+#define BMP280_OVERSAMP_2X (0x02)
+#define BMP280_OVERSAMP_4X (0x03)
+#define BMP280_OVERSAMP_8X (0x04)
+#define BMP280_OVERSAMP_16X (0x05)
+
+// configure pressure and temperature oversampling, forced sampling mode
+#define BMP280_PRESSURE_OSR (BMP280_OVERSAMP_8X)
+#define BMP280_TEMPERATURE_OSR (BMP280_OVERSAMP_1X)
+#define BMP280_MODE (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE)
+
+#define T_INIT_MAX (20)
+// 20/16 = 1.25 ms
+#define T_MEASURE_PER_OSRS_MAX (37)
+// 37/16 = 2.3125 ms
+#define T_SETUP_PRESSURE_MAX (10)
+// 10/16 = 0.625 ms
+
typedef struct bmp280_calib_param_s {
uint16_t dig_T1; /* calibration T1 data */
int16_t dig_T2; /* calibration T2 data */
@@ -64,12 +104,12 @@ STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
int32_t bmp280_up = 0;
int32_t bmp280_ut = 0;
-static void bmp280_start_ut(baroDev_t *baro);
-static void bmp280_get_ut(baroDev_t *baro);
-static void bmp280_start_up(baroDev_t *baro);
-static void bmp280_get_up(baroDev_t *baro);
+static void bmp280StartUT(baroDev_t *baro);
+static void bmp280GetUT(baroDev_t *baro);
+static void bmp280StartUP(baroDev_t *baro);
+static void bmp280GetUP(baroDev_t *baro);
-STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+STATIC_UNIT_TESTED void bmp280Calculate(int32_t *pressure, int32_t *temperature);
void bmp280BusInit(busDevice_t *busdev)
{
@@ -135,37 +175,37 @@ bool bmp280Detect(baroDev_t *baro)
// these are dummy as temperature is measured as part of pressure
baro->ut_delay = 0;
- baro->get_ut = bmp280_get_ut;
- baro->start_ut = bmp280_start_ut;
+ baro->get_ut = bmp280GetUT;
+ baro->start_ut = bmp280StartUT;
// only _up part is executed, and gets both temperature and pressure
- baro->start_up = bmp280_start_up;
- baro->get_up = bmp280_get_up;
+ baro->start_up = bmp280StartUP;
+ baro->get_up = bmp280GetUP;
baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
- baro->calculate = bmp280_calculate;
+ baro->calculate = bmp280Calculate;
return true;
}
-static void bmp280_start_ut(baroDev_t *baro)
+static void bmp280StartUT(baroDev_t *baro)
{
UNUSED(baro);
// dummy
}
-static void bmp280_get_ut(baroDev_t *baro)
+static void bmp280GetUT(baroDev_t *baro)
{
UNUSED(baro);
// dummy
}
-static void bmp280_start_up(baroDev_t *baro)
+static void bmp280StartUP(baroDev_t *baro)
{
// start measurement
// set oversampling + power mode (forced), and start sampling
busWriteRegister(&baro->busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
}
-static void bmp280_get_up(baroDev_t *baro)
+static void bmp280GetUP(baroDev_t *baro)
{
uint8_t data[BMP280_DATA_FRAME_SIZE];
@@ -177,7 +217,7 @@ static void bmp280_get_up(baroDev_t *baro)
// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
// t_fine carries fine temperature as global value
-static int32_t bmp280_compensate_T(int32_t adc_T)
+static int32_t bmp280CompensateTemperature(int32_t adc_T)
{
int32_t var1, var2, T;
@@ -191,7 +231,7 @@ static int32_t bmp280_compensate_T(int32_t adc_T)
// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
// Output value of "24674867" represents 24674867/256 = 96386.2 Pa = 963.862 hPa
-static uint32_t bmp280_compensate_P(int32_t adc_P)
+static uint32_t bmp280CompensatePressure(int32_t adc_P)
{
int64_t var1, var2, p;
var1 = ((int64_t)t_fine) - 128000;
@@ -210,13 +250,13 @@ static uint32_t bmp280_compensate_P(int32_t adc_P)
return (uint32_t)p;
}
-STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void bmp280Calculate(int32_t *pressure, int32_t *temperature)
{
// calculate
int32_t t;
uint32_t p;
- t = bmp280_compensate_T(bmp280_ut);
- p = bmp280_compensate_P(bmp280_up);
+ t = bmp280CompensateTemperature(bmp280_ut);
+ p = bmp280CompensatePressure(bmp280_up);
if (pressure)
*pressure = (int32_t)(p / 256);
diff --git a/src/main/drivers/barometer/barometer_bmp280.h b/src/main/drivers/barometer/barometer_bmp280.h
index 12f23dc462..c55e7ffd0d 100644
--- a/src/main/drivers/barometer/barometer_bmp280.h
+++ b/src/main/drivers/barometer/barometer_bmp280.h
@@ -20,43 +20,4 @@
#pragma once
-#define BMP280_I2C_ADDR (0x76)
-#define BMP280_DEFAULT_CHIP_ID (0x58)
-
-#define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */
-#define BMP280_RST_REG (0xE0) /* Softreset Register */
-#define BMP280_STAT_REG (0xF3) /* Status Register */
-#define BMP280_CTRL_MEAS_REG (0xF4) /* Ctrl Measure Register */
-#define BMP280_CONFIG_REG (0xF5) /* Configuration Register */
-#define BMP280_PRESSURE_MSB_REG (0xF7) /* Pressure MSB Register */
-#define BMP280_PRESSURE_LSB_REG (0xF8) /* Pressure LSB Register */
-#define BMP280_PRESSURE_XLSB_REG (0xF9) /* Pressure XLSB Register */
-#define BMP280_TEMPERATURE_MSB_REG (0xFA) /* Temperature MSB Reg */
-#define BMP280_TEMPERATURE_LSB_REG (0xFB) /* Temperature LSB Reg */
-#define BMP280_TEMPERATURE_XLSB_REG (0xFC) /* Temperature XLSB Reg */
-#define BMP280_FORCED_MODE (0x01)
-
-#define BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG (0x88)
-#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH (24)
-#define BMP280_DATA_FRAME_SIZE (6)
-
-#define BMP280_OVERSAMP_SKIPPED (0x00)
-#define BMP280_OVERSAMP_1X (0x01)
-#define BMP280_OVERSAMP_2X (0x02)
-#define BMP280_OVERSAMP_4X (0x03)
-#define BMP280_OVERSAMP_8X (0x04)
-#define BMP280_OVERSAMP_16X (0x05)
-
-// configure pressure and temperature oversampling, forced sampling mode
-#define BMP280_PRESSURE_OSR (BMP280_OVERSAMP_8X)
-#define BMP280_TEMPERATURE_OSR (BMP280_OVERSAMP_1X)
-#define BMP280_MODE (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE)
-
-#define T_INIT_MAX (20)
-// 20/16 = 1.25 ms
-#define T_MEASURE_PER_OSRS_MAX (37)
-// 37/16 = 2.3125 ms
-#define T_SETUP_PRESSURE_MAX (10)
-// 10/16 = 0.625 ms
-
bool bmp280Detect(baroDev_t *baro);
diff --git a/src/main/drivers/barometer/barometer_bmp388.c b/src/main/drivers/barometer/barometer_bmp388.c
new file mode 100644
index 0000000000..c47ba710df
--- /dev/null
+++ b/src/main/drivers/barometer/barometer_bmp388.c
@@ -0,0 +1,409 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see .
+ *
+ * BMP388 Driver author: Dominic Clifton
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#include "build/build_config.h"
+
+#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388))
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "barometer.h"
+
+#include "drivers/bus.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/bus_i2c_busdev.h"
+#include "drivers/bus_spi.h"
+#include "drivers/io.h"
+#include "drivers/time.h"
+#include "drivers/nvic.h"
+
+#include "barometer_bmp388.h"
+
+
+#define BMP388_I2C_ADDR (0x76) // same as BMP280/BMP180
+#define BMP388_DEFAULT_CHIP_ID (0x50) // from https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h#L130
+
+#define BMP388_CMD_REG (0x7E)
+#define BMP388_RESERVED_UPPER_REG (0x7D)
+// everything between BMP388_RESERVED_UPPER_REG and BMP388_RESERVED_LOWER_REG is reserved.
+#define BMP388_RESERVED_LOWER_REG (0x20)
+#define BMP388_CONFIG_REG (0x1F)
+#define BMP388_RESERVED_0x1E_REG (0x1E)
+#define BMP388_ODR_REG (0x1D)
+#define BMP388_OSR_REG (0x1C)
+#define BMP388_PWR_CTRL_REG (0x1B)
+#define BMP388_IF_CONFIG_REG (0x1A)
+#define BMP388_INT_CTRL_REG (0x19)
+#define BMP388_FIFO_CONFIG_2_REG (0x18)
+#define BMP388_FIFO_CONFIG_1_REG (0x17)
+#define BMP388_FIFO_WTM_1_REG (0x16)
+#define BMP388_FIFO_WTM_0_REG (0x15)
+#define BMP388_FIFO_DATA_REG (0x14)
+#define BMP388_FIFO_LENGTH_1_REG (0x13)
+#define BMP388_FIFO_LENGTH_0_REG (0x12)
+#define BMP388_INT_STATUS_REG (0x11)
+#define BMP388_EVENT_REG (0x10)
+#define BMP388_SENSORTIME_3_REG (0x0F) // BME780 only
+#define BMP388_SENSORTIME_2_REG (0x0E)
+#define BMP388_SENSORTIME_1_REG (0x0D)
+#define BMP388_SENSORTIME_0_REG (0x0C)
+#define BMP388_RESERVED_0x0B_REG (0x0B)
+#define BMP388_RESERVED_0x0A_REG (0x0A)
+
+// see friendly register names below
+#define BMP388_DATA_5_REG (0x09)
+#define BMP388_DATA_4_REG (0x08)
+#define BMP388_DATA_3_REG (0x07)
+#define BMP388_DATA_2_REG (0x06)
+#define BMP388_DATA_1_REG (0x05)
+#define BMP388_DATA_0_REG (0x04)
+
+#define BMP388_STATUS_REG (0x03)
+#define BMP388_ERR_REG (0x02)
+#define BMP388_RESERVED_0x01_REG (0x01)
+#define BMP388_CHIP_ID_REG (0x00)
+
+// friendly register names, from datasheet 4.3.4
+#define BMP388_PRESS_MSB_23_16_REG BMP388_DATA_2_REG
+#define BMP388_PRESS_LSB_15_8_REG BMP388_DATA_1_REG
+#define BMP388_PRESS_XLSB_7_0_REG BMP388_DATA_0_REG
+
+// friendly register names, from datasheet 4.3.5
+#define BMP388_TEMP_MSB_23_16_REG BMP388_DATA_5_REG
+#define BMP388_TEMP_LSB_15_8_REG BMP388_DATA_4_REG
+#define BMP388_TEMP_XLSB_7_0_REG BMP388_DATA_3_REG
+
+#define BMP388_DATA_FRAME_LENGTH ((BMP388_DATA_5_REG - BMP388_DATA_0_REG) + 1) // +1 for inclusive
+
+// from Datasheet 3.3
+#define BMP388_MODE_SLEEP (0x00)
+#define BMP388_MODE_FORCED (0x01)
+#define BMP388_MODE_NORMAL (0x02)
+
+#define BMP388_CALIRATION_LOWER_REG (0x30) // See datasheet 4.3.19, "calibration data"
+#define BMP388_TRIMMING_NVM_PAR_T1_LSB_REG (0x31) // See datasheet 3.11.1 "Memory map trimming coefficients"
+#define BMP388_TRIMMING_NVM_PAR_P11_REG (0x45) // See datasheet 3.11.1 "Memory map trimming coefficients"
+#define BMP388_CALIRATION_UPPER_REG (0x57)
+
+#define BMP388_TRIMMING_DATA_LENGTH ((BMP388_TRIMMING_NVM_PAR_P11_REG - BMP388_TRIMMING_NVM_PAR_T1_LSB_REG) + 1) // +1 for inclusive
+
+#define BMP388_DATA_FRAME_SIZE (6)
+
+#define BMP388_OVERSAMP_1X (0x00)
+#define BMP388_OVERSAMP_2X (0x01)
+#define BMP388_OVERSAMP_4X (0x02)
+#define BMP388_OVERSAMP_8X (0x03)
+#define BMP388_OVERSAMP_16X (0x04)
+#define BMP388_OVERSAMP_32X (0x05)
+
+// INT_CTRL register
+#define BMP388_INT_OD_BIT 0
+#define BMP388_INT_LEVEL_BIT 1
+#define BMP388_INT_LATCH_BIT 2
+#define BMP388_INT_FWTM_EN_BIT 3
+#define BMP388_INT_FFULL_EN_BIT 4
+#define BMP388_INT_RESERVED_5_BIT 5
+#define BMP388_INT_DRDY_EN_BIT 6
+#define BMP388_INT_RESERVED_7_BIT 7
+
+// OSR register
+#define BMP388_OSR_P_BIT 0 // to 2
+#define BMP388_OSR4_T_BIT 3 // to 5
+#define BMP388_OSR_P_MASK (0x03) // -----111
+#define BMP388_OSR4_T_MASK (0x38) // --111---
+
+// configure pressure and temperature oversampling, forced sampling mode
+#define BMP388_PRESSURE_OSR (BMP388_OVERSAMP_8X)
+#define BMP388_TEMPERATURE_OSR (BMP388_OVERSAMP_1X)
+
+// see Datasheet 3.11.1 Memory Map Trimming Coefficients
+typedef struct bmp388_calib_param_s {
+ uint16_t T1;
+ uint16_t T2;
+ int8_t T3;
+ int16_t P1;
+ int16_t P2;
+ int8_t P3;
+ int8_t P4;
+ uint16_t P5;
+ uint16_t P6;
+ int8_t P7;
+ int8_t P8;
+ int16_t P9;
+ int8_t P10;
+ int8_t P11;
+} __attribute__((packed)) bmp388_calib_param_t;
+
+STATIC_ASSERT(sizeof(bmp388_calib_param_t) == BMP388_TRIMMING_DATA_LENGTH, bmp388_calibration_structure_incorrectly_packed);
+
+static uint8_t bmp388_chip_id = 0;
+STATIC_UNIT_TESTED bmp388_calib_param_t bmp388_cal;
+// uncompensated pressure and temperature
+uint32_t bmp388_up = 0;
+uint32_t bmp388_ut = 0;
+STATIC_UNIT_TESTED int64_t t_lin = 0;
+
+static void bmp388StartUT(baroDev_t *baro);
+static void bmp388GetUT(baroDev_t *baro);
+static void bmp388StartUP(baroDev_t *baro);
+static void bmp388GetUP(baroDev_t *baro);
+
+STATIC_UNIT_TESTED void bmp388Calculate(int32_t *pressure, int32_t *temperature);
+
+void bmp388_extiHandler(extiCallbackRec_t* cb)
+{
+#ifdef DEBUG
+ static uint32_t bmp388ExtiCallbackCounter = 0;
+
+ bmp388ExtiCallbackCounter++;
+#endif
+
+ baroDev_t *baro = container_of(cb, baroDev_t, exti);
+
+ uint8_t intStatus = 0;
+ busReadRegisterBuffer(&baro->busdev, BMP388_INT_STATUS_REG, &intStatus, 1);
+}
+
+void bmp388BusInit(busDevice_t *busdev)
+{
+#ifdef USE_BARO_SPI_BMP388
+ if (busdev->bustype == BUSTYPE_SPI) {
+ IOHi(busdev->busdev_u.spi.csnPin); // Disable
+ IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
+ IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
+ spiBusSetDivisor(busdev, SPI_CLOCK_STANDARD);
+ }
+#else
+ UNUSED(busdev);
+#endif
+}
+
+void bmp388BusDeinit(busDevice_t *busdev)
+{
+#ifdef USE_BARO_SPI_BMP388
+ if (busdev->bustype == BUSTYPE_SPI) {
+ spiPreinitByIO(busdev->busdev_u.spi.csnPin);
+ }
+#else
+ UNUSED(busdev);
+#endif
+}
+
+void bmp388BeginForcedMeasurement(busDevice_t *busdev)
+{
+ // enable pressure measurement, temperature measurement, set power mode and start sampling
+ uint8_t mode = (BMP388_MODE_FORCED << 4) | (1 << 1) | (1 << 0);
+ busWriteRegister(busdev, BMP388_PWR_CTRL_REG, mode);
+}
+
+bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
+{
+ delay(20);
+
+#if defined(USE_EXTI)
+ IO_t baroIntIO = IOGetByTag(config->eocTag);
+ if (baroIntIO) {
+ IOInit(baroIntIO, OWNER_BARO_EOC, 0);
+ EXTIHandlerInit(&baro->exti, bmp388_extiHandler);
+ EXTIConfig(baroIntIO, &baro->exti, NVIC_PRIO_BARO_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING);
+ EXTIEnable(baroIntIO, true);
+ }
+#else
+ UNUSED(config);
+#endif
+
+ busDevice_t *busdev = &baro->busdev;
+ bool defaultAddressApplied = false;
+
+ bmp388BusInit(busdev);
+
+ if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
+ // Default address for BMP388
+ busdev->busdev_u.i2c.address = BMP388_I2C_ADDR;
+ defaultAddressApplied = true;
+ }
+
+ busReadRegisterBuffer(busdev, BMP388_CHIP_ID_REG, &bmp388_chip_id, 1);
+
+ if (bmp388_chip_id != BMP388_DEFAULT_CHIP_ID) {
+ bmp388BusDeinit(busdev);
+ if (defaultAddressApplied) {
+ busdev->busdev_u.i2c.address = 0;
+ }
+ return false;
+ }
+
+ if (baroIntIO) {
+ uint8_t intCtrlValue = (1 << BMP388_INT_DRDY_EN_BIT) | (0 << BMP388_INT_FFULL_EN_BIT) | (0 << BMP388_INT_FWTM_EN_BIT) | (0 << BMP388_INT_LATCH_BIT) | (1 << BMP388_INT_LEVEL_BIT) | (0 << BMP388_INT_OD_BIT);
+ busWriteRegister(busdev, BMP388_INT_CTRL_REG, intCtrlValue);
+ }
+
+ // read calibration
+ busReadRegisterBuffer(busdev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t *)&bmp388_cal, sizeof(bmp388_calib_param_t));
+
+
+ // set oversampling
+ busWriteRegister(busdev, BMP388_OSR_REG,
+ ((BMP388_PRESSURE_OSR << BMP388_OSR_P_BIT) & BMP388_OSR_P_MASK) |
+ ((BMP388_TEMPERATURE_OSR << BMP388_OSR4_T_BIT) & BMP388_OSR4_T_MASK)
+ );
+
+ bmp388BeginForcedMeasurement(busdev);
+
+ // these are dummy as temperature is measured as part of pressure
+ baro->ut_delay = 0;
+ baro->get_ut = bmp388GetUT;
+ baro->start_ut = bmp388StartUT;
+ // only _up part is executed, and gets both temperature and pressure
+ baro->start_up = bmp388StartUP;
+ baro->get_up = bmp388GetUP;
+
+ // See datasheet 3.9.2 "Measurement rate in forced mode and normal mode"
+ baro->up_delay = 234 +
+ (392 + (powerf(2, BMP388_PRESSURE_OSR + 1) * 2000)) +
+ (313 + (powerf(2, BMP388_TEMPERATURE_OSR + 1) * 2000));
+
+ baro->calculate = bmp388Calculate;
+
+ return true;
+}
+
+static void bmp388StartUT(baroDev_t *baro)
+{
+ UNUSED(baro);
+ // dummy
+}
+
+static void bmp388GetUT(baroDev_t *baro)
+{
+ UNUSED(baro);
+ // dummy
+}
+
+static void bmp388StartUP(baroDev_t *baro)
+{
+ // start measurement
+ bmp388BeginForcedMeasurement(&baro->busdev);
+}
+
+static void bmp388GetUP(baroDev_t *baro)
+{
+ uint8_t dataFrame[BMP388_DATA_FRAME_LENGTH];
+
+ // read data from sensor
+ busReadRegisterBuffer(&baro->busdev, BMP388_DATA_0_REG, dataFrame, BMP388_DATA_FRAME_LENGTH);
+
+ bmp388_up = ((uint32_t)(dataFrame[0]) << 0) | ((uint32_t)(dataFrame[1]) << 8) | ((uint32_t)(dataFrame[2]) << 16);
+ bmp388_ut = ((uint32_t)(dataFrame[3]) << 0) | ((uint32_t)(dataFrame[4]) << 8) | ((uint32_t)(dataFrame[5]) << 16);
+}
+
+// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
+static int64_t bmp388CompensateTemperature(uint32_t uncomp_temperature)
+{
+ uint64_t partial_data1;
+ uint64_t partial_data2;
+ uint64_t partial_data3;
+ int64_t partial_data4;
+ int64_t partial_data5;
+ int64_t partial_data6;
+ int64_t comp_temp;
+
+ partial_data1 = uncomp_temperature - (256 * bmp388_cal.T1);
+ partial_data2 = bmp388_cal.T2 * partial_data1;
+ partial_data3 = partial_data1 * partial_data1;
+ partial_data4 = (int64_t)partial_data3 * bmp388_cal.T3;
+ partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
+ partial_data6 = partial_data5 / 4294967296;
+ /* Update t_lin, needed for pressure calculation */
+ t_lin = partial_data6;
+ comp_temp = (int64_t)((partial_data6 * 25) / 16384);
+
+ return comp_temp;
+}
+
+static uint64_t bmp388CompensatePressure(uint32_t uncomp_pressure)
+{
+ int64_t partial_data1;
+ int64_t partial_data2;
+ int64_t partial_data3;
+ int64_t partial_data4;
+ int64_t partial_data5;
+ int64_t partial_data6;
+ int64_t offset;
+ int64_t sensitivity;
+ uint64_t comp_press;
+
+ partial_data1 = t_lin * t_lin;
+ partial_data2 = partial_data1 / 64;
+ partial_data3 = (partial_data2 * t_lin) / 256;
+ partial_data4 = (bmp388_cal.P8 * partial_data3) / 32;
+ partial_data5 = (bmp388_cal.P7 * partial_data1) * 16;
+ partial_data6 = (bmp388_cal.P6 * t_lin) * 4194304;
+ offset = (bmp388_cal.P5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6;
+
+ partial_data2 = (bmp388_cal.P4 * partial_data3) / 32;
+ partial_data4 = (bmp388_cal.P3 * partial_data1) * 4;
+ partial_data5 = (bmp388_cal.P2 - 16384) * t_lin * 2097152;
+ sensitivity = ((bmp388_cal.P1 - 16384) * 70368744177664) + partial_data2 + partial_data4
+ + partial_data5;
+
+ partial_data1 = (sensitivity / 16777216) * uncomp_pressure;
+ partial_data2 = bmp388_cal.P10 * t_lin;
+ partial_data3 = partial_data2 + (65536 * bmp388_cal.P9);
+ partial_data4 = (partial_data3 * uncomp_pressure) / 8192;
+ partial_data5 = (partial_data4 * uncomp_pressure) / 512;
+ partial_data6 = (int64_t)((uint64_t)uncomp_pressure * (uint64_t)uncomp_pressure);
+ partial_data2 = (bmp388_cal.P11 * partial_data6) / 65536;
+ partial_data3 = (partial_data2 * uncomp_pressure) / 128;
+ partial_data4 = (offset / 4) + partial_data1 + partial_data5 + partial_data3;
+ comp_press = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776);
+
+#ifdef DEBUG_BARO_BMP388
+ debug[1] = ((comp_press & 0xFFFF0000) >> 32);
+ debug[2] = ((comp_press & 0x0000FFFF) >> 0);
+#endif
+ return comp_press;
+}
+
+STATIC_UNIT_TESTED void bmp388Calculate(int32_t *pressure, int32_t *temperature)
+{
+ // calculate
+ int64_t t;
+ int64_t p;
+
+ t = bmp388CompensateTemperature(bmp388_ut);
+ p = bmp388CompensatePressure(bmp388_up);
+
+ if (pressure)
+ *pressure = (int32_t)(p / 256);
+ if (temperature)
+ *temperature = t;
+}
+
+#endif
diff --git a/src/main/drivers/barometer/barometer_bmp388.h b/src/main/drivers/barometer/barometer_bmp388.h
new file mode 100644
index 0000000000..5cccc4d711
--- /dev/null
+++ b/src/main/drivers/barometer/barometer_bmp388.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see .
+ *
+ * BMP388 Driver author: Dominic Clifton
+ *
+ * References:
+ * BMP388 datasheet - https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST-BMP388-DS001.pdf
+ * BMP3-Sensor-API - https://github.com/BoschSensortec/BMP3-Sensor-API
+ * BMP280 Cleanflight driver
+ */
+
+#pragma once
+
+typedef struct bmp388Config_s {
+ ioTag_t eocTag;
+} bmp388Config_t;
+
+bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro);
diff --git a/src/main/drivers/barometer/barometer_lps.c b/src/main/drivers/barometer/barometer_lps.c
index 92fb2fa41b..5c0372ecd3 100644
--- a/src/main/drivers/barometer/barometer_lps.c
+++ b/src/main/drivers/barometer/barometer_lps.c
@@ -219,13 +219,13 @@ static void lpsOff(busDevice_t *busdev)
lpsWriteCommand(busdev, LPS_CTRL1, 0x00 | (0x01 << 2));
}
-static void lps_nothing(baroDev_t *baro)
+static void lpsNothing(baroDev_t *baro)
{
UNUSED(baro);
return;
}
-static void lps_read(baroDev_t *baro)
+static void lpsRead(baroDev_t *baro)
{
uint8_t status = 0x00;
lpsReadCommand(&baro->busdev, LPS_STATUS, &status, 1);
@@ -242,7 +242,7 @@ static void lps_read(baroDev_t *baro)
}
}
-static void lps_calculate(int32_t *pressure, int32_t *temperature)
+static void lpsCalculate(int32_t *pressure, int32_t *temperature)
{
*pressure = (int32_t)rawP * 100 / 4096;
*temperature = (int32_t)rawT * 10 / 48 + 4250;
@@ -284,14 +284,14 @@ bool lpsDetect(baroDev_t *baro)
baro->ut_delay = 1;
baro->up_delay = 1000000 / 24;
- baro->start_ut = lps_nothing;
- baro->get_ut = lps_nothing;
- baro->start_up = lps_nothing;
- baro->get_up = lps_read;
- baro->calculate = lps_calculate;
+ baro->start_ut = lpsNothing;
+ baro->get_ut = lpsNothing;
+ baro->start_up = lpsNothing;
+ baro->get_up = lpsRead;
+ baro->calculate = lpsCalculate;
uint32_t timeout = millis();
do {
- lps_read(baro);
+ lpsRead(baro);
if ((millis() - timeout) > 500) return false;
} while (rawT == 0 && rawP == 0);
rawT = 0;
diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c
index 867dfce3c5..80883caef0 100644
--- a/src/main/drivers/barometer/barometer_ms5611.c
+++ b/src/main/drivers/barometer/barometer_ms5611.c
@@ -36,6 +36,9 @@
#include "drivers/io.h"
#include "drivers/time.h"
+// MS5611, Standard address 0x77
+#define MS5611_I2C_ADDR 0x77
+
#define CMD_RESET 0x1E // ADC reset command
#define CMD_ADC_READ 0x00 // ADC read command
#define CMD_ADC_CONV 0x40 // ADC conversion command
@@ -49,15 +52,15 @@
#define CMD_PROM_RD 0xA0 // Prom read command
#define PROM_NB 8
-static void ms5611_reset(busDevice_t *busdev);
-static uint16_t ms5611_prom(busDevice_t *busdev, int8_t coef_num);
-STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom);
-static uint32_t ms5611_read_adc(busDevice_t *busdev);
-static void ms5611_start_ut(baroDev_t *baro);
-static void ms5611_get_ut(baroDev_t *baro);
-static void ms5611_start_up(baroDev_t *baro);
-static void ms5611_get_up(baroDev_t *baro);
-STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature);
+static void ms5611Reset(busDevice_t *busdev);
+static uint16_t ms5611Prom(busDevice_t *busdev, int8_t coef_num);
+STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom);
+static uint32_t ms5611ReadAdc(busDevice_t *busdev);
+static void ms5611StartUT(baroDev_t *baro);
+static void ms5611GetUT(baroDev_t *baro);
+static void ms5611StartUP(baroDev_t *baro);
+static void ms5611GetUP(baroDev_t *baro);
+STATIC_UNIT_TESTED void ms5611Calculate(int32_t *pressure, int32_t *temperature);
STATIC_UNIT_TESTED uint32_t ms5611_ut; // static result of temperature measurement
STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement
@@ -115,25 +118,25 @@ bool ms5611Detect(baroDev_t *baro)
goto fail;
}
- ms5611_reset(busdev);
+ ms5611Reset(busdev);
// read all coefficients
for (i = 0; i < PROM_NB; i++)
- ms5611_c[i] = ms5611_prom(busdev, i);
+ ms5611_c[i] = ms5611Prom(busdev, i);
// check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
- if (ms5611_crc(ms5611_c) != 0) {
+ if (ms5611CRC(ms5611_c) != 0) {
goto fail;
}
// TODO prom + CRC
baro->ut_delay = 10000;
baro->up_delay = 10000;
- baro->start_ut = ms5611_start_ut;
- baro->get_ut = ms5611_get_ut;
- baro->start_up = ms5611_start_up;
- baro->get_up = ms5611_get_up;
- baro->calculate = ms5611_calculate;
+ baro->start_ut = ms5611StartUT;
+ baro->get_ut = ms5611GetUT;
+ baro->start_up = ms5611StartUP;
+ baro->get_up = ms5611GetUP;
+ baro->calculate = ms5611Calculate;
return true;
@@ -147,14 +150,14 @@ fail:;
return false;
}
-static void ms5611_reset(busDevice_t *busdev)
+static void ms5611Reset(busDevice_t *busdev)
{
busWriteRegister(busdev, CMD_RESET, 1);
delayMicroseconds(2800);
}
-static uint16_t ms5611_prom(busDevice_t *busdev, int8_t coef_num)
+static uint16_t ms5611Prom(busDevice_t *busdev, int8_t coef_num)
{
uint8_t rxbuf[2] = { 0, 0 };
@@ -163,7 +166,7 @@ static uint16_t ms5611_prom(busDevice_t *busdev, int8_t coef_num)
return rxbuf[0] << 8 | rxbuf[1];
}
-STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom)
+STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom)
{
int32_t i, j;
uint32_t res = 0;
@@ -193,7 +196,7 @@ STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom)
return -1;
}
-static uint32_t ms5611_read_adc(busDevice_t *busdev)
+static uint32_t ms5611ReadAdc(busDevice_t *busdev)
{
uint8_t rxbuf[3];
@@ -202,27 +205,27 @@ static uint32_t ms5611_read_adc(busDevice_t *busdev)
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
}
-static void ms5611_start_ut(baroDev_t *baro)
+static void ms5611StartUT(baroDev_t *baro)
{
busWriteRegister(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
}
-static void ms5611_get_ut(baroDev_t *baro)
+static void ms5611GetUT(baroDev_t *baro)
{
- ms5611_ut = ms5611_read_adc(&baro->busdev);
+ ms5611_ut = ms5611ReadAdc(&baro->busdev);
}
-static void ms5611_start_up(baroDev_t *baro)
+static void ms5611StartUP(baroDev_t *baro)
{
busWriteRegister(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
}
-static void ms5611_get_up(baroDev_t *baro)
+static void ms5611GetUP(baroDev_t *baro)
{
- ms5611_up = ms5611_read_adc(&baro->busdev);
+ ms5611_up = ms5611ReadAdc(&baro->busdev);
}
-STATIC_UNIT_TESTED void ms5611_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void ms5611Calculate(int32_t *pressure, int32_t *temperature)
{
uint32_t press;
int64_t temp;
diff --git a/src/main/drivers/barometer/barometer_ms5611.h b/src/main/drivers/barometer/barometer_ms5611.h
index c6e4cc9cd4..5b07080843 100644
--- a/src/main/drivers/barometer/barometer_ms5611.h
+++ b/src/main/drivers/barometer/barometer_ms5611.h
@@ -20,7 +20,4 @@
#pragma once
-// MS5611, Standard address 0x77
-#define MS5611_I2C_ADDR 0x77
-
bool ms5611Detect(baroDev_t *baro);
diff --git a/src/main/drivers/barometer/barometer_qmp6988.c b/src/main/drivers/barometer/barometer_qmp6988.c
index ac52f2fb4e..cbcb9af44d 100644
--- a/src/main/drivers/barometer/barometer_qmp6988.c
+++ b/src/main/drivers/barometer/barometer_qmp6988.c
@@ -92,12 +92,12 @@ STATIC_UNIT_TESTED qmp6988_calib_param_t qmp6988_cal;
int32_t qmp6988_up = 0;
int32_t qmp6988_ut = 0;
-static void qmp6988_start_ut(baroDev_t *baro);
-static void qmp6988_get_ut(baroDev_t *baro);
-static void qmp6988_start_up(baroDev_t *baro);
-static void qmp6988_get_up(baroDev_t *baro);
+static void qmp6988StartUT(baroDev_t *baro);
+static void qmp6988GetUT(baroDev_t *baro);
+static void qmp6988StartUP(baroDev_t *baro);
+static void qmp6988GetUP(baroDev_t *baro);
-STATIC_UNIT_TESTED void qmp6988_calculate(int32_t *pressure, int32_t *temperature);
+STATIC_UNIT_TESTED void qmp6988Calculate(int32_t *pressure, int32_t *temperature);
void qmp6988BusInit(busDevice_t *busdev)
{
@@ -261,36 +261,36 @@ bool qmp6988Detect(baroDev_t *baro)
// these are dummy as temperature is measured as part of pressure
baro->ut_delay = 0;
- baro->get_ut = qmp6988_get_ut;
- baro->start_ut = qmp6988_start_ut;
+ baro->get_ut = qmp6988GetUT;
+ baro->start_ut = qmp6988StartUT;
// only _up part is executed, and gets both temperature and pressure
- baro->start_up = qmp6988_start_up;
- baro->get_up = qmp6988_get_up;
+ baro->start_up = qmp6988StartUP;
+ baro->get_up = qmp6988GetUP;
baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << QMP6988_TEMPERATURE_OSR) >> 1) + ((1 << QMP6988_PRESSURE_OSR) >> 1)) + (QMP6988_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
- baro->calculate = qmp6988_calculate;
+ baro->calculate = qmp6988Calculate;
return true;
}
-static void qmp6988_start_ut(baroDev_t *baro)
+static void qmp6988StartUT(baroDev_t *baro)
{
UNUSED(baro);
// dummy
}
-static void qmp6988_get_ut(baroDev_t *baro)
+static void qmp6988GetUT(baroDev_t *baro)
{
UNUSED(baro);
// dummy
}
-static void qmp6988_start_up(baroDev_t *baro)
+static void qmp6988StartUP(baroDev_t *baro)
{
// start measurement
busWriteRegister(&baro->busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
}
-static void qmp6988_get_up(baroDev_t *baro)
+static void qmp6988GetUP(baroDev_t *baro)
{
uint8_t data[QMP6988_DATA_FRAME_SIZE];
@@ -302,7 +302,7 @@ static void qmp6988_get_up(baroDev_t *baro)
// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
// t_fine carries fine temperature as global value
-static float qmp6988_compensate_T(int32_t adc_T)
+static float qmp6988CompensateTemperature(int32_t adc_T)
{
int32_t var1;
float T;
@@ -315,12 +315,12 @@ static float qmp6988_compensate_T(int32_t adc_T)
-STATIC_UNIT_TESTED void qmp6988_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void qmp6988Calculate(int32_t *pressure, int32_t *temperature)
{
float tr,pr;
int32_t Dp;
- tr = qmp6988_compensate_T(qmp6988_ut);
+ tr = qmp6988CompensateTemperature(qmp6988_ut);
Dp = qmp6988_up - 1024*1024*8;
pr = qmp6988_cal.Coe_b00+qmp6988_cal.Coe_bt1*tr+qmp6988_cal.Coe_bp1*Dp+qmp6988_cal.Coe_b11*tr*Dp+qmp6988_cal.Coe_bt2*tr*tr+qmp6988_cal.Coe_bp2*Dp*Dp+qmp6988_cal.Coe_b12*Dp*tr*tr
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 2e0f354ad7..52c508f15f 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -40,6 +40,7 @@
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp085.h"
#include "drivers/barometer/barometer_bmp280.h"
+#include "drivers/barometer/barometer_bmp388.h"
#include "drivers/barometer/barometer_qmp6988.h"
#include "drivers/barometer/barometer_fake.h"
#include "drivers/barometer/barometer_ms5611.h"
@@ -69,11 +70,17 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
//
// 1. If DEFAULT_BARO_xxx is defined, use it.
// 2. Determine default based on USE_BARO_xxx
- // a. Precedence is in the order of popularity; BMP280, MS5611 then BMP085, then
+ // a. Precedence is in the order of popularity; BMP388, BMP280, MS5611 then BMP085, then
// b. If SPI variant is specified, it is likely onboard, so take it.
-#if !(defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP085) || defined(DEFAULT_BARO_SPI_LPS) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_QMP6988))
-#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
+#if !(defined(DEFAULT_BARO_SPI_BMP388) || defined(DEFAULT_BARO_BMP388) || defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP085) || defined(DEFAULT_BARO_SPI_LPS) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_QMP6988))
+#if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
+#if defined(USE_BARO_SPI_BMP388)
+#define DEFAULT_BARO_SPI_BMP388
+#else
+#define DEFAULT_BARO_BMP388
+#endif
+#elif defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
#if defined(USE_BARO_SPI_BMP280)
#define DEFAULT_BARO_SPI_BMP280
#else
@@ -98,13 +105,13 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
#endif
#endif
-#if defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_SPI_LPS)
+#if defined(DEFAULT_BARO_SPI_BMP388) || defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_SPI_LPS)
barometerConfig->baro_bustype = BUSTYPE_SPI;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(BARO_SPI_INSTANCE));
barometerConfig->baro_spi_csn = IO_TAG(BARO_CS_PIN);
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
barometerConfig->baro_i2c_address = 0;
-#elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) || defined(DEFAULT_BARO_QMP6988)
+#elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP388) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) ||defined(DEFAULT_BARO_QMP6988)
// All I2C devices shares a default config with address = 0 (per device default)
barometerConfig->baro_bustype = BUSTYPE_I2C;
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE);
@@ -147,7 +154,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
baroSensor_e baroHardware = baroHardwareToUse;
-#if !defined(USE_BARO_BMP085) && !defined(USE_BARO_MS5611) && !defined(USE_BARO_SPI_MS5611) && !defined(USE_BARO_BMP280) && !defined(USE_BARO_SPI_BMP280)&& !defined(USE_BARO_QMP6988) && !defined(USE_BARO_SPI_QMP6988)
+#if !defined(USE_BARO_BMP085) && !defined(USE_BARO_MS5611) && !defined(USE_BARO_SPI_MS5611) && !defined(USE_BARO_BMP388) && !defined(USE_BARO_BMP280) && !defined(USE_BARO_SPI_BMP280)&& !defined(USE_BARO_QMP6988) && !defined(USE_BARO_SPI_QMP6988)
UNUSED(dev);
#endif
@@ -218,6 +225,23 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
#endif
FALLTHROUGH;
+ case BARO_BMP388:
+#if defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388)
+ {
+ static bmp388Config_t defaultBMP388Config;
+
+ defaultBMP388Config.eocTag = barometerConfig()->baro_eoc_tag;
+
+ static const bmp388Config_t *bmp388Config = &defaultBMP388Config;
+
+ if (bmp388Detect(bmp388Config, dev)) {
+ baroHardware = BARO_BMP388;
+ break;
+ }
+ }
+#endif
+ FALLTHROUGH;
+
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(dev)) {
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 6e15e205f6..a0b06dd732 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -30,7 +30,8 @@ typedef enum {
BARO_MS5611 = 3,
BARO_BMP280 = 4,
BARO_LPS = 5,
- BARO_QMP6988 = 6
+ BARO_QMP6988 = 6,
+ BARO_BMP388 = 7
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48
diff --git a/src/main/target/NUCLEOF7/target.h b/src/main/target/NUCLEOF7/target.h
index c18dd2db0f..080855dc4d 100644
--- a/src/main/target/NUCLEOF7/target.h
+++ b/src/main/target/NUCLEOF7/target.h
@@ -61,6 +61,9 @@
#define USE_BARO
#define USE_FAKE_BARO
#define USE_BARO_MS5611
+#define USE_BARO_SPI_BMP388
+#define BARO_SPI_INSTANCE SPI1
+#define BARO_CS_PIN SPI1_NSS_PIN
#define USABLE_TIMER_CHANNEL_COUNT 11
diff --git a/src/main/target/NUCLEOF7/target.mk b/src/main/target/NUCLEOF7/target.mk
index bc21e51f75..63387ef756 100644
--- a/src/main/target/NUCLEOF7/target.mk
+++ b/src/main/target/NUCLEOF7/target.mk
@@ -6,6 +6,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_fake.c \
drivers/barometer/barometer_ms5611.c \
+ drivers/barometer/barometer_bmp388.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
diff --git a/src/main/target/NUCLEOF722/target.h b/src/main/target/NUCLEOF722/target.h
index ba982bdfd3..61eb2db4fa 100644
--- a/src/main/target/NUCLEOF722/target.h
+++ b/src/main/target/NUCLEOF722/target.h
@@ -61,6 +61,7 @@
#define USE_BARO
#define USE_FAKE_BARO
#define USE_BARO_MS5611
+#define USE_BARO_BMP388
#define USABLE_TIMER_CHANNEL_COUNT 9
diff --git a/src/main/target/NUCLEOF722/target.mk b/src/main/target/NUCLEOF722/target.mk
index 9dc262ef95..f64e96aad9 100644
--- a/src/main/target/NUCLEOF722/target.mk
+++ b/src/main/target/NUCLEOF722/target.mk
@@ -6,6 +6,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_fake.c \
drivers/barometer/barometer_ms5611.c \
+ drivers/barometer/barometer_bmp388.c \
drivers/compass/compass_fake.c \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
diff --git a/src/main/target/NUCLEOH743/target.h b/src/main/target/NUCLEOH743/target.h
index 87c24e1e6e..b6101eb1ba 100644
--- a/src/main/target/NUCLEOH743/target.h
+++ b/src/main/target/NUCLEOH743/target.h
@@ -147,8 +147,10 @@
#define HMC5883_CS_PIN NONE
#define USE_BARO
+#define USE_BARO_LPS
#define USE_BARO_BMP085
#define USE_BARO_BMP280
+#define USE_BARO_BMP388
#define USE_BARO_MS5611
#define USE_BARO_SPI_BMP280
#define BMP280_SPI_INSTANCE NULL
diff --git a/src/main/target/NUCLEOH743/target.mk b/src/main/target/NUCLEOH743/target.mk
index acf71c7132..7c65aee48f 100644
--- a/src/main/target/NUCLEOH743/target.mk
+++ b/src/main/target/NUCLEOH743/target.mk
@@ -21,6 +21,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
+ drivers/barometer/barometer_bmp388.c \
drivers/barometer/barometer_ms5611.c \
drivers/compass/compass_hmc5883l.c \
drivers/max7456.c \
diff --git a/src/main/target/common_unified.h b/src/main/target/common_unified.h
index a0a5f1fded..5d4ef4dd35 100644
--- a/src/main/target/common_unified.h
+++ b/src/main/target/common_unified.h
@@ -60,6 +60,8 @@
#define USE_BARO_SPI_MS5611
#define USE_BARO_BMP280
#define USE_BARO_SPI_BMP280
+#define USE_BARO_BMP388
+#define USE_BARO_SPI_BMP388
#define USE_BARO_LPS
#define USE_BARO_SPI_LPS
#define USE_BARO_QMP6988
diff --git a/src/test/Makefile b/src/test/Makefile
index aca78e8be4..465ac49509 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -65,6 +65,15 @@ baro_bmp280_unittest_DEFINES := \
USE_BARO_BMP280= \
USE_BARO_SPI_BMP280=
+baro_bmp388_unittest_SRC := \
+ $(USER_DIR)/common/maths.c \
+ $(USER_DIR)/drivers/barometer/barometer_bmp388.c
+
+baro_bmp388_unittest_DEFINES := \
+ USE_EXTI= \
+ USE_BARO_BMP388= \
+ USE_BARO_SPI_BMP388=
+
baro_ms5611_unittest_SRC := \
$(USER_DIR)/drivers/barometer/barometer_ms5611.c
diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc
index 52d7ddd0cc..6dfb32b7e8 100644
--- a/src/test/unit/baro_bmp280_unittest.cc
+++ b/src/test/unit/baro_bmp280_unittest.cc
@@ -23,7 +23,7 @@ extern "C" {
#include "drivers/barometer/barometer.h"
#include "drivers/bus.h"
-void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+void bmp280Calculate(int32_t *pressure, int32_t *temperature);
extern uint32_t bmp280_up;
extern uint32_t bmp280_ut;
@@ -75,7 +75,7 @@ TEST(baroBmp280Test, TestBmp280Calculate)
bmp280_cal.dig_P9 = 6000;
// when
- bmp280_calculate(&pressure, &temperature);
+ bmp280Calculate(&pressure, &temperature);
// then
EXPECT_EQ(100653, pressure); // 100653 Pa
@@ -105,7 +105,7 @@ TEST(baroBmp280Test, TestBmp280CalculateHighP)
bmp280_cal.dig_P9 = 6000;
// when
- bmp280_calculate(&pressure, &temperature);
+ bmp280Calculate(&pressure, &temperature);
// then
EXPECT_EQ(135382, pressure); // 135385 Pa
@@ -135,7 +135,7 @@ TEST(baroBmp280Test, TestBmp280CalculateZeroP)
bmp280_cal.dig_P9 = 6000;
// when
- bmp280_calculate(&pressure, &temperature);
+ bmp280Calculate(&pressure, &temperature);
// then
EXPECT_EQ(0, pressure); // P1=0 trips pressure to 0 Pa, avoiding division by zero
diff --git a/src/test/unit/baro_bmp388_unittest.cc b/src/test/unit/baro_bmp388_unittest.cc
new file mode 100644
index 0000000000..c37fab0f3b
--- /dev/null
+++ b/src/test/unit/baro_bmp388_unittest.cc
@@ -0,0 +1,181 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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.
+ *
+ * Cleanflight 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 Cleanflight. If not, see .
+ */
+#include
+
+extern "C" {
+
+#include "platform.h"
+#include "target.h"
+#include "drivers/barometer/barometer.h"
+#include "drivers/bus.h"
+
+void bmp388Calculate(int32_t *pressure, int32_t *temperature);
+
+extern uint32_t bmp388_up;
+extern uint32_t bmp388_ut;
+extern int64_t t_lin; // when temperature is calculated this is updated, the result is used in the pressure calculations
+
+typedef struct bmp388_calib_param_s {
+ uint16_t T1;
+ uint16_t T2;
+ int8_t T3;
+ int16_t P1;
+ int16_t P2;
+ int8_t P3;
+ int8_t P4;
+ uint16_t P5;
+ uint16_t P6;
+ int8_t P7;
+ int8_t P8;
+ int16_t P9;
+ int8_t P10;
+ int8_t P11;
+} __attribute__((packed)) bmp388_calib_param_t; // packed as we read directly from the device into this structure.
+
+bmp388_calib_param_t bmp388_cal;
+
+} // extern "C"
+
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+void baroBmp388ConfigureZeroCalibration(void)
+{
+ bmp388_cal = {
+ .T1 = 0,
+ .T2 = 0,
+ .T3 = 0,
+ .P1 = 0,
+ .P2 = 0,
+ .P3 = 0,
+ .P4 = 0,
+ .P5 = 0,
+ .P6 = 0,
+ .P7 = 0,
+ .P8 = 0,
+ .P9 = 0,
+ .P10 = 0,
+ .P11 = 0
+ };
+}
+
+void baroBmp388ConfigureSampleCalibration(void)
+{
+ bmp388_cal = {
+ .T1 = 27772,
+ .T2 = 18638,
+ .T3 = -10,
+ .P1 = 878,
+ .P2 = -2023,
+ .P3 = 35,
+ .P4 = 0,
+ .P5 = 24476,
+ .P6 = 30501,
+ .P7 = -13,
+ .P8 = -10,
+ .P9 = 16545,
+ .P10 = 21,
+ .P11 = -60
+ };
+}
+
+TEST(baroBmp388Test, TestBmp388CalculateWithZeroCalibration)
+{
+ // given
+ int32_t pressure, temperature;
+ bmp388_up = 0; // uncompensated pressure value
+ bmp388_ut = 0; // uncompensated temperature value
+ t_lin = 0;
+
+ // and
+ baroBmp388ConfigureZeroCalibration();
+
+ // when
+ bmp388Calculate(&pressure, &temperature);
+
+ // then
+ EXPECT_EQ(0, temperature);
+ EXPECT_EQ(0, t_lin);
+
+ // and
+ EXPECT_EQ(0, pressure);
+}
+
+TEST(baroBmp388Test, TestBmp388CalculateWithSampleCalibration)
+{
+ // given
+ int32_t pressure, temperature;
+ bmp388_up = 7323488; // uncompensated pressure value
+ bmp388_ut = 9937920; // uncompensated temperature value
+ t_lin = 0;
+
+ // and
+ baroBmp388ConfigureSampleCalibration();
+
+ // when
+ bmp388Calculate(&pressure, &temperature);
+
+ // then
+ EXPECT_EQ(4880, temperature); // 48.80 degrees C
+ EXPECT_NE(0, t_lin);
+
+ // and
+ EXPECT_EQ(39535, pressure); // 39535 Pa
+}
+
+// STUBS
+
+extern "C" {
+
+void delay(uint32_t) {}
+bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;}
+bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;}
+
+void spiBusSetDivisor() {
+}
+
+void spiBusTransactionInit() {
+}
+
+void spiPreinitByIO(IO_t) {
+}
+
+void IOConfigGPIO() {
+}
+
+void IOHi() {
+}
+
+IO_t IOGetByTag(ioTag_t) {
+ return IO_NONE;
+}
+
+void IOInit() {
+}
+
+void EXTIHandlerInit(extiCallbackRec_t *, extiHandlerCallback *) {
+}
+
+void EXTIConfig(IO_t, extiCallbackRec_t *, int, ioConfig_t, extiTrigger_t) {
+}
+
+void EXTIEnable(IO_t, bool) {
+}
+
+
+} // extern "C"
diff --git a/src/test/unit/baro_ms5611_unittest.cc b/src/test/unit/baro_ms5611_unittest.cc
index 0b7c1e8304..cef4522b4d 100644
--- a/src/test/unit/baro_ms5611_unittest.cc
+++ b/src/test/unit/baro_ms5611_unittest.cc
@@ -23,8 +23,8 @@ extern "C" {
#include "drivers/barometer/barometer.h"
#include "drivers/bus.h"
-int8_t ms5611_crc(uint16_t *prom);
-void ms5611_calculate(int32_t *pressure, int32_t *temperature);
+int8_t ms5611CRC(uint16_t *prom);
+void ms5611Calculate(int32_t *pressure, int32_t *temperature);
extern uint16_t ms5611_c[8];
extern uint32_t ms5611_up;
@@ -40,10 +40,10 @@ extern uint32_t ms5611_ut;
TEST(baroMS5611Test, TestValidMs5611Crc)
{
// given
- uint16_t ms5611_prom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x450B};
+ uint16_t ms5611Prom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x450B};
// when
- int8_t result = ms5611_crc(ms5611_prom);
+ int8_t result = ms5611CRC(ms5611Prom);
// then
EXPECT_EQ(0, result);
@@ -52,10 +52,10 @@ TEST(baroMS5611Test, TestValidMs5611Crc)
TEST(baroMS5611Test, TestInvalidMs5611Crc)
{
// given
- uint16_t ms5611_prom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x4500};
+ uint16_t ms5611Prom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x4500};
// when
- int8_t result = ms5611_crc(ms5611_prom);
+ int8_t result = ms5611CRC(ms5611Prom);
// then
EXPECT_EQ(-1, result);
@@ -64,10 +64,10 @@ TEST(baroMS5611Test, TestInvalidMs5611Crc)
TEST(baroMS5611Test, TestMs5611AllZeroProm)
{
// given
- uint16_t ms5611_prom[] = {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000};
+ uint16_t ms5611Prom[] = {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000};
// when
- int8_t result = ms5611_crc(ms5611_prom);
+ int8_t result = ms5611CRC(ms5611Prom);
// then
EXPECT_EQ(-1, result);
@@ -76,10 +76,10 @@ TEST(baroMS5611Test, TestMs5611AllZeroProm)
TEST(baroMS5611Test, TestMs5611AllOnesProm)
{
// given
- uint16_t ms5611_prom[] = {0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF};
+ uint16_t ms5611Prom[] = {0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF,0xFFFF};
// when
- int8_t result = ms5611_crc(ms5611_prom);
+ int8_t result = ms5611CRC(ms5611Prom);
// then
EXPECT_EQ(-1, result);
@@ -96,7 +96,7 @@ TEST(baroMS5611Test, TestMs5611CalculatePressureGT20Deg)
ms5611_ut = 8569150; // Digital temperature value from MS5611 datasheet
// when
- ms5611_calculate(&pressure, &temperature);
+ ms5611Calculate(&pressure, &temperature);
// then
EXPECT_EQ(2007, temperature); // 20.07 deg C
@@ -114,7 +114,7 @@ TEST(baroMS5611Test, TestMs5611CalculatePressureLT20Deg)
ms5611_ut = 8069150; // Digital temperature value
// when
- ms5611_calculate(&pressure, &temperature);
+ ms5611Calculate(&pressure, &temperature);
// then
EXPECT_EQ(205, temperature); // 2.05 deg C
@@ -132,7 +132,7 @@ TEST(baroMS5611Test, TestMs5611CalculatePressureLTMinus15Deg)
ms5611_ut = 7369150; // Digital temperature value
// when
- ms5611_calculate(&pressure, &temperature);
+ ms5611Calculate(&pressure, &temperature);
// then
EXPECT_EQ(-2710, temperature); // -27.10 deg C