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);