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
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <string.h>
  28
  29 #include <platform.h>
  30
  31 #include "build/build_config.h"
  32 #include "build/debug.h"
  33 #include "common/utils.h"
  34
  35 #include "drivers/io.h"
  36 #include "drivers/bus.h"
  37 #include "drivers/time.h"
  38 #include "drivers/barometer/barometer.h"
  39
  40 #include "drivers/barometer/barometer_2smpb_02b.h"
  41
  42 #if defined(USE_BARO) && defined(USE_BARO_B2SMPB)
  43
  44 #define BARO_2SMBP_I2C_ADDRESS 0x70
  45
  46 #define BARO_2SMBP_CHIP_ID 0x5C
  47
  48 #define REG_CHIP_ID 0xD1
  49 #define REG_RESET 0xE0
  50
  51 #define REG_COE_PR11 0xA0
  52 #define REG_COE_PR21 0xA3
  53 #define REG_COE_PR31 0xA5
  54 #define REG_COE_TEMP11 0xA7
  55 #define REG_COE_TEMP21 0xA9
  56 #define REG_COE_TEMP31 0xAB
  57 #define REG_COE_PTAT11 0xAD
  58 #define REG_COE_PTAT21 0xB1
  59 #define REG_COE_PTAT31 0xB3
  60
  61 #define REG_IIR_CNT 0xF1
  62 #define REG_DEVICE_STAT 0xF3
  63 #define REG_CTRL_MEAS 0xF4
  64 #define REG_IO_SETUP 0xF5
  65 #define REG_PRESS_TXD2 0xF7
  66
  67 // Value for CTRL_MEAS with 4x temperature averaging, 32x perssure, forced mode
  68 #define REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED ((0x03 << 5) | (0x05 << 2) | 0x01)
  69
  70 // IIR coefficient setting 8x
  71 #define REG_IIR_CNT_VAL_8X 0x03
  72
  73 typedef struct {
  74     float aa;
  75     float ba;
  76     int32_t ca;
  77     float ap;
  78     float bp;
  79     int32_t cp;
  80     float at;
  81     float bt;
  82     float ct;
  83 } calibrationCoefficients_t;
  84
  85 typedef struct {
  86     calibrationCoefficients_t   calib;
  87     float                       pressure;       // Pa
  88     float                       temperature;    // DegC
  89 } baroState_t;
  90
  91 static baroState_t  baroState;
  92 static uint8_t baroDataBuf[6];
  93
  94
  95 static int32_t readSignedRegister(busDevice_t * busDev, uint8_t reg, uint8_t nBytes)
  96 {
  97     uint8_t buf[3];
  98     uint32_t rawValue = 0;
  99
 100     busReadBuf(busDev, reg, &buf[0], nBytes);
 101
 102     for (int i=0; i<nBytes; i++) {
 103         rawValue += (uint32_t)buf[i] << (8 * (nBytes - i - 1));
 104     }
 105
 106     // 2's complement
 107     if (rawValue & ((int32_t)1 << (8 * nBytes - 1))) {
 108         // Negative
 109         return ((int32_t)rawValue) - ((int32_t)1 << (8 * nBytes));
 110     }
 111     else {
 112         return rawValue;
 113     }
 114 }
 115
 116 static int32_t getSigned24bitValue(uint8_t * pData)
 117 {
 118     uint32_t raw;
 119
 120     raw = (((uint32_t)pData[0] << 16) | ((uint32_t)pData[1] << 8) | (uint32_t)pData[2]) - ((uint32_t)1 << 23);
 121
 122     return raw;
 123 }
 124
 125 static bool deviceConfigure(busDevice_t * busDev)
 126 {
 127     /** Note: Chip reset causes I2C error due missing ACK. This causes interrupt based read (busReadRegisterBufferStart)
 128         to not work (read stops due to error flags). It works fine without chip reset. **/
 129
 130     //busWrite(busDev, REG_RESET, 0xE6);
 131
 132     // No need to write IO_SETUP register: default values are fine
 133
 134     // Read calibration coefficients and scale them
 135     baroState.calib.aa = (4.2e-4f  * readSignedRegister(busDev, REG_COE_PTAT31, 2)) / 32767;
 136     baroState.calib.ba = (8.0e0f  * readSignedRegister(busDev, REG_COE_PTAT21, 2)) / 32767 - 1.6e2f;
 137     baroState.calib.ca = readSignedRegister(busDev, REG_COE_PTAT11, 3);
 138     baroState.calib.ap = (3.0e-5f  * readSignedRegister(busDev, REG_COE_PR31, 2)) / 32767;
 139     baroState.calib.bp = (10 * readSignedRegister(busDev, REG_COE_PR21, 2)) / 32767 + 3.0e1f;
 140     baroState.calib.cp = readSignedRegister(busDev, REG_COE_PR11, 3);
 141     baroState.calib.at = (8.0e-11f  * readSignedRegister(busDev, REG_COE_TEMP31, 2)) / 32767;
 142     baroState.calib.bt = (1.6e-6f  * readSignedRegister(busDev, REG_COE_TEMP21, 2)) / 32767 - 6.6e-6f;
 143     baroState.calib.ct = (8.5e-3f  * readSignedRegister(busDev, REG_COE_TEMP11, 2)) / 32767 + 4.0e-2f;
 144
 145     // Configure IIR filter
 146     busWrite(busDev, REG_IIR_CNT, REG_IIR_CNT_VAL_8X);
 147
 148     return true;
 149 }
 150
 151 #define DETECTION_MAX_RETRY_COUNT   5
 152 static bool deviceDetect(busDevice_t * busDev)
 153 {
 154     for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
 155         uint8_t chipId;
 156
 157         busRead(busDev, REG_CHIP_ID, &chipId);
 158
 159         if (chipId == BARO_2SMBP_CHIP_ID) {
 160             return true;
 161         }
 162
 163         delay(50);
 164     };
 165
 166     return false;
 167 }
 168
 169 static bool b2smpbStartUP(baroDev_t *baro)
 170 {
 171     // start a forced measurement
 172     return busWrite(baro->busDev, REG_CTRL_MEAS, REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED);
 173 }
 174
 175 static bool b2smpbGetUP(baroDev_t *baro)
 176 {
 177     int32_t dtp;
 178     float tr, pl, tmp;
 179
 180     if (!busReadBuf(baro->busDev, REG_PRESS_TXD2, &baroDataBuf[0], 6)) {
 181         return false;
 182     }
 183
 184     // Calculate compensated temperature
 185     dtp = getSigned24bitValue(&baroDataBuf[3]);
 186
 187     tmp = baroState.calib.ba * baroState.calib.ba;
 188     tr = (-1 * baroState.calib.ba - sqrtf(tmp - 4 * baroState.calib.aa * (baroState.calib.ca - dtp))) / (2 * baroState.calib.aa);
 189     baroState.temperature = tr / 256;
 190
 191     // Calculate raw pressure
 192     dtp = getSigned24bitValue(&baroDataBuf[0]);
 193
 194     tmp = baroState.calib.bp * baroState.calib.bp;
 195     pl = (sqrtf(tmp - 4 * baroState.calib.ap * (baroState.calib.cp - dtp)) - baroState.calib.bp) / (2 * baroState.calib.ap);
 196
 197     // Calculate temperature compensated pressure
 198     tmp = tr * tr;
 199     baroState.pressure = pl / (baroState.calib.at * tmp + baroState.calib.bt * tr + baroState.calib.ct + 1);
 200
 201     return true;
 202 }
 203
 204 static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
 205 {
 206     UNUSED(baro);
 207
 208     if (pressure) {
 209         *pressure = baroState.pressure;
 210     }
 211
 212     if (temperature) {
 213         *temperature = (baroState.temperature * 100);   // to centidegrees
 214     }
 215
 216     return true;
 217 }
 218
 219 bool baro2SMPB02BDetect(baroDev_t *baro)
 220 {
 221     baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_B2SMPB, 0, OWNER_BARO);
 222
 223     if (baro->busDev == NULL) {
 224         return false;
 225     }
 226
 227     if (!deviceDetect(baro->busDev)) {
 228         busDeviceDeInit(baro->busDev);
 229         return false;
 230     }
 231
 232     if (!deviceConfigure(baro->busDev)) {
 233         busDeviceDeInit(baro->busDev);
 234         return false;
 235     }
 236
 237     baro->up_delay = 35000; // measurement takes 33.7 ms with 4x / 32x averaging
 238     baro->start_up = b2smpbStartUP;
 239     baro->get_up = b2smpbGetUP;
 240
 241     baro->ut_delay = 0;
 242     baro->start_ut = NULL;
 243     baro->get_ut = NULL;
 244
 245     baro->calculate = deviceCalculate;
 246
 247     return true;
 248 }
 249
 250 #endif