From 495cfb4d0739aa08036c42e3aed9623b030f867b Mon Sep 17 00:00:00 2001 From: Loopur Date: Fri, 18 May 2018 16:26:07 +0800 Subject: [PATCH] Add barometer sensor QMP6988 --- make/source.mk | 1 + .../drivers/barometer/barometer_qmp6988.c | 304 ++++++++++++++++++ .../drivers/barometer/barometer_qmp6988.h | 58 ++++ src/main/interface/settings.c | 2 +- src/main/sensors/barometer.c | 29 +- src/main/sensors/barometer.h | 3 +- 6 files changed, 391 insertions(+), 6 deletions(-) create mode 100755 src/main/drivers/barometer/barometer_qmp6988.c create mode 100755 src/main/drivers/barometer/barometer_qmp6988.h diff --git a/make/source.mk b/make/source.mk index 4a28c95c0..0599bc73c 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 000000000..4d046ad0a --- /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 000000000..da0e99788 --- /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 75dc2cc81..a7f3546f5 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 ba0707000..a310962c9 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 de3c12ec1..4a2604f0a 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