diff --git a/make/source.mk b/make/source.mk
index 4a28c95c0b..0599bc73cb 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -278,6 +278,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/barometer/barometer_fake.c \
drivers/barometer/barometer_ms5611.c \
drivers/barometer/barometer_lps.c \
+ drivers/barometer/barometer_qmp6988.c \
drivers/bus_i2c_config.c \
drivers/bus_spi_config.c \
drivers/bus_spi_pinconfig.c \
diff --git a/src/main/drivers/barometer/barometer_qmp6988.c b/src/main/drivers/barometer/barometer_qmp6988.c
new file mode 100755
index 0000000000..4d046ad0af
--- /dev/null
+++ b/src/main/drivers/barometer/barometer_qmp6988.c
@@ -0,0 +1,304 @@
+/*
+ * 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
+#include
+#include
+#include "build/build_config.h"
+#include "build/debug.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/serial.h"
+#include "io/serial.h"
+#include "common/printf.h"
+#include "barometer_qmp6988.h"
+
+#if defined(USE_BARO) && (defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988))
+
+typedef struct qmp6988_calib_param_s {
+ float Coe_a0;
+ float Coe_a1;
+ float Coe_a2;
+ float Coe_b00;
+ float Coe_bt1;
+ float Coe_bt2;
+ float Coe_bp1;
+ float Coe_b11;
+ float Coe_bp2;
+ float Coe_b12;
+ float Coe_b21;
+ float Coe_bp3;
+} qmp6988_calib_param_t;
+
+static uint8_t qmp6988_chip_id = 0;
+STATIC_UNIT_TESTED qmp6988_calib_param_t qmp6988_cal;
+// uncompensated pressure and temperature
+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_UNIT_TESTED void qmp6988_calculate(int32_t *pressure, int32_t *temperature);
+
+
+void qmp6988BusInit(busDevice_t *busdev)
+{
+#ifdef USE_BARO_SPI_QMP6988
+ 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);
+ spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD); // XXX
+ }
+#else
+ UNUSED(busdev);
+#endif
+}
+
+void qmp6988BusDeinit(busDevice_t *busdev)
+{
+#ifdef USE_BARO_SPI_QMP6988
+ if (busdev->bustype == BUSTYPE_SPI) {
+ IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU);
+ IORelease(busdev->busdev_u.spi.csnPin);
+ IOInit(busdev->busdev_u.spi.csnPin, OWNER_SPI_PREINIT, 0);
+ }
+#else
+ UNUSED(busdev);
+#endif
+}
+
+bool qmp6988Detect(baroDev_t *baro)
+{
+ uint8_t databuf[25] = {0};
+ int Coe_a0_;
+ int Coe_a1_;
+ int Coe_a2_;
+ int Coe_b00_;
+ int Coe_bt1_;
+ int Coe_bt2_;
+ int Coe_bp1_;
+ int Coe_b11_;
+ int Coe_bp2_;
+ int Coe_b12_;
+ int Coe_b21_;
+ int Coe_bp3_;
+ u16 lb=0,hb=0;
+ u32 lw=0,hw=0,temp1,temp2;
+
+ delay(20);
+
+ busDevice_t *busdev = &baro->busdev;
+ bool defaultAddressApplied = false;
+
+ qmp6988BusInit(busdev);
+
+ if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
+ busdev->busdev_u.i2c.address = QMP6988_I2C_ADDR;
+ defaultAddressApplied = true;
+ }
+
+ busReadRegisterBuffer(busdev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1); /* read Chip Id */
+
+#if 1
+ if (qmp6988_chip_id != QMP6988_DEFAULT_CHIP_ID) {
+ qmp6988BusDeinit(busdev);
+ if (defaultAddressApplied) {
+ busdev->busdev_u.i2c.address = 0;
+ }
+ return false;
+ }
+#endif
+
+ // IO_SETUP
+ // qmp6988WriteRegister(busdev, QMP6988_IO_SETUP_REG, QMP6988_MODE);
+
+ // SetIIR
+ busWriteRegister(busdev, QMP6988_SET_IIR_REG, 0x05);
+
+ //read OTP
+ busReadRegisterBuffer(busdev, QMP6988_COE_B00_1_REG, databuf, 25);
+
+
+ //algo OTP
+ hw = databuf[0];
+ lw = databuf[1];
+ temp1 = (hw<<12) | (lw<<4);
+
+ hb = databuf[2];
+ lb = databuf[3];
+ Coe_bt1_ = (short)((hb<<8) | lb);
+
+ hb = databuf[4];
+ lb = databuf[5];
+ Coe_bt2_ = (short)((hb<<8) | lb);
+
+ hb = databuf[6];
+ lb = databuf[7];
+ Coe_bp1_ = (short)((hb<<8) | lb);
+
+ hb = databuf[8];
+ lb = databuf[9];
+ Coe_b11_ = (short)((hb<<8) | lb);
+
+ hb = databuf[10];
+ lb = databuf[11];
+ Coe_bp2_ = (short)((hb<<8) | lb);
+
+ hb = databuf[12];
+ lb = databuf[13];
+ Coe_b12_ = (short)((hb<<8) | lb);
+
+ hb = databuf[14];
+ lb = databuf[15];
+ Coe_b21_ = (short)((hb<<8) | lb);
+
+ hb = databuf[16];
+ lb = databuf[17];
+ Coe_bp3_ = (short)((hb<<8) | lb);
+
+ hw = databuf[18];
+ lw = databuf[19];
+ temp2 = (hw<<12) | (lw<<4);
+
+ hb = databuf[20];
+ lb = databuf[21];
+ Coe_a1_ = (short)((hb<<8) | lb);
+
+ hb = databuf[22];
+ lb = databuf[23];
+ Coe_a2_ = (short)((hb<<8) | lb);
+
+ hb = databuf[24];
+
+
+ temp1 = temp1|((hb&0xf0)>>4);
+ if(temp1&0x80000)
+ Coe_b00_ = ((int)temp1 - (int)0x100000);
+ else
+ Coe_b00_ = temp1;
+
+ temp2 = temp2|(hb&0x0f);
+ if(temp2&0x80000)
+ Coe_a0_ = ((int)temp2 - (int)0x100000);
+ else
+ Coe_a0_ = temp2;
+
+ qmp6988_cal.Coe_a0=(float)Coe_a0_/16.0;
+ qmp6988_cal.Coe_a1=(-6.30E-03)+(4.30E-04)*(float)Coe_a1_/32767.0;
+ qmp6988_cal.Coe_a2=(-1.9E-11)+(1.2E-10)*(float)Coe_a2_/32767.0;
+
+ qmp6988_cal.Coe_b00 = Coe_b00_/16.0;
+ qmp6988_cal.Coe_bt1 = (1.00E-01)+(9.10E-02)*(float)Coe_bt1_/32767.0;
+ qmp6988_cal.Coe_bt2= (1.20E-08)+(1.20E-06)*(float)Coe_bt2_/32767.0;
+
+ qmp6988_cal.Coe_bp1 = (3.30E-02)+(1.90E-02)*(float)Coe_bp1_/32767.0;
+ qmp6988_cal.Coe_b11= (2.10E-07)+(1.40E-07)*(float)Coe_b11_/32767.0;
+
+ qmp6988_cal.Coe_bp2 = (-6.30E-10)+(3.50E-10)*(float)Coe_bp2_/32767.0;
+ qmp6988_cal.Coe_b12= (2.90E-13)+(7.60E-13)*(float)Coe_b12_/32767.0;
+
+
+ qmp6988_cal.Coe_b21 = (2.10E-15)+(1.20E-14)*(float)Coe_b21_/32767.0;
+ qmp6988_cal.Coe_bp3= (1.30E-16)+(7.90E-17)*(float)Coe_bp3_/32767.0;
+
+ // Set power mode and sample times
+ busWriteRegister(busdev, QMP6988_CTRL_MEAS_REG, 0x7B);
+
+ // 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;
+ // only _up part is executed, and gets both temperature and pressure
+ baro->start_up = qmp6988_start_up;
+ baro->get_up = qmp6988_get_up;
+ 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;
+
+ return true;
+}
+
+static void qmp6988_start_ut(baroDev_t *baro)
+{
+ UNUSED(baro);
+ // dummy
+}
+
+static void qmp6988_get_ut(baroDev_t *baro)
+{
+ UNUSED(baro);
+ // dummy
+}
+
+static void qmp6988_start_up(baroDev_t *baro)
+{
+ // start measurement
+ busWriteRegister(&baro->busdev, QMP6988_CTRL_MEAS_REG, 0x7B);
+}
+
+static void qmp6988_get_up(baroDev_t *baro)
+{
+ uint8_t data[QMP6988_DATA_FRAME_SIZE];
+
+ // read data from sensor
+ busReadRegisterBuffer(&baro->busdev, QMP6988_PRESSURE_MSB_REG, data, QMP6988_DATA_FRAME_SIZE);
+ qmp6988_up = (int32_t)((((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2] ));
+ qmp6988_ut = (int32_t)((((uint32_t)(data[3])) << 16) | (((uint32_t)(data[4])) << 8) | ((uint32_t)data[5]));
+}
+
+// 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)
+{
+ int32_t var1;
+ float T;
+
+ var1=adc_T-1024*1024*8;
+ T= qmp6988_cal.Coe_a0+qmp6988_cal.Coe_a1*var1+qmp6988_cal.Coe_a2*var1*var1;
+
+ return T;
+}
+
+
+
+STATIC_UNIT_TESTED void qmp6988_calculate(int32_t *pressure, int32_t *temperature)
+{
+ float tr,pr;
+ int32_t Dp;
+
+ tr = qmp6988_compensate_T(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
+ +qmp6988_cal.Coe_b21*Dp*Dp*tr+qmp6988_cal.Coe_bp3*Dp*Dp*Dp;
+
+ if (pr)
+ *pressure = (int32_t)(pr);
+ if (tr)
+ *temperature = (int32_t)tr/256;
+}
+
+#endif
diff --git a/src/main/drivers/barometer/barometer_qmp6988.h b/src/main/drivers/barometer/barometer_qmp6988.h
new file mode 100755
index 0000000000..da0e99788d
--- /dev/null
+++ b/src/main/drivers/barometer/barometer_qmp6988.h
@@ -0,0 +1,58 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#define QMP6988_I2C_ADDR (0x70)
+#define QMP6988_DEFAULT_CHIP_ID (0x5c)
+
+#define QMP6988_CHIP_ID_REG (0xD1) /* Chip ID Register */
+#define QMP6988_IO_SETUP_REG (0xF5)
+#define QMP6988_SET_IIR_REG (0xF1)
+#define QMP6988_CTRL_MEAS_REG (0xF4)
+#define QMP6988_COE_B00_1_REG (0xA0)
+#define QMP6988_PRESSURE_MSB_REG (0xF7) /* Pressure MSB Register */
+#define QMP6988_PRESSURE_LSB_REG (0xF8) /* Pressure LSB Register */
+#define QMP6988_PRESSURE_XLSB_REG (0xF9) /* Pressure XLSB Register */
+#define QMP6988_TEMPERATURE_MSB_REG (0xFA) /* Temperature MSB Reg */
+#define QMP6988_TEMPERATURE_LSB_REG (0xFB) /* Temperature LSB Reg */
+#define QMP6988_TEMPERATURE_XLSB_REG (0xFC) /* Temperature XLSB Reg */
+#define QMP6988_DATA_FRAME_SIZE 6
+#define QMP6988_FORCED_MODE (0x01)
+
+
+
+#define QMP6988_OVERSAMP_SKIPPED (0x00)
+#define QMP6988_OVERSAMP_1X (0x01)
+#define QMP6988_OVERSAMP_2X (0x02)
+#define QMP6988_OVERSAMP_4X (0x03)
+#define QMP6988_OVERSAMP_8X (0x04)
+#define QMP6988_OVERSAMP_16X (0x05)
+
+// configure pressure and temperature oversampling, forced sampling mode
+#define QMP6988_PRESSURE_OSR (QMP6988_OVERSAMP_8X)
+#define QMP6988_TEMPERATURE_OSR (QMP6988_OVERSAMP_1X)
+#define QMP6988_MODE (QMP6988_PRESSURE_OSR << 2 | QMP6988_TEMPERATURE_OSR << 5 | QMP6988_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 qmp6988Detect(baroDev_t *baro);
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 75dc2cc810..a7f3546f53 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -110,7 +110,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"
+ "AUTO", "NONE", "BMP085", "MS5611", "BMP280", "LPS", "QMP6988"
};
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index ba07070002..a310962c9c 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -33,6 +33,7 @@
#include "drivers/barometer/barometer.h"
#include "drivers/barometer/barometer_bmp085.h"
#include "drivers/barometer/barometer_bmp280.h"
+#include "drivers/barometer/barometer_qmp6988.h"
#include "drivers/barometer/barometer_fake.h"
#include "drivers/barometer/barometer_ms5611.h"
#include "drivers/barometer/barometer_lps.h"
@@ -65,7 +66,7 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
// a. Precedence is in the order of popularity; 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))
+#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(USE_BARO_SPI_BMP280)
#define DEFAULT_BARO_SPI_BMP280
@@ -78,6 +79,12 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
#else
#define DEFAULT_BARO_MS5611
#endif
+#elif defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
+#if defined(USE_BARO_SPI_QMP6988)
+#define DEFAULT_BARO_SPI_QMP6988
+#else
+#define DEFAULT_BARO_QMP6988
+#endif
#elif defined(USE_BARO_SPI_LPS)
#define DEFAULT_BARO_SPI_LPS
#elif defined(DEFAULT_BARO_BMP085)
@@ -97,13 +104,19 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
barometerConfig->baro_spi_csn = IO_TAG(MS5611_CS_PIN);
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
barometerConfig->baro_i2c_address = 0;
+#elif defined(DEFAULT_BARO_SPI_QMP6988)
+ barometerConfig->baro_bustype = BUSTYPE_SPI;
+ barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(QMP6988_SPI_INSTANCE));
+ barometerConfig->baro_spi_csn = IO_TAG(QMP6988_CS_PIN);
+ barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
+ barometerConfig->baro_i2c_address = 0;
#elif defined(DEFAULT_BARO_SPI_LPS)
barometerConfig->baro_bustype = BUSTYPE_SPI;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(LPS_SPI_INSTANCE));
barometerConfig->baro_spi_csn = IO_TAG(LPS_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)
+#elif defined(DEFAULT_BARO_MS5611) || 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);
@@ -136,7 +149,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)
+#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)
UNUSED(dev);
#endif
@@ -212,7 +225,15 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
}
#endif
FALLTHROUGH;
-
+
+ case BARO_QMP6988:
+#if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
+ if (qmp6988Detect(dev)) {
+ baroHardware = BARO_QMP6988;
+ break;
+ }
+#endif
+ FALLTHROUGH;
case BARO_NONE:
baroHardware = BARO_NONE;
break;
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index de3c12ec17..4a2604f0a0 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -26,7 +26,8 @@ typedef enum {
BARO_BMP085 = 2,
BARO_MS5611 = 3,
BARO_BMP280 = 4,
- BARO_LPS = 5
+ BARO_LPS = 5,
+ BARO_QMP6988 = 6
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48