From 278ebf3aeafe04a03afa1ac7ae4b09bfbf2c9fa0 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 26 Apr 2020 18:59:58 +1200 Subject: [PATCH] Fixed the MS5611 baro in SPI mode. --- src/main/drivers/barometer/barometer_ms5611.c | 140 +++++++++--------- src/main/drivers/bus.c | 60 ++++++++ src/main/drivers/bus.h | 4 + src/test/unit/baro_ms5611_unittest.cc | 8 +- 4 files changed, 135 insertions(+), 77 deletions(-) diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c index 358ae5fda7..3ef2545973 100644 --- a/src/main/drivers/barometer/barometer_ms5611.c +++ b/src/main/drivers/barometer/barometer_ms5611.c @@ -52,18 +52,6 @@ #define CMD_PROM_RD 0xA0 // Prom read command #define PROM_NB 8 -static void ms5611Reset(busDevice_t *busdev); -static uint16_t ms5611Prom(busDevice_t *busdev, int8_t coef_num); -STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom); -static void ms5611ReadAdc(busDevice_t *busdev); -static void ms5611StartUT(baroDev_t *baro); -static bool ms5611ReadUT(baroDev_t *baro); -static bool ms5611GetUT(baroDev_t *baro); -static void ms5611StartUP(baroDev_t *baro); -static bool ms5611ReadUP(baroDev_t *baro); -static bool ms5611GetUP(baroDev_t *baro); -STATIC_UNIT_TESTED void ms5611Calculate(int32_t *pressure, int32_t *temperature); - STATIC_UNIT_TESTED uint32_t ms5611_ut; // static result of temperature measurement STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM @@ -100,65 +88,9 @@ void ms5611BusDeinit(busDevice_t *busdev) #endif } -bool ms5611Detect(baroDev_t *baro) -{ - uint8_t sig; - int i; - bool defaultAddressApplied = false; - - delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms - - busDevice_t *busdev = &baro->busdev; - - ms5611BusInit(busdev); - - if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { - // Default address for MS5611 - busdev->busdev_u.i2c.address = MS5611_I2C_ADDR; - defaultAddressApplied = true; - } - - if (!busReadRegisterBuffer(busdev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) { - goto fail; - } - - ms5611Reset(busdev); - - // read all coefficients - for (i = 0; i < PROM_NB; i++) - ms5611_c[i] = ms5611Prom(busdev, i); - - // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line! - if (ms5611CRC(ms5611_c) != 0) { - goto fail; - } - - // TODO prom + CRC - baro->ut_delay = 10000; - baro->up_delay = 10000; - baro->start_ut = ms5611StartUT; - baro->read_ut = ms5611ReadUT; - baro->get_ut = ms5611GetUT; - baro->start_up = ms5611StartUP; - baro->read_up = ms5611ReadUP; - baro->get_up = ms5611GetUP; - baro->calculate = ms5611Calculate; - - return true; - -fail:; - ms5611BusDeinit(busdev); - - if (defaultAddressApplied) { - busdev->busdev_u.i2c.address = 0; - } - - return false; -} - static void ms5611Reset(busDevice_t *busdev) { - busWriteRegister(busdev, CMD_RESET, 1); + busRawWriteRegister(busdev, CMD_RESET, 1); delayMicroseconds(2800); } @@ -167,7 +99,7 @@ static uint16_t ms5611Prom(busDevice_t *busdev, int8_t coef_num) { uint8_t rxbuf[2] = { 0, 0 }; - busReadRegisterBuffer(busdev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command + busRawReadRegisterBuffer(busdev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command return rxbuf[0] << 8 | rxbuf[1]; } @@ -204,12 +136,12 @@ STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom) static void ms5611ReadAdc(busDevice_t *busdev) { - busReadRegisterBufferStart(busdev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC + busRawReadRegisterBufferStart(busdev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC } static void ms5611StartUT(baroDev_t *baro) { - busWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! + busRawWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start! } static bool ms5611ReadUT(baroDev_t *baro) @@ -219,6 +151,7 @@ static bool ms5611ReadUT(baroDev_t *baro) } ms5611ReadAdc(&baro->busdev); + return true; } @@ -227,13 +160,15 @@ static bool ms5611GetUT(baroDev_t *baro) if (busBusy(&baro->busdev, NULL)) { return false; } + ms5611_ut = sensor_data[0] << 16 | sensor_data[1] << 8 | sensor_data[2]; + return true; } static void ms5611StartUP(baroDev_t *baro) { - busWriteRegister(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! + busRawWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start! } static bool ms5611ReadUP(baroDev_t *baro) @@ -243,6 +178,7 @@ static bool ms5611ReadUP(baroDev_t *baro) } ms5611ReadAdc(&baro->busdev); + return true; } @@ -251,7 +187,9 @@ static bool ms5611GetUP(baroDev_t *baro) if (busBusy(&baro->busdev, NULL)) { return false; } + ms5611_up = sensor_data[0] << 16 | sensor_data[1] << 8 | sensor_data[2]; + return true; } @@ -286,4 +224,60 @@ STATIC_UNIT_TESTED void ms5611Calculate(int32_t *pressure, int32_t *temperature) if (temperature) *temperature = temp; } + +bool ms5611Detect(baroDev_t *baro) +{ + uint8_t sig; + int i; + bool defaultAddressApplied = false; + + delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms + + busDevice_t *busdev = &baro->busdev; + + ms5611BusInit(busdev); + + if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) { + // Default address for MS5611 + busdev->busdev_u.i2c.address = MS5611_I2C_ADDR; + defaultAddressApplied = true; + } + + if (!busRawReadRegisterBuffer(busdev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) { + goto fail; + } + + ms5611Reset(busdev); + + // read all coefficients + for (i = 0; i < PROM_NB; i++) + ms5611_c[i] = ms5611Prom(busdev, i); + + // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line! + if (ms5611CRC(ms5611_c) != 0) { + goto fail; + } + + // TODO prom + CRC + baro->ut_delay = 10000; + baro->up_delay = 10000; + baro->start_ut = ms5611StartUT; + baro->read_ut = ms5611ReadUT; + baro->get_ut = ms5611GetUT; + baro->start_up = ms5611StartUP; + baro->read_up = ms5611ReadUP; + baro->get_up = ms5611GetUP; + baro->calculate = ms5611Calculate; + + return true; + +fail:; + ms5611BusDeinit(busdev); + + if (defaultAddressApplied) { + busdev->busdev_u.i2c.address = 0; + } + + return false; +} #endif diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index 45ec936832..d0ef7634c5 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -27,6 +27,21 @@ #include "drivers/bus_i2c_busdev.h" #include "drivers/bus_spi.h" +bool busRawWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) +{ +#ifdef USE_SPI + if (busdev->bustype == BUSTYPE_SPI) { +#ifdef USE_SPI_TRANSACTION + spiBusTransactionSetup(busdev); +#endif + return spiBusWriteRegister(busdev, reg, data); + } else +#endif + { + return busWriteRegister(busdev, reg, data); + } +} + bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) { #if !defined(USE_SPI) && !defined(USE_I2C) @@ -52,6 +67,21 @@ bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data) } } +bool busRawWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) +{ +#ifdef USE_SPI + if (busdev->bustype == BUSTYPE_SPI) { +#ifdef USE_SPI_TRANSACTION + spiBusTransactionSetup(busdev); +#endif + return spiBusWriteRegister(busdev, reg, data); + } else +#endif + { + return busWriteRegisterStart(busdev, reg, data); + } +} + bool busWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) { #if !defined(USE_SPI) && !defined(USE_I2C) @@ -77,6 +107,21 @@ bool busWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data) } } +bool busRawReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +{ +#ifdef USE_SPI + if (busdev->bustype == BUSTYPE_SPI) { +#ifdef USE_SPI_TRANSACTION + spiBusTransactionSetup(bus); +#endif + return spiBusRawReadRegisterBuffer(busdev, reg, data, length); + } else +#endif + { + return busReadRegisterBuffer(busdev, reg, data, length); + } +} + bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) { #if !defined(USE_SPI) && !defined(USE_I2C) @@ -103,6 +148,21 @@ bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data } } +bool busRawReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) +{ +#ifdef USE_SPI + if (busdev->bustype == BUSTYPE_SPI) { +#ifdef USE_SPI_TRANSACTION + spiBusTransactionSetup(bus); +#endif + return spiBusRawReadRegisterBuffer(busdev, reg, data, length); + } else +#endif + { + return busReadRegisterBufferStart(busdev, reg, data, length); + } +} + // Start the I2C read, but do not wait for completion bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length) { diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 483d94d7ba..dee7c0ab63 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -65,9 +65,13 @@ typedef struct busDevice_s { void targetBusInit(void); #endif +bool busRawWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); bool busWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool busRawWriteRegisterStart(const busDevice_t *bus, uint8_t reg, uint8_t data); bool busWriteRegisterStart(const busDevice_t *bus, uint8_t reg, uint8_t data); +bool busRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); bool busReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length); uint8_t busReadRegister(const busDevice_t *bus, uint8_t reg); +bool busRawReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length); bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length); bool busBusy(const busDevice_t *busdev, bool *error); diff --git a/src/test/unit/baro_ms5611_unittest.cc b/src/test/unit/baro_ms5611_unittest.cc index cdc2e175a0..0ce4dcdc58 100644 --- a/src/test/unit/baro_ms5611_unittest.cc +++ b/src/test/unit/baro_ms5611_unittest.cc @@ -147,10 +147,10 @@ void delay(uint32_t) {} void delayMicroseconds(uint32_t) {} bool busBusy(const busDevice_t*, bool*) {return false;} -bool busReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busReadRegisterBufferStart(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} -bool busWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} -bool busWriteRegisterStart(const busDevice_t*, uint8_t, uint8_t) {return true;} +bool busRawReadRegisterBuffer(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busRawReadRegisterBufferStart(const busDevice_t*, uint8_t, uint8_t*, uint8_t) {return true;} +bool busRawWriteRegister(const busDevice_t*, uint8_t, uint8_t) {return true;} +bool busRawWriteRegisterStart(const busDevice_t*, uint8_t, uint8_t) {return true;} void spiBusSetDivisor() { }