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 static int32_t readSignedRegister(const extDevice_t *dev, uint8_t reg, uint8_t nBytes)
 103 {
 104     uint8_t buf[3];
 105     uint32_t rawValue = 0;
 106
 107     busReadRegisterBuffer(dev, reg, &buf[0], nBytes);
 108
 109     for (int i=0; i<nBytes; i++) {
 110         rawValue += (uint32_t)buf[i] << (8 * (nBytes - i - 1));
 111     }
 112
 113     // 2's complement
 114     if (rawValue & ((int32_t)1 << (8 * nBytes - 1))) {
 115         // Negative
 116         return ((int32_t)rawValue) - ((int32_t)1 << (8 * nBytes));
 117     }
 118     else {
 119         return rawValue;
 120     }
 121 }
 122
 123 static int32_t getSigned24bitValue(uint8_t * pData)
 124 {
 125     uint32_t raw;
 126
 127     raw = (((uint32_t)pData[0] << 16) | ((uint32_t)pData[1] << 8) | (uint32_t)pData[2]) - ((uint32_t)1 << 23);
 128
 129     return raw;
 130 }
 131
 132 static bool deviceConfigure(const extDevice_t *dev)
 133 {
 134     /** Note: Chip reset causes I2C error due missing ACK. This causes interrupt based read (busReadRegisterBufferStart)
 135         to not work (read stops due to error flags). It works fine without chip reset. **/
 136
 137     //busWriteRegister(busDev, REG_RESET, 0xE6);
 138
 139     // No need to write IO_SETUP register: default values are fine
 140
 141     // Read calibration coefficients and scale them
 142     baroState.calib.aa = (4.2e-4f  * readSignedRegister(dev, REG_COE_PTAT31, 2)) / 32767;
 143     baroState.calib.ba = (8.0e0f  * readSignedRegister(dev, REG_COE_PTAT21, 2)) / 32767 - 1.6e2f;
 144     baroState.calib.ca = readSignedRegister(dev, REG_COE_PTAT11, 3);
 145     baroState.calib.ap = (3.0e-5f  * readSignedRegister(dev, REG_COE_PR31, 2)) / 32767;
 146     baroState.calib.bp = (10 * readSignedRegister(dev, REG_COE_PR21, 2)) / 32767 + 3.0e1f;
 147     baroState.calib.cp = readSignedRegister(dev, REG_COE_PR11, 3);
 148     baroState.calib.at = (8.0e-11f  * readSignedRegister(dev, REG_COE_TEMP31, 2)) / 32767;
 149     baroState.calib.bt = (1.6e-6f  * readSignedRegister(dev, REG_COE_TEMP21, 2)) / 32767 - 6.6e-6f;
 150     baroState.calib.ct = (8.5e-3f  * readSignedRegister(dev, REG_COE_TEMP11, 2)) / 32767 + 4.0e-2f;
 151
 152     // Configure IIR filter
 153     busWriteRegister(dev, REG_IIR_CNT, REG_IIR_CNT_VAL_8X);
 154
 155     return true;
 156 }
 157
 158 #define DETECTION_MAX_RETRY_COUNT   5
 159 static bool deviceDetect(const extDevice_t *dev)
 160 {
 161     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
 162         uint8_t chipId;
 163
 164         chipId = busReadRegister(dev, REG_CHIP_ID);
 165
 166         if (chipId == BARO_2SMBP_CHIP_ID) {
 167             return true;
 168         }
 169
 170         delay(50);
 171     };
 172
 173     return false;
 174 }
 175
 176 static void deviceInit(const extDevice_t *dev, resourceOwner_e owner)
 177 {
 178 #ifdef USE_BARO_SPI_2SMBP_02B
 179     if (dev->bus->busType == BUS_TYPE_SPI) {
 180         IOHi(dev->busType_u.spi.csnPin); // Disable
 181         IOInit(dev->busType_u.spi.csnPin, owner, 0);
 182         IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
 183         spiSetClkDivisor(dev, spiCalculateDivider(BARO_2SMBP_MAX_SPI_CLK_HZ));
 184     }
 185 #else
 186     UNUSED(dev);
 187     UNUSED(owner);
 188 #endif
 189 }
 190
 191 static void busDeviceDeInit(const extDevice_t *dev)
 192 {
 193 #ifdef USE_BARO_SPI_2SMBP_02B
 194     if (dev->bus->busType == BUS_TYPE_SPI) {
 195         spiPreinitByIO(dev->busType_u.spi.csnPin);
 196     }
 197 #else
 198     UNUSED(dev);
 199 #endif
 200 }
 201
 202 static bool b2smpbStartUP(baroDev_t *baro)
 203 {
 204     // start a forced measurement
 205     return busWriteRegister(&baro->dev, REG_CTRL_MEAS, REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED);
 206 }
 207
 208 static bool b2smpbReadUP(baroDev_t *baro)
 209 {
 210     if (busBusy(&baro->dev, NULL)) {
 211         return false;
 212     }
 213
 214     // Start reading temperature and pressure data
 215     return busReadRegisterBufferStart(&baro->dev, REG_PRESS_TXD2, &baroDataBuf[0], 6);
 216 }
 217
 218 static bool b2smpbGetUP(baroDev_t *baro)
 219 {
 220     int32_t dtp;
 221     float tr, pl, tmp;
 222
 223     if (busBusy(&baro->dev, NULL)) {
 224         return false;
 225     }
 226
 227     // Calculate compensated temperature
 228     dtp = getSigned24bitValue(&baroDataBuf[3]);
 229
 230     tmp = baroState.calib.ba * baroState.calib.ba;
 231     tr = (-1 * baroState.calib.ba - sqrtf(tmp - 4 * baroState.calib.aa * (baroState.calib.ca - dtp))) / (2 * baroState.calib.aa);
 232     baroState.temperature = tr / 256;
 233
 234     // Calculate raw pressure
 235     dtp = getSigned24bitValue(&baroDataBuf[0]);
 236
 237     tmp = baroState.calib.bp * baroState.calib.bp;
 238     pl = (sqrtf(tmp - 4 * baroState.calib.ap * (baroState.calib.cp - dtp)) - baroState.calib.bp) / (2 * baroState.calib.ap);
 239
 240     // Calculate temperature compensated pressure
 241     tmp = tr * tr;
 242     baroState.pressure = pl / (baroState.calib.at * tmp + baroState.calib.bt * tr + baroState.calib.ct + 1);
 243
 244     return true;
 245 }
 246
 247 static bool b2smpbStartUT(baroDev_t *baro)
 248 {
 249     UNUSED(baro);
 250
 251     return true;
 252 }
 253
 254 static bool b2smpbReadUT(baroDev_t *baro)
 255 {
 256     UNUSED(baro);
 257
 258     return true;
 259 }
 260
 261 static bool b2smpbGetUT(baroDev_t *baro)
 262 {
 263     UNUSED(baro);
 264
 265     return true;
 266 }
 267
 268 static void deviceCalculate(int32_t *pressure, int32_t *temperature)
 269 {
 270     if (pressure) {
 271         *pressure = baroState.pressure;
 272     }
 273
 274     if (temperature) {
 275         *temperature = (baroState.temperature * 100);   // to centidegrees
 276     }
 277 }
 278
 279 bool baro2SMPB02BDetect(baroDev_t *baro)
 280 {
 281     extDevice_t *dev = &baro->dev;
 282     bool defaultAddressApplied = false;
 283
 284     deviceInit(&baro->dev, OWNER_BARO_CS);
 285
 286     if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
 287         dev->busType_u.i2c.address = BARO_2SMBP_I2C_ADDRESS;
 288         defaultAddressApplied = true;
 289     }
 290
 291     if (!deviceDetect(dev)) {
 292         busDeviceDeInit(dev);
 293         if (defaultAddressApplied) {
 294             dev->busType_u.i2c.address = 0;
 295         }
 296         return false;
 297     }
 298
 299     if (!deviceConfigure(dev)) {
 300         busDeviceDeInit(dev);
 301         return false;
 302     }
 303
 304     baro->up_delay = 35000; // measurement takes 33.7 ms with 4x / 32x averaging
 305     baro->start_up = b2smpbStartUP;
 306     baro->read_up = b2smpbReadUP;
 307     baro->get_up = b2smpbGetUP;
 308
 309     // these are dummies, temperature is read with pressure
 310     baro->ut_delay = 0;
 311     baro->start_ut = b2smpbStartUT;
 312     baro->read_ut = b2smpbReadUT;
 313     baro->get_ut = b2smpbGetUT;
 314
 315     baro->calculate = deviceCalculate;
 316
 317     return true;
 318 }
 319
 320 #endif