src/main/drivers/barometer/barometer_2smpb_02b.c

   1 /*
   2  * This file is part of Cleanflight, Betaflight and INAV.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  *
  24  * Copyright: INAVFLIGHT OU
  25  */
  26
  27 #include <stdbool.h>
  28 #include <stdint.h>
  29 #include <string.h>
  30 #include <math.h>
  31
  32 #include "platform.h"
  33
  34 #include "build/build_config.h"
  35 #include "build/debug.h"
  36 #include "common/utils.h"
  37
  38 #include "drivers/io.h"
  39 #include "drivers/bus.h"
  40 #include "drivers/bus_spi.h"
  41 #include "drivers/time.h"
  42 #include "drivers/barometer/barometer.h"
  43 #include "drivers/resource.h"
  44
  45 #include "barometer_2smpb_02b.h"
  46
  47 // 10 MHz max SPI frequency
  48 #define BARO_2SMBP_MAX_SPI_CLK_HZ 10000000
  49
  50 #if defined(USE_BARO) && defined(USE_BARO_2SMBP_02B)
  51
  52 #define BARO_2SMBP_I2C_ADDRESS 0x70
  53
  54 #define BARO_2SMBP_CHIP_ID 0x5C
  55
  56 #define REG_CHIP_ID 0xD1
  57 #define REG_RESET 0xE0
  58
  59 #define REG_COE_PR11 0xA0
  60 #define REG_COE_PR21 0xA3
  61 #define REG_COE_PR31 0xA5
  62 #define REG_COE_TEMP11 0xA7
  63 #define REG_COE_TEMP21 0xA9
  64 #define REG_COE_TEMP31 0xAB
  65 #define REG_COE_PTAT11 0xAD
  66 #define REG_COE_PTAT21 0xB1
  67 #define REG_COE_PTAT31 0xB3
  68
  69 #define REG_IIR_CNT 0xF1
  70 #define REG_DEVICE_STAT 0xF3
  71 #define REG_CTRL_MEAS 0xF4
  72 #define REG_IO_SETUP 0xF5
  73 #define REG_PRESS_TXD2 0xF7
  74
  75 // Value for CTRL_MEAS with 4x temperature averaging, 32x perssure, forced mode
  76 #define REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED ((0x03 << 5) | (0x05 << 2) | 0x01)
  77
  78 // IIR coefficient setting 8x
  79 #define REG_IIR_CNT_VAL_8X 0x03
  80
  81 typedef struct {
  82     float aa;
  83     float ba;
  84     int32_t ca;
  85     float ap;
  86     float bp;
  87     int32_t cp;
  88     float at;
  89     float bt;
  90     float ct;
  91 } calibrationCoefficients_t;
  92
  93 typedef struct {
  94     calibrationCoefficients_t   calib;
  95     float                       pressure;       // Pa
  96     float                       temperature;    // DegC
  97 } baroState_t;
  98
  99 static baroState_t  baroState;
 100 static uint8_t baroDataBuf[6];
 101
 102
 103 static int32_t readSignedRegister(const extDevice_t *dev, uint8_t reg, uint8_t nBytes)
 104 {
 105     uint8_t buf[3];
 106     uint32_t rawValue = 0;
 107
 108     busReadRegisterBuffer(dev, reg, &buf[0], nBytes);
 109
 110     for (int i=0; i<nBytes; i++) {
 111         rawValue += (uint32_t)buf[i] << (8 * (nBytes - i - 1));
 112     }
 113
 114     // 2's complement
 115     if (rawValue & ((int32_t)1 << (8 * nBytes - 1))) {
 116         // Negative
 117         return ((int32_t)rawValue) - ((int32_t)1 << (8 * nBytes));
 118     }
 119     else {
 120         return rawValue;
 121     }
 122 }
 123
 124 static int32_t getSigned24bitValue(uint8_t * pData)
 125 {
 126     uint32_t raw;
 127
 128     raw = (((uint32_t)pData[0] << 16) | ((uint32_t)pData[1] << 8) | (uint32_t)pData[2]) - ((uint32_t)1 << 23);
 129
 130     return raw;
 131 }
 132
 133 static bool deviceConfigure(const extDevice_t *dev)
 134 {
 135     /** Note: Chip reset causes I2C error due missing ACK. This causes interrupt based read (busReadRegisterBufferStart)
 136         to not work (read stops due to error flags). It works fine without chip reset. **/
 137
 138     //busWriteRegister(busDev, REG_RESET, 0xE6);
 139
 140     // No need to write IO_SETUP register: default values are fine
 141
 142     // Read calibration coefficients and scale them
 143     baroState.calib.aa = (4.2e-4f  * readSignedRegister(dev, REG_COE_PTAT31, 2)) / 32767;
 144     baroState.calib.ba = (8.0e0f  * readSignedRegister(dev, REG_COE_PTAT21, 2)) / 32767 - 1.6e2f;
 145     baroState.calib.ca = readSignedRegister(dev, REG_COE_PTAT11, 3);
 146     baroState.calib.ap = (3.0e-5f  * readSignedRegister(dev, REG_COE_PR31, 2)) / 32767;
 147     baroState.calib.bp = (10 * readSignedRegister(dev, REG_COE_PR21, 2)) / 32767 + 3.0e1f;
 148     baroState.calib.cp = readSignedRegister(dev, REG_COE_PR11, 3);
 149     baroState.calib.at = (8.0e-11f  * readSignedRegister(dev, REG_COE_TEMP31, 2)) / 32767;
 150     baroState.calib.bt = (1.6e-6f  * readSignedRegister(dev, REG_COE_TEMP21, 2)) / 32767 - 6.6e-6f;
 151     baroState.calib.ct = (8.5e-3f  * readSignedRegister(dev, REG_COE_TEMP11, 2)) / 32767 + 4.0e-2f;
 152
 153     // Configure IIR filter
 154     busWriteRegister(dev, REG_IIR_CNT, REG_IIR_CNT_VAL_8X);
 155
 156     return true;
 157 }
 158
 159 #define DETECTION_MAX_RETRY_COUNT   5
 160 static bool deviceDetect(const extDevice_t *dev)
 161 {
 162     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
 163         uint8_t chipId;
 164
 165         chipId = busReadRegister(dev, REG_CHIP_ID);
 166
 167         if (chipId == BARO_2SMBP_CHIP_ID) {
 168             return true;
 169         }
 170
 171         delay(50);
 172     };
 173
 174     return false;
 175 }
 176
 177 static void deviceInit(const extDevice_t *dev, resourceOwner_e owner)
 178 {
 179 #ifdef USE_BARO_SPI_2SMBP_02B
 180     if (dev->bus->busType == BUS_TYPE_SPI) {
 181         IOHi(dev->busType_u.spi.csnPin); // Disable
 182         IOInit(dev->busType_u.spi.csnPin, owner, 0);
 183         IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
 184         spiSetClkDivisor(dev, spiCalculateDivider(BARO_2SMBP_MAX_SPI_CLK_HZ));
 185     }
 186 #else
 187     UNUSED(dev);
 188     UNUSED(owner);
 189 #endif
 190 }
 191
 192 static void busDeviceDeInit(const extDevice_t *dev)
 193 {
 194 #ifdef USE_BARO_SPI_2SMBP_02B
 195     if (dev->bus->busType == BUS_TYPE_SPI) {
 196         spiPreinitByIO(dev->busType_u.spi.csnPin);
 197     }
 198 #else
 199     UNUSED(dev);
 200 #endif
 201 }
 202
 203 static void b2smpbStartUP(baroDev_t *baro)
 204 {
 205     // start a forced measurement
 206     busWriteRegister(&baro->dev, REG_CTRL_MEAS, REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED);
 207 }
 208
 209 static bool b2smpbReadUP(baroDev_t *baro)
 210 {
 211     if (busBusy(&baro->dev, NULL)) {
 212         return false;
 213     }
 214
 215     // Start reading temperature and pressure data
 216     busReadRegisterBufferStart(&baro->dev, REG_PRESS_TXD2, &baroDataBuf[0], 6);
 217
 218     return true;
 219 }
 220
 221 static bool b2smpbGetUP(baroDev_t *baro)
 222 {
 223     int32_t dtp;
 224     float tr, pl, tmp;
 225
 226     if (busBusy(&baro->dev, NULL)) {
 227         return false;
 228     }
 229
 230     // Calculate compensated temperature
 231     dtp = getSigned24bitValue(&baroDataBuf[3]);
 232
 233     tmp = baroState.calib.ba * baroState.calib.ba;
 234     tr = (-1 * baroState.calib.ba - sqrtf(tmp - 4 * baroState.calib.aa * (baroState.calib.ca - dtp))) / (2 * baroState.calib.aa);
 235     baroState.temperature = tr / 256;
 236
 237     // Calculate raw pressure
 238     dtp = getSigned24bitValue(&baroDataBuf[0]);
 239
 240     tmp = baroState.calib.bp * baroState.calib.bp;
 241     pl = (sqrtf(tmp - 4 * baroState.calib.ap * (baroState.calib.cp - dtp)) - baroState.calib.bp) / (2 * baroState.calib.ap);
 242
 243     // Calculate temperature compensated pressure
 244     tmp = tr * tr;
 245     baroState.pressure = pl / (baroState.calib.at * tmp + baroState.calib.bt * tr + baroState.calib.ct + 1);
 246
 247     return true;
 248 }
 249
 250 static void b2smpbStartUT(baroDev_t *baro)
 251 {
 252     UNUSED(baro);
 253 }
 254
 255 static bool b2smpbReadUT(baroDev_t *baro)
 256 {
 257     UNUSED(baro);
 258
 259     return true;
 260 }
 261
 262 static bool b2smpbGetUT(baroDev_t *baro)
 263 {
 264     UNUSED(baro);
 265
 266     return true;
 267 }
 268
 269 static void deviceCalculate(int32_t *pressure, int32_t *temperature)
 270 {
 271     if (pressure) {
 272         *pressure = baroState.pressure;
 273     }
 274
 275     if (temperature) {
 276         *temperature = (baroState.temperature * 100);   // to centidegrees
 277     }
 278 }
 279
 280 bool baro2SMPB02BDetect(baroDev_t *baro)
 281 {
 282     extDevice_t *dev = &baro->dev;
 283     bool defaultAddressApplied = false;
 284
 285     deviceInit(&baro->dev, OWNER_BARO_CS);
 286
 287     if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
 288         dev->busType_u.i2c.address = BARO_2SMBP_I2C_ADDRESS;
 289         defaultAddressApplied = true;
 290     }
 291
 292     if (!deviceDetect(dev)) {
 293         busDeviceDeInit(dev);
 294         if (defaultAddressApplied) {
 295             dev->busType_u.i2c.address = 0;
 296         }
 297         return false;
 298     }
 299
 300     if (!deviceConfigure(dev)) {
 301         busDeviceDeInit(dev);
 302         return false;
 303     }
 304
 305     baro->up_delay = 35000; // measurement takes 33.7 ms with 4x / 32x averaging
 306     baro->start_up = b2smpbStartUP;
 307     baro->read_up = b2smpbReadUP;
 308     baro->get_up = b2smpbGetUP;
 309
 310     // these are dummies, temperature is read with pressure
 311     baro->ut_delay = 0;
 312     baro->start_ut = b2smpbStartUT;
 313     baro->read_ut = b2smpbReadUT;
 314     baro->get_ut = b2smpbGetUT;
 315
 316     baro->calculate = deviceCalculate;
 317
 318     return true;
 319 }
 320
 321 #endif