diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c new file mode 100644 index 0000000000..dcedfc373c --- /dev/null +++ b/src/main/drivers/barometer/barometer_dps310.c @@ -0,0 +1,321 @@ +/* + * This file is part of 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_dps310.h" + +#if defined(USE_BARO) && defined(USE_BARO_DPS310) + +#define DPS310_REG_PSR_B2 0x00 +#define DPS310_REG_PSR_B1 0x01 +#define DPS310_REG_PSR_B0 0x02 +#define DPS310_REG_TMP_B2 0x03 +#define DPS310_REG_TMP_B1 0x04 +#define DPS310_REG_TMP_B0 0x05 +#define DPS310_REG_PRS_CFG 0x06 +#define DPS310_REG_TMP_CFG 0x07 +#define DPS310_REG_MEAS_CFG 0x08 +#define DPS310_REG_CFG_REG 0x09 + +#define DPS310_REG_RESET 0x0C +#define DPS310_REG_ID 0x0D + +#define DPS310_REG_COEF 0x10 +#define DPS310_REG_COEF_SRCE 0x28 + + +#define DPS310_ID_REV_AND_PROD_ID (0x10) + +#define DPS310_RESET_BIT_SOFT_RST (0x09) // 0b1001 + +#define DPS310_MEAS_CFG_COEF_RDY (1 << 7) +#define DPS310_MEAS_CFG_SENSOR_RDY (1 << 6) +#define DPS310_MEAS_CFG_TMP_RDY (1 << 5) +#define DPS310_MEAS_CFG_PRS_RDY (1 << 4) +#define DPS310_MEAS_CFG_MEAS_CTRL_CONT (0x7) + +#define DPS310_PRS_CFG_BIT_PM_RATE_32HZ (0x50) // 101 - 32 measurements pr. sec. +#define DPS310_PRS_CFG_BIT_PM_PRC_16 (0x04) // 0100 - 16 times (Standard). + +#define DPS310_TMP_CFG_BIT_TMP_EXT (0x80) // +#define DPS310_TMP_CFG_BIT_TMP_RATE_32HZ (0x50) // 101 - 32 measurements pr. sec. +#define DPS310_TMP_CFG_BIT_TMP_PRC_16 (0x04) // 0100 - 16 times (Standard). + +#define DPS310_CFG_REG_BIT_P_SHIFT (0x04) +#define DPS310_CFG_REG_BIT_T_SHIFT (0x08) + +#define DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE (0x80) + +typedef struct { + int16_t c0; // 12bit + int16_t c1; // 12bit + int32_t c00; // 20bit + int32_t c10; // 20bit + int16_t c01; // 16bit + int16_t c11; // 16bit + int16_t c20; // 16bit + int16_t c21; // 16bit + int16_t c30; // 16bit +} calibrationCoefficients_t; + +typedef struct { + calibrationCoefficients_t calib; + float pressure; // Pa + float temperature; // DegC +} baroState_t; + +static baroState_t baroState; + + +// Helper functions +static uint8_t registerRead(busDevice_t * busDev, uint8_t reg) +{ + uint8_t buf; + busRead(busDev, reg, &buf); + return buf; +} + +static void registerWrite(busDevice_t * busDev, uint8_t reg, uint8_t value) +{ + busWrite(busDev, reg, value); +} + +static void registerSetBits(busDevice_t * busDev, uint8_t reg, uint8_t setbits) +{ + uint8_t val = registerRead(busDev, reg); + + if ((val & setbits) != setbits) { + val |= setbits; + registerWrite(busDev, reg, val); + } +} + +static int32_t getTwosComplement(uint32_t raw, uint8_t length) +{ + if (raw & ((int)1 << (length - 1))) { + return ((int32_t)raw) - ((int32_t)1 << length); + } + else { + return raw; + } +} + +static bool deviceConfigure(busDevice_t * busDev) +{ + // Trigger a chip reset + registerSetBits(busDev, DPS310_REG_RESET, DPS310_RESET_BIT_SOFT_RST); + + // Sleep 40ms + delay(40); + + uint8_t status = registerRead(busDev, DPS310_REG_MEAS_CFG); + + // Check if coefficients are available + if ((status & DPS310_MEAS_CFG_COEF_RDY) == 0) { + return false; + } + + // Check if sensor initialization is complete + if ((status & DPS310_MEAS_CFG_SENSOR_RDY) == 0) { + return false; + } + + // 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register. + // Note: The coefficients read from the coefficient register are 2's complement numbers. + uint8_t coef[18]; + if (!busReadBuf(busDev, DPS310_REG_COEF, coef, sizeof(coef))) { + return false; + } + + // 0x11 c0 [3:0] + 0x10 c0 [11:4] + baroState.calib.c0 = getTwosComplement(((uint32_t)coef[0] << 4) | (((uint32_t)coef[1] >> 4) & 0x0F), 12); + + // 0x11 c1 [11:8] + 0x12 c1 [7:0] + baroState.calib.c1 = getTwosComplement((((uint32_t)coef[1] & 0x0F) << 8) | (uint32_t)coef[2], 12); + + // 0x13 c00 [19:12] + 0x14 c00 [11:4] + 0x15 c00 [3:0] + baroState.calib.c00 = getTwosComplement(((uint32_t)coef[3] << 12) | ((uint32_t)coef[4] << 4) | (((uint32_t)coef[5] >> 4) & 0x0F), 20); + + // 0x15 c10 [19:16] + 0x16 c10 [15:8] + 0x17 c10 [7:0] + baroState.calib.c10 = getTwosComplement((((uint32_t)coef[5] & 0x0F) << 16) | ((uint32_t)coef[6] << 8) | (uint32_t)coef[7], 20); + + // 0x18 c01 [15:8] + 0x19 c01 [7:0] + baroState.calib.c01 = getTwosComplement(((uint32_t)coef[8] << 8) | (uint32_t)coef[9], 16); + + // 0x1A c11 [15:8] + 0x1B c11 [7:0] + baroState.calib.c11 = getTwosComplement(((uint32_t)coef[8] << 8) | (uint32_t)coef[9], 16); + + // 0x1C c20 [15:8] + 0x1D c20 [7:0] + baroState.calib.c20 = getTwosComplement(((uint32_t)coef[12] << 8) | (uint32_t)coef[13], 16); + + // 0x1E c21 [15:8] + 0x1F c21 [7:0] + baroState.calib.c21 = getTwosComplement(((uint32_t)coef[14] << 8) | (uint32_t)coef[15], 16); + + // 0x20 c30 [15:8] + 0x21 c30 [7:0] + baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16); + + // PRS_CFG: pressure measurement rate (32 Hz) and oversampling (16 time standard) + registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16); + + // TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times) + const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + + // CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times) + registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT); + + // MEAS_CFG: Continuous pressure and temperature measurement + registerSetBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_CONT); + + return true; +} + +static bool deviceReadMeasurement(baroDev_t *baro) +{ + // 1. Check if pressure is ready + bool pressure_ready = registerRead(baro->busDev, DPS310_REG_MEAS_CFG) & DPS310_MEAS_CFG_PRS_RDY; + if (!pressure_ready) { + return false; + } + + // 2. Choose scaling factors kT (for temperature) and kP (for pressure) based on the chosen precision rate. + // The scaling factors are listed in Table 9. + static float kT = 253952; // 16 times (Standard) + static float kP = 253952; // 16 times (Standard) + + // 3. Read the pressure and temperature result from the registers + // Read PSR_B2, PSR_B1, PSR_B0, TMP_B2, TMP_B1, TMP_B0 + uint8_t buf[6]; + if (!busReadBuf(baro->busDev, DPS310_REG_PSR_B2, buf, 6)) { + return false; + } + + const int32_t Praw = getTwosComplement((buf[0] << 16) + (buf[1] << 8) + buf[2], 24); + const int32_t Traw = getTwosComplement((buf[3] << 16) + (buf[4] << 8) + buf[5], 24); + + // 4. Calculate scaled measurement results. + const float Praw_sc = Praw / kP; + const float Traw_sc = Traw / kT; + + // 5. Calculate compensated measurement results. + const float c00 = baroState.calib.c00; + const float c01 = baroState.calib.c01; + const float c10 = baroState.calib.c10; + const float c11 = baroState.calib.c11; + const float c20 = baroState.calib.c20; + const float c21 = baroState.calib.c21; + const float c30 = baroState.calib.c30; + + const float c0 = baroState.calib.c0; + const float c1 = baroState.calib.c1; + + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + baroState.temperature = c0 * 0.5f + c1 * Traw_sc; + + 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; +} + + + +#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[1]; + + delay(100); + + bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1); + + if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) { + return true; + } + }; + + return false; +} + +bool baroDPS310Detect(baroDev_t *baro) +{ + baro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_DPS310, 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; + } + + const uint32_t baroDelay = 1000000 / 32 / 2; // twice the sample rate to capture all new data + + baro->ut_delay = 0; + baro->start_ut = NULL; + baro->get_ut = NULL; + + baro->up_delay = baroDelay; + baro->start_up = NULL; + baro->get_up = deviceReadMeasurement; + + baro->calculate = deviceCalculate; + + return true; +} + +#endif diff --git a/src/main/drivers/barometer/barometer_dps310.h b/src/main/drivers/barometer/barometer_dps310.h new file mode 100644 index 0000000000..314c282c5c --- /dev/null +++ b/src/main/drivers/barometer/barometer_dps310.h @@ -0,0 +1,29 @@ +/* + * This file is part of 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 baroDPS310Detect(baroDev_t *baro); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 7f7d06d092..d5a877f7d6 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -104,6 +104,7 @@ typedef enum { DEVHW_LPS25H, DEVHW_SPL06, DEVHW_BMP388, + DEVHW_DPS310, /* Compass chips */ DEVHW_HMC5883, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5a760bde8f..29da670777 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -16,7 +16,7 @@ tables: values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"] enum: opticalFlowSensor_e - name: baro_hardware - values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "FAKE"] + values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "FAKE"] enum: baroSensor_e - name: pitot_hardware values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE"] diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index f9e72f15a8..567264f783 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -38,6 +38,7 @@ #include "drivers/barometer/barometer_fake.h" #include "drivers/barometer/barometer_ms56xx.h" #include "drivers/barometer/barometer_spl06.h" +#include "drivers/barometer/barometer_dps310.h" #include "drivers/time.h" #include "fc/runtime_config.h" @@ -172,6 +173,19 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) } FALLTHROUGH; + case BARO_DPS310: +#if defined(USE_BARO_DPS310) + if (baroDPS310Detect(dev)) { + baroHardware = BARO_DPS310; + 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_FAKE: #ifdef USE_FAKE_BARO if (fakeBaroDetect(dev)) { @@ -265,15 +279,23 @@ uint32_t baroUpdate(void) switch (state) { default: case BAROMETER_NEEDS_SAMPLES: - baro.dev.get_ut(&baro.dev); - baro.dev.start_up(&baro.dev); + if (baro.dev.get_ut) { + baro.dev.get_ut(&baro.dev); + } + if (baro.dev.start_up) { + baro.dev.start_up(&baro.dev); + } state = BAROMETER_NEEDS_CALCULATION; return baro.dev.up_delay; break; case BAROMETER_NEEDS_CALCULATION: - baro.dev.get_up(&baro.dev); - baro.dev.start_ut(&baro.dev); + if (baro.dev.get_up) { + baro.dev.get_up(&baro.dev); + } + if (baro.dev.start_ut) { + baro.dev.start_ut(&baro.dev); + } baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature); if (barometerConfig()->use_median_filtering) { baro.baroPressure = applyBarometerMedianFilter(baro.baroPressure); diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 3e10700f0c..6301c79b0b 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -29,9 +29,10 @@ typedef enum { BARO_BMP280 = 4, BARO_MS5607 = 5, BARO_LPS25H = 6, - BARO_SPL06 = 7, + BARO_SPL06 = 7, BARO_BMP388 = 8, - BARO_FAKE = 9, + BARO_DPS310 = 9, + BARO_FAKE = 10, BARO_MAX = BARO_FAKE } baroSensor_e; diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index c4603911cb..c445959610 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -120,6 +120,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/FURYF4OSD/target.mk b/src/main/target/FURYF4OSD/target.mk index 0b76bb0235..272b981bb7 100644 --- a/src/main/target/FURYF4OSD/target.mk +++ b/src/main/target/FURYF4OSD/target.mk @@ -7,6 +7,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_ak8963.c \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index 27d99bdcf7..cbb9d29428 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -60,6 +60,7 @@ #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_BMP280 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/IFLIGHTF4_TWING/target.mk b/src/main/target/IFLIGHTF4_TWING/target.mk index 8979319492..b4c202b347 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.mk +++ b/src/main/target/IFLIGHTF4_TWING/target.mk @@ -5,6 +5,7 @@ FEATURES += VCP ONBOARDFLASH MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_ist8310.c \ diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index a6a9e63de2..bac28ceea0 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -63,6 +63,7 @@ #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_BMP280 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 diff --git a/src/main/target/IFLIGHTF7_TWING/target.mk b/src/main/target/IFLIGHTF7_TWING/target.mk index 57bc66e9e8..77545402ae 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.mk +++ b/src/main/target/IFLIGHTF7_TWING/target.mk @@ -4,6 +4,7 @@ FEATURES += ONBOARDFLASH VCP MSC TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_ist8310.c \ diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 077155b1ff..37e7f78096 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -147,6 +147,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKF405/target.mk b/src/main/target/MATEKF405/target.mk index dd5ce7864a..62dd163f0c 100755 --- a/src/main/target/MATEKF405/target.mk +++ b/src/main/target/MATEKF405/target.mk @@ -7,6 +7,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_ak8963.c \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 513b14a8b2..81681b94f5 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -58,6 +58,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKF405SE/target.mk b/src/main/target/MATEKF405SE/target.mk index 8a0cc40ea7..13886f855e 100644 --- a/src/main/target/MATEKF405SE/target.mk +++ b/src/main/target/MATEKF405SE/target.mk @@ -6,6 +6,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_ak8963.c \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 7063d18c38..967e3c0ef8 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -118,6 +118,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define USE_BARO_DPS310 #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411/target.mk b/src/main/target/MATEKF411/target.mk index f383248f31..1ea03d20db 100755 --- a/src/main/target/MATEKF411/target.mk +++ b/src/main/target/MATEKF411/target.mk @@ -7,6 +7,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index cbffcd1a2c..790be95bac 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -95,6 +95,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411SE/target.mk b/src/main/target/MATEKF411SE/target.mk index ee090ce48f..9481999868 100755 --- a/src/main/target/MATEKF411SE/target.mk +++ b/src/main/target/MATEKF411SE/target.mk @@ -6,6 +6,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_mag3110.c \ drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 62227998e8..a99a60afc1 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -56,6 +56,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 #define USE_BARO_BMP085 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722/target.mk b/src/main/target/MATEKF722/target.mk index f221bb51ca..a62d66f532 100755 --- a/src/main/target/MATEKF722/target.mk +++ b/src/main/target/MATEKF722/target.mk @@ -5,6 +5,7 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_dps310.c \ drivers/barometer/barometer_ms56xx.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index 38e9a8f380..f2f1bca242 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -53,6 +53,7 @@ #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722PX/target.mk b/src/main/target/MATEKF722PX/target.mk index 0db5e55e78..3b7c3615dd 100755 --- a/src/main/target/MATEKF722PX/target.mk +++ b/src/main/target/MATEKF722PX/target.mk @@ -5,6 +5,7 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index 56dcdcc886..7942a30e21 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -70,6 +70,7 @@ #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722SE/target.mk b/src/main/target/MATEKF722SE/target.mk index 63b91d3000..19ae98915c 100644 --- a/src/main/target/MATEKF722SE/target.mk +++ b/src/main/target/MATEKF722SE/target.mk @@ -7,6 +7,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_ak8975.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index fcddab2693..8fefd8d077 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -74,6 +74,7 @@ #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF765/target.mk b/src/main/target/MATEKF765/target.mk index 11d75e59b3..9e34a0864e 100644 --- a/src/main/target/MATEKF765/target.mk +++ b/src/main/target/MATEKF765/target.mk @@ -7,6 +7,7 @@ TARGET_SRC = \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms56xx.c \ + drivers/barometer/barometer_dps310.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/compass/compass_ist8310.c \ diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 1e35d2baef..b372e9c8f7 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -149,6 +149,18 @@ #endif #endif +#if defined(USE_BARO_DPS310) + #if defined(DPS310_SPI_BUS) + BUSDEV_REGISTER_SPI(busdev_dps310, DEVHW_DPS310, DPS310_SPI_BUS, DPS310_CS_PIN, NONE, DEVFLAGS_NONE, 0); + #elif defined(DPS310_I2C_BUS) || defined(BARO_I2C_BUS) + #if !defined(DPS310_I2C_BUS) + #define DPS310_I2C_BUS BARO_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_dps310, DEVHW_DPS310, DPS310_I2C_BUS, 0x76, NONE, DEVFLAGS_NONE, 0); + #endif +#endif + + /** COMPASS SENSORS **/ #if !defined(USE_TARGET_MAG_HARDWARE_DESCRIPTORS) #if defined(USE_MAG_HMC5883)