src/main/drivers/barometer/barometer_qmp6988.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include "platform.h"
  22
  23 #include "build/build_config.h"
  24 #include "build/debug.h"
  25 #include "barometer.h"
  26 #include "drivers/bus.h"
  27 #include "drivers/bus_i2c.h"
  28 #include "drivers/bus_i2c_busdev.h"
  29 #include "drivers/bus_spi.h"
  30 #include "drivers/io.h"
  31 #include "drivers/time.h"
  32
  33 #include "drivers/serial.h"
  34 #include "io/serial.h"
  35 #include "common/printf.h"
  36 #include "barometer_qmp6988.h"
  37
  38 #if defined(USE_BARO) && (defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988))
  39
  40 // 10 MHz max SPI frequency
  41 #define QMP6988_MAX_SPI_CLK_HZ 10000000
  42
  43 #define QMP6988_I2C_ADDR                0x70
  44 #define QMP6988_DEFAULT_CHIP_ID         0x5c
  45 #define QMP6988_CHIP_ID_REG             0xD1  /* Chip ID Register */
  46
  47 #define QMP6988_IO_SETUP_REG            0xF5
  48 #define QMP6988_SET_IIR_REG             0xF1
  49 #define QMP6988_CTRL_MEAS_REG           0xF4
  50 #define QMP6988_COE_B00_1_REG           0xA0
  51 #define QMP6988_PRESSURE_MSB_REG        0xF7  /* Pressure MSB Register */
  52 #define QMP6988_PRESSURE_LSB_REG        0xF8  /* Pressure LSB Register */
  53 #define QMP6988_PRESSURE_XLSB_REG       0xF9  /* Pressure XLSB Register */
  54 #define QMP6988_TEMPERATURE_MSB_REG     0xFA  /* Temperature MSB Reg */
  55 #define QMP6988_TEMPERATURE_LSB_REG     0xFB  /* Temperature LSB Reg */
  56 #define QMP6988_TEMPERATURE_XLSB_REG    0xFC  /* Temperature XLSB Reg */
  57 #define QMP6988_DATA_FRAME_SIZE         6
  58 #define QMP6988_FORCED_MODE             0x01
  59 #define QMP6988_PWR_SAMPLE_MODE         0x7B
  60
  61 #define QMP6988_OVERSAMP_SKIPPED        0x00
  62 #define QMP6988_OVERSAMP_1X             0x01
  63 #define QMP6988_OVERSAMP_2X             0x02
  64 #define QMP6988_OVERSAMP_4X             0x03
  65 #define QMP6988_OVERSAMP_8X             0x04
  66 #define QMP6988_OVERSAMP_16X            0x05
  67
  68 // configure pressure and temperature oversampling, forced sampling mode
  69 #define QMP6988_PRESSURE_OSR            QMP6988_OVERSAMP_8X
  70 #define QMP6988_TEMPERATURE_OSR         QMP6988_OVERSAMP_1X
  71 #define QMP6988_MODE                    (QMP6988_PRESSURE_OSR << 2 | QMP6988_TEMPERATURE_OSR << 5 | QMP6988_FORCED_MODE)
  72
  73 #define T_INIT_MAX                      20
  74 #define T_MEASURE_PER_OSRS_MAX          37
  75 #define T_SETUP_PRESSURE_MAX            10
  76
  77 typedef struct qmp6988_calib_param_s {
  78     float Coe_a0;
  79     float Coe_a1;
  80     float Coe_a2;
  81     float Coe_b00;
  82     float Coe_bt1;
  83     float Coe_bt2;
  84     float Coe_bp1;
  85     float Coe_b11;
  86     float Coe_bp2;
  87     float Coe_b12;
  88     float Coe_b21;
  89     float Coe_bp3;
  90 } qmp6988_calib_param_t;
  91
  92 static uint8_t qmp6988_chip_id = 0;
  93 STATIC_UNIT_TESTED qmp6988_calib_param_t qmp6988_cal;
  94 // uncompensated pressure and temperature
  95 int32_t qmp6988_up = 0;
  96 int32_t qmp6988_ut = 0;
  97 static DMA_DATA_ZERO_INIT uint8_t sensor_data[QMP6988_DATA_FRAME_SIZE];
  98
  99 static bool qmp6988StartUT(baroDev_t *baro);
 100 static bool qmp6988ReadUT(baroDev_t *baro);
 101 static bool qmp6988GetUT(baroDev_t *baro);
 102 static bool qmp6988StartUP(baroDev_t *baro);
 103 static bool qmp6988ReadUP(baroDev_t *baro);
 104 static bool qmp6988GetUP(baroDev_t *baro);
 105
 106 STATIC_UNIT_TESTED void qmp6988Calculate(int32_t *pressure, int32_t *temperature);
 107
 108 void qmp6988BusInit(const extDevice_t *dev)
 109 {
 110 #ifdef USE_BARO_SPI_QMP6988
 111     if (dev->bus->busType == BUS_TYPE_SPI) {
 112         IOHi(dev->busType_u.spi.csnPin);
 113         IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
 114         IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
 115         spiSetClkDivisor(dev, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ));
 116     }
 117 #else
 118     UNUSED(dev);
 119 #endif
 120 }
 121
 122 void qmp6988BusDeinit(const extDevice_t *dev)
 123 {
 124 #ifdef USE_BARO_SPI_QMP6988
 125     if (dev->bus->busType == BUS_TYPE_SPI) {
 126         IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_IPU);
 127         IORelease(dev->busType_u.spi.csnPin);
 128         IOInit(dev->busType_u.spi.csnPin, OWNER_PREINIT, 0);
 129     }
 130 #else
 131     UNUSED(dev);
 132 #endif
 133 }
 134
 135 bool qmp6988Detect(baroDev_t *baro)
 136 {
 137     uint8_t databuf[25] = {0};
 138     int Coe_a0_;
 139     int Coe_a1_;
 140     int Coe_a2_;
 141     int Coe_b00_;
 142     int Coe_bt1_;
 143     int Coe_bt2_;
 144     int Coe_bp1_;
 145     int Coe_b11_;
 146     int Coe_bp2_;
 147     int Coe_b12_;
 148     int Coe_b21_;
 149     int Coe_bp3_;
 150     uint16_t lb=0,hb=0;
 151     uint32_t lw=0,hw=0,temp1,temp2;
 152
 153     delay(20);
 154
 155     extDevice_t *dev = &baro->dev;
 156     bool defaultAddressApplied = false;
 157
 158     qmp6988BusInit(dev);
 159
 160     if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
 161         dev->busType_u.i2c.address = QMP6988_I2C_ADDR;
 162         defaultAddressApplied = true;
 163     }
 164
 165     busReadRegisterBuffer(dev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1);  /* read Chip Id */
 166
 167     if (qmp6988_chip_id != QMP6988_DEFAULT_CHIP_ID) {
 168         qmp6988BusDeinit(dev);
 169         if (defaultAddressApplied) {
 170             dev->busType_u.i2c.address = 0;
 171         }
 172         return false;
 173     }
 174
 175     busDeviceRegister(dev);
 176
 177     // SetIIR
 178     busWriteRegister(dev, QMP6988_SET_IIR_REG, 0x05);
 179
 180     //read OTP
 181     busReadRegisterBuffer(dev, QMP6988_COE_B00_1_REG, databuf, 25);
 182
 183     //algo OTP
 184     hw = databuf[0];
 185     lw =  databuf[1];
 186     temp1 = hw<<12 | lw<<4;
 187
 188     hb = databuf[2];
 189     lb = databuf[3];
 190     Coe_bt1_ = hb<<8 | lb;
 191
 192     hb = databuf[4];
 193     lb = databuf[5];
 194     Coe_bt2_ = hb<<8 | lb;
 195
 196     hb = databuf[6];
 197     lb = databuf[7];
 198     Coe_bp1_ = hb<<8 | lb;
 199
 200     hb = databuf[8];
 201     lb = databuf[9];
 202     Coe_b11_ = hb<<8 | lb;
 203
 204     hb = databuf[10];
 205     lb = databuf[11];
 206     Coe_bp2_ = hb<<8 | lb;
 207
 208     hb = databuf[12];
 209     lb = databuf[13];
 210     Coe_b12_ = hb<<8 | lb;
 211
 212     hb = databuf[14];
 213     lb = databuf[15];
 214     Coe_b21_ = hb<<8 | lb;
 215
 216     hb = databuf[16];
 217     lb = databuf[17];
 218     Coe_bp3_ = hb<<8 | lb;
 219
 220     hw = databuf[18];
 221     lw = databuf[19];
 222     temp2 = hw<<12 | lw<<4;
 223
 224     hb = databuf[20];
 225     lb = databuf[21];
 226     Coe_a1_ = hb<<8 | lb;
 227
 228     hb = databuf[22];
 229     lb = databuf[23];
 230     Coe_a2_ = hb<<8 | lb;
 231
 232     hb = databuf[24];
 233
 234     temp1 = temp1|((hb&0xf0)>>4);
 235     if(temp1&0x80000)
 236        Coe_b00_ = ((int)temp1 - (int)0x100000);
 237     else
 238        Coe_b00_ = temp1;
 239
 240     temp2 = temp2|(hb&0x0f);
 241     if(temp2&0x80000)
 242         Coe_a0_  = ((int)temp2 - (int)0x100000);
 243     else
 244         Coe_a0_ = temp2;
 245
 246     qmp6988_cal.Coe_a0=(float)Coe_a0_/16.0;
 247     qmp6988_cal.Coe_a1=(-6.30E-03)+(4.30E-04)*(float)Coe_a1_/32767.0;
 248     qmp6988_cal.Coe_a2=(-1.9E-11)+(1.2E-10)*(float)Coe_a2_/32767.0;
 249
 250     qmp6988_cal.Coe_b00 = Coe_b00_/16.0;
 251     qmp6988_cal.Coe_bt1 = (1.00E-01)+(9.10E-02)*(float)Coe_bt1_/32767.0;
 252     qmp6988_cal.Coe_bt2= (1.20E-08)+(1.20E-06)*(float)Coe_bt2_/32767.0;
 253
 254     qmp6988_cal.Coe_bp1 = (3.30E-02)+(1.90E-02)*(float)Coe_bp1_/32767.0;
 255     qmp6988_cal.Coe_b11= (2.10E-07)+(1.40E-07)*(float)Coe_b11_/32767.0;
 256
 257     qmp6988_cal.Coe_bp2 = (-6.30E-10)+(3.50E-10)*(float)Coe_bp2_/32767.0;
 258     qmp6988_cal.Coe_b12= (2.90E-13)+(7.60E-13)*(float)Coe_b12_/32767.0;
 259
 260     qmp6988_cal.Coe_b21 = (2.10E-15)+(1.20E-14)*(float)Coe_b21_/32767.0;
 261     qmp6988_cal.Coe_bp3= (1.30E-16)+(7.90E-17)*(float)Coe_bp3_/32767.0;
 262
 263     // Set power mode and sample times
 264     busWriteRegister(dev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
 265
 266     // these are dummy as temperature is measured as part of pressure
 267     baro->combined_read = true;
 268     baro->ut_delay = 0;
 269     baro->start_ut = qmp6988StartUT;
 270     baro->read_ut = qmp6988ReadUT;
 271     baro->get_ut = qmp6988GetUT;
 272     // only _up part is executed, and gets both temperature and pressure
 273     baro->start_up = qmp6988StartUP;
 274     baro->read_up = qmp6988ReadUP;
 275     baro->get_up = qmp6988GetUP;
 276     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;
 277     baro->calculate = qmp6988Calculate;
 278
 279     return true;
 280 }
 281
 282 static bool qmp6988StartUT(baroDev_t *baro)
 283 {
 284     UNUSED(baro);
 285     // dummy
 286
 287     return true;
 288 }
 289
 290 static bool qmp6988ReadUT(baroDev_t *baro)
 291 {
 292     UNUSED(baro);
 293     // dummy
 294     return true;
 295 }
 296
 297 static bool qmp6988GetUT(baroDev_t *baro)
 298 {
 299     UNUSED(baro);
 300     // dummy
 301     return true;
 302 }
 303
 304 static bool qmp6988StartUP(baroDev_t *baro)
 305 {
 306     // start measurement
 307     return busWriteRegister(&baro->dev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
 308 }
 309
 310 static bool qmp6988ReadUP(baroDev_t *baro)
 311 {
 312     if (busBusy(&baro->dev, NULL)) {
 313         return false;
 314     }
 315
 316     // read data from sensor
 317     return busReadRegisterBufferStart(&baro->dev, QMP6988_PRESSURE_MSB_REG, sensor_data, QMP6988_DATA_FRAME_SIZE);
 318 }
 319
 320 static bool qmp6988GetUP(baroDev_t *baro)
 321 {
 322     if (busBusy(&baro->dev, NULL)) {
 323         return false;
 324     }
 325
 326     qmp6988_up = sensor_data[0] << 16 | sensor_data[1] << 8 | sensor_data[2];
 327     qmp6988_ut = sensor_data[3] << 16 | sensor_data[4] << 8 | sensor_data[5];
 328
 329     return true;
 330 }
 331
 332 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
 333 // t_fine carries fine temperature as global value
 334 static float qmp6988CompensateTemperature(int32_t adc_T)
 335 {
 336     int32_t var1;
 337     float T;
 338
 339     var1=adc_T-1024*1024*8;
 340     T= qmp6988_cal.Coe_a0+qmp6988_cal.Coe_a1*var1+qmp6988_cal.Coe_a2*var1*var1;
 341
 342     return T;
 343 }
 344
 345 STATIC_UNIT_TESTED void qmp6988Calculate(int32_t *pressure, int32_t *temperature)
 346 {
 347     float tr,pr;
 348     int32_t Dp;
 349
 350     tr = qmp6988CompensateTemperature(qmp6988_ut);
 351     Dp = qmp6988_up - 1024*1024*8;
 352
 353     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
 354     +qmp6988_cal.Coe_b21*Dp*Dp*tr+qmp6988_cal.Coe_bp3*Dp*Dp*Dp;
 355
 356     if (pr)
 357     *pressure = (int32_t)(pr);
 358     if (tr)
 359     *temperature = (int32_t)tr/256;
 360 }
 361
 362 #endif