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 #define QMP6988_I2C_ADDR                                        (0x70)
  41 #define QMP6988_DEFAULT_CHIP_ID                 (0x5c)
  42 #define QMP6988_CHIP_ID_REG                             (0xD1)  /* Chip ID Register */
  43
  44 #define QMP6988_IO_SETUP_REG                            (0xF5)
  45 #define QMP6988_SET_IIR_REG                             (0xF1)
  46 #define QMP6988_CTRL_MEAS_REG                           (0xF4)
  47 #define QMP6988_COE_B00_1_REG                           (0xA0)
  48 #define QMP6988_PRESSURE_MSB_REG                        (0xF7)  /* Pressure MSB Register */
  49 #define QMP6988_PRESSURE_LSB_REG                        (0xF8)  /* Pressure LSB Register */
  50 #define QMP6988_PRESSURE_XLSB_REG               (0xF9)  /* Pressure XLSB Register */
  51 #define QMP6988_TEMPERATURE_MSB_REG             (0xFA)  /* Temperature MSB Reg */
  52 #define QMP6988_TEMPERATURE_LSB_REG             (0xFB)  /* Temperature LSB Reg */
  53 #define QMP6988_TEMPERATURE_XLSB_REG            (0xFC)  /* Temperature XLSB Reg */
  54 #define QMP6988_DATA_FRAME_SIZE                 6
  55 #define QMP6988_FORCED_MODE                             (0x01)
  56 #define QMP6988_PWR_SAMPLE_MODE                 (0x7B)
  57
  58 #define QMP6988_OVERSAMP_SKIPPED                        (0x00)
  59 #define QMP6988_OVERSAMP_1X                             (0x01)
  60 #define QMP6988_OVERSAMP_2X                             (0x02)
  61 #define QMP6988_OVERSAMP_4X                             (0x03)
  62 #define QMP6988_OVERSAMP_8X                             (0x04)
  63 #define QMP6988_OVERSAMP_16X                            (0x05)
  64
  65 // configure pressure and temperature oversampling, forced sampling mode
  66 #define QMP6988_PRESSURE_OSR                                    (QMP6988_OVERSAMP_8X)
  67 #define QMP6988_TEMPERATURE_OSR                         (QMP6988_OVERSAMP_1X)
  68 #define QMP6988_MODE                                                    (QMP6988_PRESSURE_OSR << 2 | QMP6988_TEMPERATURE_OSR << 5 | QMP6988_FORCED_MODE)
  69
  70 #define T_INIT_MAX                                                              (20)
  71 #define T_MEASURE_PER_OSRS_MAX                                  (37)
  72 #define T_SETUP_PRESSURE_MAX                                    (10)
  73
  74 typedef struct qmp6988_calib_param_s {
  75     float Coe_a0;
  76     float Coe_a1;
  77     float Coe_a2;
  78     float Coe_b00;
  79     float Coe_bt1;
  80     float Coe_bt2;
  81     float Coe_bp1;
  82     float Coe_b11;
  83     float Coe_bp2;
  84     float Coe_b12;
  85     float Coe_b21;
  86     float Coe_bp3;
  87 } qmp6988_calib_param_t;
  88
  89 static uint8_t qmp6988_chip_id = 0;
  90 STATIC_UNIT_TESTED qmp6988_calib_param_t qmp6988_cal;
  91 // uncompensated pressure and temperature
  92 int32_t qmp6988_up = 0;
  93 int32_t qmp6988_ut = 0;
  94
  95 static void qmp6988_start_ut(baroDev_t *baro);
  96 static void qmp6988_get_ut(baroDev_t *baro);
  97 static void qmp6988_start_up(baroDev_t *baro);
  98 static void qmp6988_get_up(baroDev_t *baro);
  99
 100 STATIC_UNIT_TESTED void qmp6988_calculate(int32_t *pressure, int32_t *temperature);
 101
 102 void qmp6988BusInit(busDevice_t *busdev)
 103 {
 104 #ifdef USE_BARO_SPI_QMP6988
 105     if (busdev->bustype == BUSTYPE_SPI) {
 106         IOHi(busdev->busdev_u.spi.csnPin);
 107         IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
 108         IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
 109 #ifdef USE_SPI_TRANSACTION
 110         spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, SPI_CLOCK_STANDARD);
 111 #else
 112         spiBusSetDivisor(busdev, SPI_CLOCK_STANDARD);
 113 #endif
 114     }
 115 #else
 116     UNUSED(busdev);
 117 #endif
 118 }
 119
 120 void qmp6988BusDeinit(busDevice_t *busdev)
 121 {
 122 #ifdef USE_BARO_SPI_QMP6988
 123     if (busdev->bustype == BUSTYPE_SPI) {
 124         IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU);
 125         IORelease(busdev->busdev_u.spi.csnPin);
 126         IOInit(busdev->busdev_u.spi.csnPin, OWNER_PREINIT, 0);
 127     }
 128 #else
 129     UNUSED(busdev);
 130 #endif
 131 }
 132
 133 bool qmp6988Detect(baroDev_t *baro)
 134 {
 135     uint8_t databuf[25] = {0};
 136     int Coe_a0_;
 137     int Coe_a1_;
 138     int Coe_a2_;
 139     int Coe_b00_;
 140     int Coe_bt1_;
 141     int Coe_bt2_;
 142     int Coe_bp1_;
 143     int Coe_b11_;
 144     int Coe_bp2_;
 145     int Coe_b12_;
 146     int Coe_b21_;
 147     int Coe_bp3_;
 148     uint16_t lb=0,hb=0;
 149     uint32_t lw=0,hw=0,temp1,temp2;
 150
 151     delay(20);
 152
 153     busDevice_t *busdev = &baro->busdev;
 154     bool defaultAddressApplied = false;
 155
 156     qmp6988BusInit(busdev);
 157
 158     if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
 159         busdev->busdev_u.i2c.address = QMP6988_I2C_ADDR;
 160         defaultAddressApplied = true;
 161     }
 162
 163     busReadRegisterBuffer(busdev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1);  /* read Chip Id */
 164
 165     if (qmp6988_chip_id != QMP6988_DEFAULT_CHIP_ID) {
 166         qmp6988BusDeinit(busdev);
 167         if (defaultAddressApplied) {
 168             busdev->busdev_u.i2c.address = 0;
 169         }
 170         return false;
 171     }
 172
 173     // SetIIR
 174     busWriteRegister(busdev, QMP6988_SET_IIR_REG, 0x05);
 175
 176     //read OTP
 177     busReadRegisterBuffer(busdev, QMP6988_COE_B00_1_REG, databuf, 25);
 178
 179     //algo OTP
 180     hw = databuf[0];
 181     lw =  databuf[1];
 182     temp1 = (hw<<12) | (lw<<4);
 183
 184     hb = databuf[2];
 185     lb = databuf[3];
 186     Coe_bt1_ = (short)((hb<<8) | lb);
 187
 188     hb = databuf[4];
 189     lb = databuf[5];
 190     Coe_bt2_ = (short)((hb<<8) | lb);
 191
 192     hb = databuf[6];
 193     lb = databuf[7];
 194     Coe_bp1_ = (short)((hb<<8) | lb);
 195
 196     hb = databuf[8];
 197     lb = databuf[9];
 198     Coe_b11_ = (short)((hb<<8) | lb);
 199
 200     hb = databuf[10];
 201     lb = databuf[11];
 202     Coe_bp2_ = (short)((hb<<8) | lb);
 203
 204     hb = databuf[12];
 205     lb = databuf[13];
 206     Coe_b12_ = (short)((hb<<8) | lb);
 207
 208     hb = databuf[14];
 209     lb = databuf[15];
 210     Coe_b21_ = (short)((hb<<8) | lb);
 211
 212     hb = databuf[16];
 213     lb = databuf[17];
 214     Coe_bp3_ = (short)((hb<<8) | lb);
 215
 216     hw = databuf[18];
 217     lw = databuf[19];
 218     temp2 = (hw<<12) | (lw<<4);
 219
 220     hb = databuf[20];
 221     lb = databuf[21];
 222     Coe_a1_ = (short)((hb<<8) | lb);
 223
 224     hb = databuf[22];
 225     lb = databuf[23];
 226     Coe_a2_ = (short)((hb<<8) | lb);
 227
 228     hb = databuf[24];
 229
 230     temp1 = temp1|((hb&0xf0)>>4);
 231     if(temp1&0x80000)
 232        Coe_b00_ = ((int)temp1 - (int)0x100000);
 233     else
 234        Coe_b00_ = temp1;
 235
 236     temp2 = temp2|(hb&0x0f);
 237     if(temp2&0x80000)
 238         Coe_a0_  = ((int)temp2 - (int)0x100000);
 239     else
 240         Coe_a0_ = temp2;
 241
 242     qmp6988_cal.Coe_a0=(float)Coe_a0_/16.0;
 243     qmp6988_cal.Coe_a1=(-6.30E-03)+(4.30E-04)*(float)Coe_a1_/32767.0;
 244     qmp6988_cal.Coe_a2=(-1.9E-11)+(1.2E-10)*(float)Coe_a2_/32767.0;
 245
 246     qmp6988_cal.Coe_b00 = Coe_b00_/16.0;
 247     qmp6988_cal.Coe_bt1 = (1.00E-01)+(9.10E-02)*(float)Coe_bt1_/32767.0;
 248     qmp6988_cal.Coe_bt2= (1.20E-08)+(1.20E-06)*(float)Coe_bt2_/32767.0;
 249
 250     qmp6988_cal.Coe_bp1 = (3.30E-02)+(1.90E-02)*(float)Coe_bp1_/32767.0;
 251     qmp6988_cal.Coe_b11= (2.10E-07)+(1.40E-07)*(float)Coe_b11_/32767.0;
 252
 253     qmp6988_cal.Coe_bp2 = (-6.30E-10)+(3.50E-10)*(float)Coe_bp2_/32767.0;
 254     qmp6988_cal.Coe_b12= (2.90E-13)+(7.60E-13)*(float)Coe_b12_/32767.0;
 255
 256     qmp6988_cal.Coe_b21 = (2.10E-15)+(1.20E-14)*(float)Coe_b21_/32767.0;
 257     qmp6988_cal.Coe_bp3= (1.30E-16)+(7.90E-17)*(float)Coe_bp3_/32767.0;
 258
 259     // Set power mode and sample times
 260     busWriteRegister(busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
 261
 262     // these are dummy as temperature is measured as part of pressure
 263     baro->ut_delay = 0;
 264     baro->get_ut = qmp6988_get_ut;
 265     baro->start_ut = qmp6988_start_ut;
 266     // only _up part is executed, and gets both temperature and pressure
 267     baro->start_up = qmp6988_start_up;
 268     baro->get_up = qmp6988_get_up;
 269     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;
 270     baro->calculate = qmp6988_calculate;
 271
 272     return true;
 273 }
 274
 275 static void qmp6988_start_ut(baroDev_t *baro)
 276 {
 277     UNUSED(baro);
 278     // dummy
 279 }
 280
 281 static void qmp6988_get_ut(baroDev_t *baro)
 282 {
 283     UNUSED(baro);
 284     // dummy
 285 }
 286
 287 static void qmp6988_start_up(baroDev_t *baro)
 288 {
 289     // start measurement
 290     busWriteRegister(&baro->busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
 291 }
 292
 293 static void qmp6988_get_up(baroDev_t *baro)
 294 {
 295     uint8_t data[QMP6988_DATA_FRAME_SIZE];
 296
 297     // read data from sensor
 298     busReadRegisterBuffer(&baro->busdev, QMP6988_PRESSURE_MSB_REG, data, QMP6988_DATA_FRAME_SIZE);
 299     qmp6988_up = (int32_t)((((uint32_t)(data[0])) << 16) | (((uint32_t)(data[1])) << 8) | ((uint32_t)data[2] ));
 300     qmp6988_ut = (int32_t)((((uint32_t)(data[3])) << 16) | (((uint32_t)(data[4])) << 8) | ((uint32_t)data[5]));
 301 }
 302
 303 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
 304 // t_fine carries fine temperature as global value
 305 static float qmp6988_compensate_T(int32_t adc_T)
 306 {
 307     int32_t var1;
 308     float T;
 309
 310     var1=adc_T-1024*1024*8;
 311     T= qmp6988_cal.Coe_a0+qmp6988_cal.Coe_a1*var1+qmp6988_cal.Coe_a2*var1*var1;
 312
 313     return T;
 314 }
 315
 316
 317
 318 STATIC_UNIT_TESTED void qmp6988_calculate(int32_t *pressure, int32_t *temperature)
 319 {
 320     float tr,pr;
 321     int32_t Dp;
 322
 323     tr = qmp6988_compensate_T(qmp6988_ut);
 324     Dp = qmp6988_up - 1024*1024*8;
 325
 326     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
 327     +qmp6988_cal.Coe_b21*Dp*Dp*tr+qmp6988_cal.Coe_bp3*Dp*Dp*Dp;
 328
 329     if (pr)
 330     *pressure = (int32_t)(pr);
 331     if (tr)
 332     *temperature = (int32_t)tr/256;
 333 }
 334
 335 #endif