mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-17 21:35:37 +03:00
Merge pull request #7755 from mluessi/ml_baro_2SMPB-02B
Add support for Omron 2SMPB-02B barometer
This commit is contained in:
Paweł Spychalski
GitHub
commit
a4bb357f55
7 changed files with 311 additions and 2 deletions
|
|
@ -121,6 +121,8 @@ main_sources(COMMON_SRC
|
|||
drivers/barometer/barometer_spl06.h
|
||||
drivers/barometer/barometer_msp.c
|
||||
drivers/barometer/barometer_msp.h
|
||||
drivers/barometer/barometer_2smpb_02b.c
|
||||
drivers/barometer/barometer_2smpb_02b.h
|
||||
|
||||
drivers/buf_writer.c
|
||||
drivers/buf_writer.h
|
||||
|
|
|
|||
252
src/main/drivers/barometer/barometer_2smpb_02b.c
Normal file
252
src/main/drivers/barometer/barometer_2smpb_02b.c
Normal file
|
|
@ -0,0 +1,252 @@
|
|||
/*
|
||||
* 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
|
||||
29
src/main/drivers/barometer/barometer_2smpb_02b.h
Normal file
29
src/main/drivers/barometer/barometer_2smpb_02b.h
Normal file
|
|
@ -0,0 +1,29 @@
|
|||
/*
|
||||
* 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
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
bool baro2SMPB02BDetect(baroDev_t *baro);
|
||||
|
|
@ -98,6 +98,7 @@ typedef enum {
|
|||
DEVHW_SPL06,
|
||||
DEVHW_BMP388,
|
||||
DEVHW_DPS310,
|
||||
DEVHW_B2SMPB,
|
||||
|
||||
/* Compass chips */
|
||||
DEVHW_HMC5883,
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@
|
|||
#include "drivers/barometer/barometer_ms56xx.h"
|
||||
#include "drivers/barometer/barometer_spl06.h"
|
||||
#include "drivers/barometer/barometer_dps310.h"
|
||||
#include "drivers/barometer/barometer_2smpb_02b.h"
|
||||
#include "drivers/barometer/barometer_msp.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
|
|
@ -184,6 +185,19 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
|
|||
}
|
||||
FALLTHROUGH;
|
||||
|
||||
case BARO_B2SMPB:
|
||||
#if defined(USE_BARO_B2SMPB)
|
||||
if (baro2SMPB02BDetect(dev)) {
|
||||
baroHardware = BARO_B2SMPB;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (baroHardwareToUse != BARO_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
||||
case BARO_MSP:
|
||||
#ifdef USE_BARO_MSP
|
||||
// Skip autodetection for MSP baro, only allow manual config
|
||||
|
|
|
|||
|
|
@ -32,8 +32,9 @@ typedef enum {
|
|||
BARO_SPL06 = 7,
|
||||
BARO_BMP388 = 8,
|
||||
BARO_DPS310 = 9,
|
||||
BARO_MSP = 10,
|
||||
BARO_FAKE = 11,
|
||||
BARO_B2SMPB = 10,
|
||||
BARO_MSP = 11,
|
||||
BARO_FAKE = 12,
|
||||
BARO_MAX = BARO_FAKE
|
||||
} baroSensor_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -164,6 +164,16 @@
|
|||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(USE_BARO_B2SMPB)
|
||||
#if defined(B2SMPB_SPI_BUS)
|
||||
BUSDEV_REGISTER_SPI(busdev_b2smpb, DEVHW_B2SMPB, B2SMPB_SPI_BUS, B2SMPB_CS_PIN, NONE, DEVFLAGS_NONE, 0);
|
||||
#elif defined(B2SMPB_I2C_BUS) || defined(BARO_I2C_BUS)
|
||||
#if !defined(B2SMPB_I2C_BUS)
|
||||
#define B2SMPB_I2C_BUS BARO_I2C_BUS
|
||||
#endif
|
||||
BUSDEV_REGISTER_I2C(busdev_b2smpb, DEVHW_B2SMPB, B2SMPB_I2C_BUS, 0x70, NONE, DEVFLAGS_NONE, 0);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/** COMPASS SENSORS **/
|
||||
#if !defined(USE_TARGET_MAG_HARDWARE_DESCRIPTORS)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue