From 0649c7ccc70f6e1f2d06a48df5ee0162a7cacfd5 Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Wed, 16 May 2018 10:05:28 -0700 Subject: [PATCH 1/6] pios_bmp280: prevent i2c/spi r/w peeking at globals --- flight/PiOS/Common/pios_bmp280.c | 55 ++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 24 deletions(-) diff --git a/flight/PiOS/Common/pios_bmp280.c b/flight/PiOS/Common/pios_bmp280.c index 92ab8547d1..ecfc3efcf0 100644 --- a/flight/PiOS/Common/pios_bmp280.c +++ b/flight/PiOS/Common/pios_bmp280.c @@ -349,7 +349,8 @@ static int32_t PIOS_BMP280_ReadADC() } #ifndef PIOS_EXCLUDE_BMP280_I2C -static int32_t PIOS_BMP280_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) +static int32_t PIOS_BMP280_I2C_Read(pios_i2c_t i2c_id, + uint8_t address, uint8_t *buffer, uint8_t len) { const struct pios_i2c_txn txn_list[] = { { @@ -369,10 +370,11 @@ static int32_t PIOS_BMP280_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t le } }; - return PIOS_I2C_Transfer(dev->i2c_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } -static int32_t PIOS_BMP280_I2C_Write(uint8_t *buffer, uint8_t len) { +static int32_t PIOS_BMP280_I2C_Write(pios_i2c_t i2c_id, + uint8_t *buffer, uint8_t len) { const struct pios_i2c_txn txn_list[] = { { .info = __func__, @@ -384,52 +386,54 @@ static int32_t PIOS_BMP280_I2C_Write(uint8_t *buffer, uint8_t len) { , }; - return PIOS_I2C_Transfer(dev->i2c_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } #endif /* PIOS_EXCLUDE_BMP280_I2C */ #ifdef PIOS_INCLUDE_BMP280_SPI -static int32_t PIOS_BMP280_ClaimBus(struct bmp280_dev *bmp_dev) +static int32_t PIOS_BMP280_ClaimBus(pios_spi_t spi_id, uint32_t spi_slave) { - if (PIOS_SPI_ClaimBus(bmp_dev->spi_id) != 0) + if (PIOS_SPI_ClaimBus(spi_id) != 0) return -2; PIOS_DELAY_WaituS(1); - PIOS_SPI_RC_PinSet(bmp_dev->spi_id, bmp_dev->spi_slave, false); + PIOS_SPI_RC_PinSet(spi_id, spi_slave, false); PIOS_DELAY_WaituS(1); - PIOS_SPI_SetClockSpeed(bmp_dev->spi_id, PIOS_BMP_SPI_SPEED); + PIOS_SPI_SetClockSpeed(spi_id, PIOS_BMP_SPI_SPEED); return 0; } -static void PIOS_BMP280_ReleaseBus(struct bmp280_dev *bmp_dev) +static void PIOS_BMP280_ReleaseBus(pios_spi_t spi_id, uint32_t spi_slave) { - PIOS_SPI_RC_PinSet(bmp_dev->spi_id, bmp_dev->spi_slave, true); + PIOS_SPI_RC_PinSet(spi_id, spi_slave, true); - PIOS_SPI_ReleaseBus(bmp_dev->spi_id); + PIOS_SPI_ReleaseBus(spi_id); } -static int32_t PIOS_BMP280_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) { - if (PIOS_BMP280_ClaimBus(dev) != 0) +static int32_t PIOS_BMP280_SPI_Read(pios_spi_t spi_id, uint32_t spi_slave, + uint8_t address, uint8_t *buffer, uint8_t len) { + if (PIOS_BMP280_ClaimBus(spi_id, spi_slave) != 0) return -1; - PIOS_SPI_TransferByte(dev->spi_id, 0x80 | address); + PIOS_SPI_TransferByte(spi_id, 0x80 | address); - int ret = PIOS_SPI_TransferBlock(dev->spi_id, NULL, buffer, len); + int ret = PIOS_SPI_TransferBlock(spi_id, NULL, buffer, len); - PIOS_BMP280_ReleaseBus(dev); + PIOS_BMP280_ReleaseBus(spi_id, spi_slave); return ret; } -static int32_t PIOS_BMP280_SPI_Write(uint8_t *buffer, uint8_t len) { - if (PIOS_BMP280_ClaimBus(dev) != 0) +static int32_t PIOS_BMP280_SPI_Write(pios_spi_t spi_id, uint32_t spi_slave, + uint8_t *buffer, uint8_t len) { + if (PIOS_BMP280_ClaimBus(spi_id, spi_slave) != 0) return -1; - int ret = PIOS_SPI_TransferBlock(dev->spi_id, buffer, NULL, len); + int ret = PIOS_SPI_TransferBlock(spi_id, buffer, NULL, len); - PIOS_BMP280_ReleaseBus(dev); + PIOS_BMP280_ReleaseBus(spi_id, spi_slave); return ret; } @@ -450,11 +454,12 @@ static int32_t PIOS_BMP280_Read(uint8_t address, uint8_t *buffer, uint8_t len) if (0) { #ifndef PIOS_EXCLUDE_BMP280_I2C } else if (dev->i2c_id) { - return PIOS_BMP280_I2C_Read(address, buffer, len); + return PIOS_BMP280_I2C_Read(dev->i2c_id, address, buffer, len); #endif #ifdef PIOS_INCLUDE_BMP280_SPI } else if (dev->spi_id) { - return PIOS_BMP280_SPI_Read(address, buffer, len); + return PIOS_BMP280_SPI_Read(dev->spi_id, dev->spi_slave, + address, buffer, len); #endif } @@ -481,12 +486,14 @@ static int32_t PIOS_BMP280_WriteCommand(uint8_t address, uint8_t buffer) if (0) { #ifndef PIOS_EXCLUDE_BMP280_I2C } else if (dev->i2c_id) { - return PIOS_BMP280_I2C_Write(data, sizeof(data)); + return PIOS_BMP280_I2C_Write(dev->i2c_id, + data, sizeof(data)); #endif #ifdef PIOS_INCLUDE_BMP280_SPI } else if (dev->spi_id) { data[0] &= 0x7f; /* Clear high bit */ - return PIOS_BMP280_SPI_Write(data, sizeof(data)); + return PIOS_BMP280_SPI_Write(dev->spi_id, dev->spi_slave, + data, sizeof(data)); #endif } From 58e5ffceed89f24c9ab3b096b5ef7e46034c0c5d Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Wed, 16 May 2018 10:06:23 -0700 Subject: [PATCH 2/6] pios_bmp280: formatting --- flight/PiOS/Common/pios_bmp280.c | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/flight/PiOS/Common/pios_bmp280.c b/flight/PiOS/Common/pios_bmp280.c index ecfc3efcf0..cadf0fbe15 100644 --- a/flight/PiOS/Common/pios_bmp280.c +++ b/flight/PiOS/Common/pios_bmp280.c @@ -359,15 +359,14 @@ static int32_t PIOS_BMP280_I2C_Read(pios_i2c_t i2c_id, .rw = PIOS_I2C_TXN_WRITE, .len = 1, .buf = &address, - } - , + }, { - .info = __func__, - .addr = BMP280_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } + .info = __func__, + .addr = BMP280_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } }; return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); @@ -377,13 +376,12 @@ static int32_t PIOS_BMP280_I2C_Write(pios_i2c_t i2c_id, uint8_t *buffer, uint8_t len) { const struct pios_i2c_txn txn_list[] = { { - .info = __func__, - .addr = BMP280_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = len, - .buf = buffer, - } - , + .info = __func__, + .addr = BMP280_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = len, + .buf = buffer, + } }; return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); From c64f31d262a883cf9ba9bd8343195e7344167f64 Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Wed, 16 May 2018 10:18:22 -0700 Subject: [PATCH 3/6] pios_bmp280: probe without allocating structures --- flight/PiOS/Common/pios_bmp280.c | 140 ++++++++++++++++++++----------- 1 file changed, 93 insertions(+), 47 deletions(-) diff --git a/flight/PiOS/Common/pios_bmp280.c b/flight/PiOS/Common/pios_bmp280.c index cadf0fbe15..980095b479 100644 --- a/flight/PiOS/Common/pios_bmp280.c +++ b/flight/PiOS/Common/pios_bmp280.c @@ -154,15 +154,6 @@ static int32_t PIOS_BMP280_Common_Init(const struct pios_bmp280_cfg *cfg) dev->cfg = cfg; uint8_t data[24]; - uint16_t ref_digT1; - if (PIOS_BMP280_Read(BMP280_CAL_ADDR, data, 2) != 0) - return -2; - - /* Ignore first result */ - if (PIOS_BMP280_Read(BMP280_CAL_ADDR, data, 2) != 0) - return -2; - - ref_digT1 = (data[ 1] << 8) | data[ 0]; if (PIOS_BMP280_Read(BMP280_CAL_ADDR, data, 24) != 0) return -2; @@ -180,13 +171,8 @@ static int32_t PIOS_BMP280_Common_Init(const struct pios_bmp280_cfg *cfg) dev->digP8 = (data[21] << 8) | data[20]; dev->digP9 = (data[23] << 8) | data[22]; - if (ref_digT1 != dev->digT1) { - /* Values inconsistent, so no good device detect */ - return -10; - } - - if ((ref_digT1 == dev->digT2) && - (ref_digT1 == dev->digP1)) { + if ((dev->digT1 == dev->digT2) && + (dev->digT1 == dev->digP1)) { /* Values invariant, so no good device detect */ return -11; } @@ -199,37 +185,6 @@ static int32_t PIOS_BMP280_Common_Init(const struct pios_bmp280_cfg *cfg) return 0; } -/** - * Initialise the BMP280 sensor - */ -#ifndef PIOS_EXCLUDE_BMP280_I2C -int32_t PIOS_BMP280_Init(const struct pios_bmp280_cfg *cfg, pios_i2c_t i2c_device) -{ - dev = (struct bmp280_dev *) PIOS_BMP280_alloc(); - if (dev == NULL) - return -1; - - dev->i2c_id = i2c_device; - - return PIOS_BMP280_Common_Init(cfg); -} -#endif /* PIOS_EXCLUDE_BMP280_I2C */ - -#ifdef PIOS_INCLUDE_BMP280_SPI -int32_t PIOS_BMP280_SPI_Init(const struct pios_bmp280_cfg *cfg, pios_spi_t spi_device, - uint32_t spi_slave) -{ - dev = (struct bmp280_dev *) PIOS_BMP280_alloc(); - if (dev == NULL) - return -1; - - dev->spi_id = spi_device; - dev->spi_slave = spi_slave; - - return PIOS_BMP280_Common_Init(cfg); -} -#endif /* PIOS_INCLUDE_BMP280_SPI */ - /** * Claim the BMP280 device semaphore. * \return 0 if no error @@ -348,6 +303,28 @@ static int32_t PIOS_BMP280_ReadADC() return 0; } +static inline int32_t PIOS_BMP280_CheckData(const uint8_t *data, + const uint8_t *data_b) +{ + int i; + + /* Verify we got consistent results */ + for (i = 0; i < 6; i++) { + if (data[i] != data_b[i]) { + return -3; + } + } + + /* Verify the probed data varies */ + for (i = 1; i < 6; i++) { + if (data[i] != data[0]) { + return 0; + } + } + + return -4; +} + #ifndef PIOS_EXCLUDE_BMP280_I2C static int32_t PIOS_BMP280_I2C_Read(pios_i2c_t i2c_id, uint8_t address, uint8_t *buffer, uint8_t len) @@ -386,6 +363,39 @@ static int32_t PIOS_BMP280_I2C_Write(pios_i2c_t i2c_id, return PIOS_I2C_Transfer(i2c_id, txn_list, NELEMENTS(txn_list)); } + +/** + * Initialise the BMP280 sensor + */ +int32_t PIOS_BMP280_Init(const struct pios_bmp280_cfg *cfg, pios_i2c_t i2c_device) +{ + uint8_t data[6]; + uint8_t data_b[6]; + + /* Ignore first result */ + if (PIOS_BMP280_I2C_Read(i2c_device, BMP280_CAL_ADDR, data, 6) != 0) + return -2; + + if (PIOS_BMP280_I2C_Read(i2c_device, BMP280_CAL_ADDR, data, 6) != 0) + return -2; + + if (PIOS_BMP280_I2C_Read(i2c_device, BMP280_CAL_ADDR, data_b, 6) != 0) + return -2; + + int ret = PIOS_BMP280_CheckData(data, data_b); + + if (ret) { + return ret; + } + + dev = (struct bmp280_dev *) PIOS_BMP280_alloc(); + if (dev == NULL) + return -1; + + dev->i2c_id = i2c_device; + + return PIOS_BMP280_Common_Init(cfg); +} #endif /* PIOS_EXCLUDE_BMP280_I2C */ #ifdef PIOS_INCLUDE_BMP280_SPI @@ -435,6 +445,42 @@ static int32_t PIOS_BMP280_SPI_Write(pios_spi_t spi_id, uint32_t spi_slave, return ret; } + +int32_t PIOS_BMP280_SPI_Init(const struct pios_bmp280_cfg *cfg, pios_spi_t spi_device, + uint32_t spi_slave) +{ + uint8_t data[6]; + uint8_t data_b[6]; + + /* Ignore first result */ + if (PIOS_BMP280_SPI_Read(spi_device, spi_slave, + BMP280_CAL_ADDR, data, 6) != 0) + return -2; + + if (PIOS_BMP280_SPI_Read(spi_device, spi_slave, + BMP280_CAL_ADDR, data, 6) != 0) + return -2; + + if (PIOS_BMP280_SPI_Read(spi_device, spi_slave, + BMP280_CAL_ADDR, data_b, 6) != 0) + return -2; + + int ret = PIOS_BMP280_CheckData(data, data_b); + + if (ret) { + return ret; + } + + dev = (struct bmp280_dev *) PIOS_BMP280_alloc(); + if (dev == NULL) + return -1; + + dev->spi_id = spi_device; + dev->spi_slave = spi_slave; + + return PIOS_BMP280_Common_Init(cfg); +} + #endif /* PIOS_INCLUDE_BMP280_SPI */ /* From c1c66f2e5a29caaec10ed8cc99cf0da26fb72dab Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Wed, 16 May 2018 10:28:48 -0700 Subject: [PATCH 4/6] pios_i2c: check bus clear before completion of init --- flight/PiOS/STM32/pios_i2c.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/flight/PiOS/STM32/pios_i2c.c b/flight/PiOS/STM32/pios_i2c.c index 1266e861ed..2b7cd32496 100644 --- a/flight/PiOS/STM32/pios_i2c.c +++ b/flight/PiOS/STM32/pios_i2c.c @@ -277,6 +277,13 @@ int32_t PIOS_I2C_Init(pios_i2c_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg /* Initialize the state machine */ i2c_adapter_fsm_init(i2c_adapter); + if (PIOS_I2C_CheckClear(i2c_adapter)) { + /* Check for sanity before returning adapter and + * initing interrupts + */ + goto out_fail; + } + *i2c_id = i2c_adapter; /* Configure and enable I2C interrupts */ From ee490abfac58ea8bb827aa2e2d134f08dbeeb7b9 Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Wed, 16 May 2018 10:29:06 -0700 Subject: [PATCH 5/6] revolution: simplify i2c init / clone logic --- flight/targets/revolution/fw/pios_board.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/flight/targets/revolution/fw/pios_board.c b/flight/targets/revolution/fw/pios_board.c index 68c2210ced..6a3de02a5c 100644 --- a/flight/targets/revolution/fw/pios_board.c +++ b/flight/targets/revolution/fw/pios_board.c @@ -235,14 +235,11 @@ void PIOS_Board_Init(void) { /* Configure IO ports */ #if defined(PIOS_INCLUDE_I2C) - if ((!is_modified_clone) && PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { - PIOS_HAL_CriticalError(PIOS_LED_HEARTBEAT, PIOS_HAL_PANIC_I2C_INT); - } - - if ((!is_modified_clone) && - (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0)) { - PIOS_HAL_CriticalError(PIOS_LED_HEARTBEAT, PIOS_HAL_PANIC_I2C_INT); - } else if (!is_modified_clone) { + if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { + if (!is_modified_clone) { + PIOS_HAL_CriticalError(PIOS_LED_HEARTBEAT, PIOS_HAL_PANIC_I2C_INT); + } + } else { if (AlarmsGet(SYSTEMALARMS_ALARM_I2C) == SYSTEMALARMS_ALARM_UNINITIALISED) { AlarmsSet(SYSTEMALARMS_ALARM_I2C, SYSTEMALARMS_ALARM_OK); } From a3f59e449ad26d07034d239a65b2d5fbe2bf7312 Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Wed, 16 May 2018 10:32:39 -0700 Subject: [PATCH 6/6] revolution: probe bmp280 on clones --- flight/targets/revolution/fw/pios_board.c | 10 ++++++++++ flight/targets/revolution/fw/pios_config.h | 1 + 2 files changed, 11 insertions(+) diff --git a/flight/targets/revolution/fw/pios_board.c b/flight/targets/revolution/fw/pios_board.c index 6a3de02a5c..c6e66ba368 100644 --- a/flight/targets/revolution/fw/pios_board.c +++ b/flight/targets/revolution/fw/pios_board.c @@ -587,6 +587,16 @@ void PIOS_Board_Init(void) { //I2C is slow, sensor init as well, reset watchdog to prevent reset here PIOS_WDG_Clear(); +#if defined(PIOS_INCLUDE_BMP280) && !defined(PIOS_EXCLUDE_BMP280_I2C) + /* BMP280 I2C onboard support on clones */ + if (is_modified_clone && + (pios_i2c_mag_pressure_adapter_id) && + (!PIOS_SENSORS_IsRegistered(PIOS_SENSOR_BARO))) { + PIOS_BMP280_Init(&pios_bmp280_cfg, + pios_i2c_mag_pressure_adapter_id); + } +#endif /* defined(PIOS_INCLUDE_BMP280) && !defined(PIOS_EXCLUDE_BMP280_I2C) */ + #endif /* PIOS_INCLUDE_I2C */ #ifdef PIOS_INCLUDE_SPI diff --git a/flight/targets/revolution/fw/pios_config.h b/flight/targets/revolution/fw/pios_config.h index c46fcce197..97d5e8f60e 100644 --- a/flight/targets/revolution/fw/pios_config.h +++ b/flight/targets/revolution/fw/pios_config.h @@ -57,6 +57,7 @@ #define PIOS_INCLUDE_MAX7456 /* Select the sensors to include */ +#define PIOS_INCLUDE_BMP280 #define PIOS_INCLUDE_BMP280_SPI #define PIOS_INCLUDE_HMC5883 #define PIOS_INCLUDE_HMC5983_I2C