diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c
index 3edb55c5f2..6591c72a1c 100644
--- a/src/main/drivers/barometer_bmp280.c
+++ b/src/main/drivers/barometer_bmp280.c
@@ -90,16 +90,16 @@ typedef struct bmp280_calib_param_t {
static uint8_t bmp280_chip_id = 0;
static bool bmp280InitDone = false;
-static bmp280_calib_param_t bmp280_cal;
+STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
// uncompensated pressure and temperature
-static int32_t bmp280_up = 0;
-static int32_t bmp280_ut = 0;
+STATIC_UNIT_TESTED int32_t bmp280_up = 0;
+STATIC_UNIT_TESTED int32_t bmp280_ut = 0;
static void bmp280_start_ut(void);
static void bmp280_get_ut(void);
static void bmp280_start_up(void);
static void bmp280_get_up(void);
-static void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
bool bmp280Detect(baro_t *baro)
{
@@ -160,9 +160,9 @@ static void bmp280_get_up(void)
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
}
-// Returns temperature in DegC, float precision. Output value of “51.23” equals 51.23 DegC.
+// Returns temperature in DegC, float precision. Output value of "51.23" equals 51.23 DegC.
// t_fine carries fine temperature as global value
-float bmp280_compensate_T(int32_t adc_T)
+static float bmp280_compensate_T(int32_t adc_T)
{
float var1, var2, T;
@@ -174,8 +174,8 @@ float bmp280_compensate_T(int32_t adc_T)
return T;
}
-// Returns pressure in Pa as float. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa
-float bmp280_compensate_P(int32_t adc_P)
+// Returns pressure in Pa as float. Output value of "96386.2" equals 96386.2 Pa = 963.862 hPa
+static float bmp280_compensate_P(int32_t adc_P)
{
float var1, var2, p;
var1 = ((float)bmp280_cal.t_fine / 2.0f) - 64000.0f;
@@ -196,7 +196,7 @@ float bmp280_compensate_P(int32_t adc_P)
return p;
}
-static void bmp280_calculate(int32_t *pressure, int32_t *temperature)
+STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature)
{
// calculate
float t, p;
diff --git a/src/test/Makefile b/src/test/Makefile
index b48353663c..5bbc50b7be 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -512,6 +512,29 @@ $(OBJECT_DIR)/baro_bmp085_unittest : \
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+$(OBJECT_DIR)/drivers/barometer_bmp280.o : \
+ $(USER_DIR)/drivers/barometer_bmp280.c \
+ $(USER_DIR)/drivers/barometer_bmp280.h \
+ $(GTEST_HEADERS)
+
+ @mkdir -p $(dir $@)
+ $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_bmp280.c -o $@
+
+$(OBJECT_DIR)/baro_bmp280_unittest.o : \
+ $(TEST_DIR)/baro_bmp280_unittest.cc \
+ $(USER_DIR)/drivers/barometer_bmp280.h \
+ $(GTEST_HEADERS)
+
+ @mkdir -p $(dir $@)
+ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_bmp280_unittest.cc -o $@
+
+$(OBJECT_DIR)/baro_bmp280_unittest : \
+ $(OBJECT_DIR)/drivers/barometer_bmp280.o \
+ $(OBJECT_DIR)/baro_bmp280_unittest.o \
+ $(OBJECT_DIR)/gtest_main.a
+
+ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
$(OBJECT_DIR)/sensors/boardalignment.o : \
$(USER_DIR)/sensors/boardalignment.c \
$(USER_DIR)/sensors/boardalignment.h \
diff --git a/src/test/unit/baro_bmp280_unittest.cc b/src/test/unit/baro_bmp280_unittest.cc
new file mode 100644
index 0000000000..50f1b56307
--- /dev/null
+++ b/src/test/unit/baro_bmp280_unittest.cc
@@ -0,0 +1,154 @@
+/*
+ * 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" {
+
+ void bmp280_calculate(int32_t *pressure, int32_t *temperature);
+ extern uint32_t bmp280_up;
+ extern uint32_t bmp280_ut;
+
+typedef struct bmp280_calib_param_t {
+ uint16_t dig_T1; /* calibration T1 data */
+ int16_t dig_T2; /* calibration T2 data */
+ int16_t dig_T3; /* calibration T3 data */
+ uint16_t dig_P1; /* calibration P1 data */
+ int16_t dig_P2; /* calibration P2 data */
+ int16_t dig_P3; /* calibration P3 data */
+ int16_t dig_P4; /* calibration P4 data */
+ int16_t dig_P5; /* calibration P5 data */
+ int16_t dig_P6; /* calibration P6 data */
+ int16_t dig_P7; /* calibration P7 data */
+ int16_t dig_P8; /* calibration P8 data */
+ int16_t dig_P9; /* calibration P9 data */
+ int32_t t_fine; /* calibration t_fine data */
+} bmp280_calib_param_t;
+
+ bmp280_calib_param_t bmp280_cal;
+}
+
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+
+TEST(baroBmp280Test, TestBmp280Calculate)
+{
+
+ // given
+ int32_t pressure, temperature;
+ bmp280_up = 415148; // Digital pressure value
+ bmp280_ut = 519888; // Digital temperature value
+
+ // and
+ bmp280_cal.dig_T1 = 27504;
+ bmp280_cal.dig_T2 = 26435;
+ bmp280_cal.dig_T3 = -1000;
+ bmp280_cal.dig_P1 = 36477;
+ bmp280_cal.dig_P2 = -10685;
+ bmp280_cal.dig_P3 = 3024;
+ bmp280_cal.dig_P4 = 2855;
+ bmp280_cal.dig_P5 = 140;
+ bmp280_cal.dig_P6 = -7;
+ bmp280_cal.dig_P7 = 15500;
+ bmp280_cal.dig_P8 = -14600;
+ bmp280_cal.dig_P9 = 6000;
+
+ // when
+ bmp280_calculate(&pressure, &temperature);
+
+ // then
+ EXPECT_EQ(100653, pressure); // 100653 Pa
+ EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+
+}
+
+TEST(baroBmp280Test, TestBmp280CalculateHighP)
+{
+
+ // given
+ int32_t pressure, temperature;
+ bmp280_up = 215148; // Digital pressure value
+ bmp280_ut = 519888; // Digital temperature value
+
+ // and
+ bmp280_cal.dig_T1 = 27504;
+ bmp280_cal.dig_T2 = 26435;
+ bmp280_cal.dig_T3 = -1000;
+ bmp280_cal.dig_P1 = 36477;
+ bmp280_cal.dig_P2 = -10685;
+ bmp280_cal.dig_P3 = 3024;
+ bmp280_cal.dig_P4 = 2855;
+ bmp280_cal.dig_P5 = 140;
+ bmp280_cal.dig_P6 = -7;
+ bmp280_cal.dig_P7 = 15500;
+ bmp280_cal.dig_P8 = -14600;
+ bmp280_cal.dig_P9 = 6000;
+
+ // when
+ bmp280_calculate(&pressure, &temperature);
+
+ // then
+ EXPECT_EQ(135382, pressure); // 135385 Pa
+ EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+
+}
+
+TEST(baroBmp280Test, TestBmp280CalculateZeroP)
+{
+
+ // given
+ int32_t pressure, temperature;
+ bmp280_up = 415148; // Digital pressure value
+ bmp280_ut = 519888; // Digital temperature value
+
+ // and
+ bmp280_cal.dig_T1 = 27504;
+ bmp280_cal.dig_T2 = 26435;
+ bmp280_cal.dig_T3 = -1000;
+ bmp280_cal.dig_P1 = 0;
+ bmp280_cal.dig_P2 = -10685;
+ bmp280_cal.dig_P3 = 3024;
+ bmp280_cal.dig_P4 = 2855;
+ bmp280_cal.dig_P5 = 140;
+ bmp280_cal.dig_P6 = -7;
+ bmp280_cal.dig_P7 = 15500;
+ bmp280_cal.dig_P8 = -14600;
+ bmp280_cal.dig_P9 = 6000;
+
+ // when
+ bmp280_calculate(&pressure, &temperature);
+
+ // then
+ EXPECT_EQ(0, pressure); // P1=0 trips pressure to 0 Pa, avoiding division by zero
+ EXPECT_EQ(2500, temperature); // 25.00 degC (data sheet says 25.08)
+
+}
+
+// STUBS
+
+extern "C" {
+
+ void delay(uint32_t) {}
+ bool i2cWrite(uint8_t, uint8_t, uint8_t) {
+ return 1;
+ }
+ bool i2cRead(uint8_t, uint8_t, uint8_t, uint8_t) {
+ return 1;
+ }
+
+}