diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c
index 8c0d7c27d1..804291c8f4 100644
--- a/src/main/drivers/barometer_ms5611.c
+++ b/src/main/drivers/barometer_ms5611.c
@@ -23,6 +23,7 @@
#include "build/build_config.h"
#include "barometer.h"
+#include "barometer_spi_ms5611.h"
#include "gpio.h"
#include "system.h"
@@ -66,8 +67,15 @@ bool ms5611Detect(baroDev_t *baro)
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
+#ifdef USE_BARO_SPI_MS5611
+ ms5611SpiInit();
+ ms5611SpiReadCommand(CMD_PROM_RD, 1, &sig);
+ if (sig == 0xFF)
+ return false;
+#else
if (!i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig))
return false;
+#endif
ms5611_reset();
// read all coefficients
@@ -91,14 +99,22 @@ bool ms5611Detect(baroDev_t *baro)
static void ms5611_reset(void)
{
+#ifdef USE_BARO_SPI_MS5611
+ ms5611SpiWriteCommand(CMD_RESET, 1);
+#else
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1);
+#endif
delayMicroseconds(2800);
}
static uint16_t ms5611_prom(int8_t coef_num)
{
uint8_t rxbuf[2] = { 0, 0 };
+#ifdef USE_BARO_SPI_MS5611
+ ms5611SpiReadCommand(CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
+#else
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
+#endif
return rxbuf[0] << 8 | rxbuf[1];
}
@@ -135,13 +151,21 @@ STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom)
static uint32_t ms5611_read_adc(void)
{
uint8_t rxbuf[3];
+#ifdef USE_BARO_SPI_MS5611
+ ms5611SpiReadCommand(CMD_ADC_READ, 3, rxbuf); // read ADC
+#else
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
+#endif
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
}
static void ms5611_start_ut(void)
{
+#ifdef USE_BARO_SPI_MS5611
+ ms5611SpiWriteCommand(CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
+#else
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
+#endif
}
static void ms5611_get_ut(void)
@@ -151,7 +175,11 @@ static void ms5611_get_ut(void)
static void ms5611_start_up(void)
{
+#ifdef USE_BARO_SPI_MS5611
+ ms5611SpiWriteCommand(CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
+#else
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
+#endif
}
static void ms5611_get_up(void)
diff --git a/src/main/drivers/barometer_spi_ms5611.c b/src/main/drivers/barometer_spi_ms5611.c
new file mode 100755
index 0000000000..700f7c78e3
--- /dev/null
+++ b/src/main/drivers/barometer_spi_ms5611.c
@@ -0,0 +1,74 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV. If not, see .
+ */
+
+#include
+#include
+
+#include
+
+#include "io.h"
+#include "bus_spi.h"
+
+#include "barometer.h"
+#include "barometer_ms5611.h"
+
+#ifdef USE_BARO_SPI_MS5611
+
+#define DISABLE_MS5611 IOHi(ms5611CsPin)
+#define ENABLE_MS5611 IOLo(ms5611CsPin)
+
+static IO_t ms5611CsPin = IO_NONE;
+
+bool ms5611SpiWriteCommand(uint8_t reg, uint8_t data)
+{
+ ENABLE_MS5611;
+ spiTransferByte(MS5611_SPI_INSTANCE, reg & 0x7F);
+ spiTransferByte(MS5611_SPI_INSTANCE, data);
+ DISABLE_MS5611;
+
+ return true;
+}
+
+bool ms5611SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data)
+{
+ ENABLE_MS5611;
+ spiTransferByte(MS5611_SPI_INSTANCE, reg | 0x80);
+ spiTransfer(MS5611_SPI_INSTANCE, data, NULL, length);
+ DISABLE_MS5611;
+
+ return true;
+}
+
+void ms5611SpiInit(void)
+{
+ static bool hardwareInitialised = false;
+
+ if (hardwareInitialised) {
+ return;
+ }
+
+ ms5611CsPin = IOGetByTag(IO_TAG(MS5611_CS_PIN));
+ IOInit(ms5611CsPin, OWNER_BARO_CS, 0);
+ IOConfigGPIO(ms5611CsPin, IOCFG_OUT_PP);
+
+ DISABLE_MS5611;
+
+ spiSetDivisor(MS5611_SPI_INSTANCE, SPI_CLOCK_STANDARD);
+
+ hardwareInitialised = true;
+}
+#endif
diff --git a/src/main/drivers/barometer_spi_ms5611.h b/src/main/drivers/barometer_spi_ms5611.h
new file mode 100755
index 0000000000..6404f3d0c5
--- /dev/null
+++ b/src/main/drivers/barometer_spi_ms5611.h
@@ -0,0 +1,22 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV. If not, see .
+ */
+
+#pragma once
+
+void ms5611SpiInit(void);
+bool ms5611SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data);
+bool ms5611SpiWriteCommand(uint8_t reg, uint8_t data);