mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
252 lines
7 KiB
C
252 lines
7 KiB
C
/*
|
|
* This file is part of Cleanflight, Betaflight and INAV.
|
|
*
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
|
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* Alternatively, the contents of this file may be used under the terms
|
|
* of the GNU General Public License Version 3, as described below:
|
|
*
|
|
* This file is free software: you may copy, redistribute and/or modify
|
|
* it under the terms of the GNU General Public License as published by the
|
|
* Free Software Foundation, either version 3 of the License, or (at your
|
|
* option) any later version.
|
|
*
|
|
* This file is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
|
* Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see http://www.gnu.org/licenses/.
|
|
*
|
|
* Copyright: INAVFLIGHT OU
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include <platform.h>
|
|
|
|
#include "build/build_config.h"
|
|
#include "build/debug.h"
|
|
#include "common/utils.h"
|
|
|
|
#include "drivers/io.h"
|
|
#include "drivers/bus.h"
|
|
#include "drivers/time.h"
|
|
#include "drivers/barometer/barometer.h"
|
|
|
|
#include "drivers/barometer/barometer_2smpb_02b.h"
|
|
|
|
#if defined(USE_BARO) && defined(USE_BARO_B2SMPB)
|
|
|
|
#define BARO_2SMBP_I2C_ADDRESS 0x70
|
|
|
|
#define BARO_2SMBP_CHIP_ID 0x5C
|
|
|
|
#define REG_CHIP_ID 0xD1
|
|
#define REG_RESET 0xE0
|
|
|
|
#define REG_COE_PR11 0xA0
|
|
#define REG_COE_PR21 0xA3
|
|
#define REG_COE_PR31 0xA5
|
|
#define REG_COE_TEMP11 0xA7
|
|
#define REG_COE_TEMP21 0xA9
|
|
#define REG_COE_TEMP31 0xAB
|
|
#define REG_COE_PTAT11 0xAD
|
|
#define REG_COE_PTAT21 0xB1
|
|
#define REG_COE_PTAT31 0xB3
|
|
|
|
#define REG_IIR_CNT 0xF1
|
|
#define REG_DEVICE_STAT 0xF3
|
|
#define REG_CTRL_MEAS 0xF4
|
|
#define REG_IO_SETUP 0xF5
|
|
#define REG_PRESS_TXD2 0xF7
|
|
|
|
// Value for CTRL_MEAS with 4x temperature averaging, 32x perssure, forced mode
|
|
#define REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED ((0x03 << 5) | (0x05 << 2) | 0x01)
|
|
|
|
// IIR coefficient setting 8x
|
|
#define REG_IIR_CNT_VAL_8X 0x03
|
|
|
|
typedef struct {
|
|
float aa;
|
|
float ba;
|
|
int32_t ca;
|
|
float ap;
|
|
float bp;
|
|
int32_t cp;
|
|
float at;
|
|
float bt;
|
|
float ct;
|
|
} calibrationCoefficients_t;
|
|
|
|
typedef struct {
|
|
calibrationCoefficients_t calib;
|
|
float pressure; // Pa
|
|
float temperature; // DegC
|
|
} baroState_t;
|
|
|
|
static baroState_t baroState;
|
|
static uint8_t baroDataBuf[6];
|
|
|
|
|
|
static int32_t readSignedRegister(busDevice_t * busDev, uint8_t reg, uint8_t nBytes)
|
|
{
|
|
uint8_t buf[3];
|
|
uint32_t rawValue = 0;
|
|
|
|
busReadBuf(busDev, reg, &buf[0], nBytes);
|
|
|
|
for (int i=0; i<nBytes; i++) {
|
|
rawValue += (uint32_t)buf[i] << (8 * (nBytes - i - 1));
|
|
}
|
|
|
|
// 2's complement
|
|
if (rawValue & ((int32_t)1 << (8 * nBytes - 1))) {
|
|
// Negative
|
|
return ((int32_t)rawValue) - ((int32_t)1 << (8 * nBytes));
|
|
}
|
|
else {
|
|
return rawValue;
|
|
}
|
|
}
|
|
|
|
static int32_t getSigned24bitValue(uint8_t * pData)
|
|
{
|
|
uint32_t raw;
|
|
|
|
raw = (((uint32_t)pData[0] << 16) | ((uint32_t)pData[1] << 8) | (uint32_t)pData[2]) - ((uint32_t)1 << 23);
|
|
|
|
return raw;
|
|
}
|
|
|
|
static bool deviceConfigure(busDevice_t * busDev)
|
|
{
|
|
/** Note: Chip reset causes I2C error due missing ACK. This causes interrupt based read (busReadRegisterBufferStart)
|
|
to not work (read stops due to error flags). It works fine without chip reset. **/
|
|
|
|
//busWrite(busDev, REG_RESET, 0xE6);
|
|
|
|
// No need to write IO_SETUP register: default values are fine
|
|
|
|
// Read calibration coefficients and scale them
|
|
baroState.calib.aa = (4.2e-4f * readSignedRegister(busDev, REG_COE_PTAT31, 2)) / 32767;
|
|
baroState.calib.ba = (8.0e0f * readSignedRegister(busDev, REG_COE_PTAT21, 2)) / 32767 - 1.6e2f;
|
|
baroState.calib.ca = readSignedRegister(busDev, REG_COE_PTAT11, 3);
|
|
baroState.calib.ap = (3.0e-5f * readSignedRegister(busDev, REG_COE_PR31, 2)) / 32767;
|
|
baroState.calib.bp = (10 * readSignedRegister(busDev, REG_COE_PR21, 2)) / 32767 + 3.0e1f;
|
|
baroState.calib.cp = readSignedRegister(busDev, REG_COE_PR11, 3);
|
|
baroState.calib.at = (8.0e-11f * readSignedRegister(busDev, REG_COE_TEMP31, 2)) / 32767;
|
|
baroState.calib.bt = (1.6e-6f * readSignedRegister(busDev, REG_COE_TEMP21, 2)) / 32767 - 6.6e-6f;
|
|
baroState.calib.ct = (8.5e-3f * readSignedRegister(busDev, REG_COE_TEMP11, 2)) / 32767 + 4.0e-2f;
|
|
|
|
// Configure IIR filter
|
|
busWrite(busDev, REG_IIR_CNT, REG_IIR_CNT_VAL_8X);
|
|
|
|
return true;
|
|
}
|
|
|
|
#define DETECTION_MAX_RETRY_COUNT 5
|
|
static bool deviceDetect(busDevice_t * busDev)
|
|
{
|
|
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
|
|
uint8_t chipId;
|
|
|
|
busRead(busDev, REG_CHIP_ID, &chipId);
|
|
|
|
if (chipId == BARO_2SMBP_CHIP_ID) {
|
|
return true;
|
|
}
|
|
|
|
delay(50);
|
|
};
|
|
|
|
return false;
|
|
}
|
|
|
|
static bool b2smpbStartUP(baroDev_t *baro)
|
|
{
|
|
// start a forced measurement
|
|
return busWrite(baro->busDev, REG_CTRL_MEAS, REG_CLT_MEAS_VAL_TAVG4X_PAVG32X_FORCED);
|
|
}
|
|
|
|
static bool b2smpbGetUP(baroDev_t *baro)
|
|
{
|
|
int32_t dtp;
|
|
float tr, pl, tmp;
|
|
|
|
if (!busReadBuf(baro->busDev, REG_PRESS_TXD2, &baroDataBuf[0], 6)) {
|
|
return false;
|
|
}
|
|
|
|
// Calculate compensated temperature
|
|
dtp = getSigned24bitValue(&baroDataBuf[3]);
|
|
|
|
tmp = baroState.calib.ba * baroState.calib.ba;
|
|
tr = (-1 * baroState.calib.ba - sqrtf(tmp - 4 * baroState.calib.aa * (baroState.calib.ca - dtp))) / (2 * baroState.calib.aa);
|
|
baroState.temperature = tr / 256;
|
|
|
|
// Calculate raw pressure
|
|
dtp = getSigned24bitValue(&baroDataBuf[0]);
|
|
|
|
tmp = baroState.calib.bp * baroState.calib.bp;
|
|
pl = (sqrtf(tmp - 4 * baroState.calib.ap * (baroState.calib.cp - dtp)) - baroState.calib.bp) / (2 * baroState.calib.ap);
|
|
|
|
// Calculate temperature compensated pressure
|
|
tmp = tr * tr;
|
|
baroState.pressure = pl / (baroState.calib.at * tmp + baroState.calib.bt * tr + baroState.calib.ct + 1);
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *temperature)
|
|
{
|
|
UNUSED(baro);
|
|
|
|
if (pressure) {
|
|
*pressure = baroState.pressure;
|
|
}
|
|
|
|
if (temperature) {
|
|
*temperature = (baroState.temperature * 100); // to centidegrees
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool baro2SMPB02BDetect(baroDev_t *baro)
|
|
{
|
|
baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_B2SMPB, 0, OWNER_BARO);
|
|
|
|
if (baro->busDev == NULL) {
|
|
return false;
|
|
}
|
|
|
|
if (!deviceDetect(baro->busDev)) {
|
|
busDeviceDeInit(baro->busDev);
|
|
return false;
|
|
}
|
|
|
|
if (!deviceConfigure(baro->busDev)) {
|
|
busDeviceDeInit(baro->busDev);
|
|
return false;
|
|
}
|
|
|
|
baro->up_delay = 35000; // measurement takes 33.7 ms with 4x / 32x averaging
|
|
baro->start_up = b2smpbStartUP;
|
|
baro->get_up = b2smpbGetUP;
|
|
|
|
baro->ut_delay = 0;
|
|
baro->start_ut = NULL;
|
|
baro->get_ut = NULL;
|
|
|
|
baro->calculate = deviceCalculate;
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif
|