From 53167b161fb7206f34714c92cceed51b81a6f03c Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 5 Jan 2019 23:39:56 +0100 Subject: [PATCH] BMP388 - Add support for BMP388 barometer. BMP388 - Move the static assert. BMP388 - Build faster when the baro driver is not enabled. BMP388 - Fix spi init due to changes in master. BMP388 - Add missing bmp388 unit test files. BMP388 - Remove debug code. BMP388 - Prepare EXTI/EOC handling for unified targets. BMP388 - enable on unified targets. BMP388 - Add support to NUCLEOF722. BMP388 - Add support to NUCLEOH743. BMP388 - Add BMP388 (via SPI) support to NUCLEOF7 * For some CI visibility on the conditional baro SPI code. NUCLEOH743 - Add LPS baro for more CI visibility. Remove whitespace, as requested. Move barometer `#defines` into the implementations. Cleanup style of method names in baro drivers. --- src/main/cli/settings.c | 2 +- src/main/drivers/barometer/barometer.h | 5 + src/main/drivers/barometer/barometer_bmp085.c | 54 +-- src/main/drivers/barometer/barometer_bmp085.h | 2 - src/main/drivers/barometer/barometer_bmp280.c | 78 +++- src/main/drivers/barometer/barometer_bmp280.h | 39 -- src/main/drivers/barometer/barometer_bmp388.c | 409 ++++++++++++++++++ src/main/drivers/barometer/barometer_bmp388.h | 34 ++ src/main/drivers/barometer/barometer_lps.c | 18 +- src/main/drivers/barometer/barometer_ms5611.c | 59 +-- src/main/drivers/barometer/barometer_ms5611.h | 3 - .../drivers/barometer/barometer_qmp6988.c | 34 +- src/main/sensors/barometer.c | 36 +- src/main/sensors/barometer.h | 3 +- src/main/target/NUCLEOF7/target.h | 3 + src/main/target/NUCLEOF7/target.mk | 1 + src/main/target/NUCLEOF722/target.h | 1 + src/main/target/NUCLEOF722/target.mk | 1 + src/main/target/NUCLEOH743/target.h | 2 + src/main/target/NUCLEOH743/target.mk | 1 + src/main/target/common_unified.h | 2 + src/test/Makefile | 9 + src/test/unit/baro_bmp280_unittest.cc | 8 +- src/test/unit/baro_bmp388_unittest.cc | 181 ++++++++ src/test/unit/baro_ms5611_unittest.cc | 26 +- 25 files changed, 842 insertions(+), 169 deletions(-) create mode 100644 src/main/drivers/barometer/barometer_bmp388.c create mode 100644 src/main/drivers/barometer/barometer_bmp388.h create mode 100644 src/test/unit/baro_bmp388_unittest.cc 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