Merge pull request #5965 from loopur/mybranch

Add barometer sensor QMP6988

make/source.mk

@@ -256,6 +256,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 \

src/main/drivers/barometer/barometer_qmp6988.c

@@ -0,0 +1,329 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <platform.h>
+#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))
+
+#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_PWR_SAMPLE_MODE			(0x7B)
+
+#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)
+#define T_MEASURE_PER_OSRS_MAX           			(37)
+#define T_SETUP_PRESSURE_MAX             			(10)
+
+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); 
+        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);
+    }
+#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 (qmp6988_chip_id != QMP6988_DEFAULT_CHIP_ID) {
+        qmp6988BusDeinit(busdev);
+        if (defaultAddressApplied) {
+            busdev->busdev_u.i2c.address = 0;
+        }
+        return false;
+    }
+
+    // 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, QMP6988_PWR_SAMPLE_MODE);	
+
+    // 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, QMP6988_PWR_SAMPLE_MODE);	
+}
+
+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

src/main/drivers/barometer/barometer_qmp6988.h

@@ -0,0 +1,20 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool qmp6988Detect(baroDev_t *baro);

src/main/interface/settings.c

@@ -115,7 +115,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)

src/main/sensors/barometer.c

@@ -36,6 +36,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"
@@ -68,7 +69,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
@@ -81,6 +82,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)
@@ -100,13 +107,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);
@@ -139,7 +152,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
 
@@ -215,7 +228,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;

src/main/sensors/barometer.h

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