From caa1c8cc36e80524e4c1bdca34db4662140df9ff Mon Sep 17 00:00:00 2001 From: Martin Luessi Date: Fri, 14 Jan 2022 15:09:37 -0800 Subject: [PATCH] Add support for Omron 2SMPB-02B barometer --- src/main/CMakeLists.txt | 2 + .../drivers/barometer/barometer_2smpb_02b.c | 252 ++++++++++++++++++ .../drivers/barometer/barometer_2smpb_02b.h | 29 ++ src/main/drivers/bus.h | 1 + src/main/sensors/barometer.c | 14 + src/main/sensors/barometer.h | 5 +- src/main/target/common_hardware.c | 10 + 7 files changed, 311 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/barometer/barometer_2smpb_02b.c create mode 100644 src/main/drivers/barometer/barometer_2smpb_02b.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index decb759a75..c79f295959 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -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 diff --git a/src/main/drivers/barometer/barometer_2smpb_02b.c b/src/main/drivers/barometer/barometer_2smpb_02b.c new file mode 100644 index 0000000000..0c975bca2b --- /dev/null +++ b/src/main/drivers/barometer/barometer_2smpb_02b.c @@ -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 +#include +#include + +#include + +#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; ibusDev, 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 diff --git a/src/main/drivers/barometer/barometer_2smpb_02b.h b/src/main/drivers/barometer/barometer_2smpb_02b.h new file mode 100644 index 0000000000..5496991873 --- /dev/null +++ b/src/main/drivers/barometer/barometer_2smpb_02b.h @@ -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); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 1e3123c3c6..8b48855905 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -98,6 +98,7 @@ typedef enum { DEVHW_SPL06, DEVHW_BMP388, DEVHW_DPS310, + DEVHW_B2SMPB, /* Compass chips */ DEVHW_HMC5883, diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 63532a3986..822ccec5cd 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -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 diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 3c04c86e46..79ffa6339d 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -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; diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 4c44fe991b..7ff1444177 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -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)