From b78a8bd51943f64674dde5346f51172639befa1c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 1 Jul 2017 17:26:45 +0100 Subject: [PATCH] Improved efficiency of gyro read for SPI --- .../Inc/stm32f7xx_hal_spi.h | 4 ++-- .../Src/stm32f7xx_hal_spi.c | 4 ++-- src/main/drivers/accgyro/accgyro_mpu.c | 18 ++++++++++++++++++ src/main/drivers/accgyro/accgyro_mpu.h | 1 + src/main/drivers/accgyro/accgyro_spi_bmi160.c | 2 +- .../drivers/accgyro/accgyro_spi_icm20689.c | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu6000.c | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu6500.c | 2 +- src/main/drivers/accgyro/accgyro_spi_mpu9250.c | 2 +- src/main/drivers/bus_spi.c | 8 ++++++++ src/main/drivers/bus_spi.h | 3 +++ src/main/drivers/bus_spi_hal.c | 14 +++++++++++++- 12 files changed, 52 insertions(+), 10 deletions(-) diff --git a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h index efe31c6782..4cbc10ec7c 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h +++ b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_spi.h @@ -622,9 +622,9 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi); * @{ */ /* I/O operation functions ***************************************************/ -HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, const uint8_t *pData, uint16_t Size, uint32_t Timeout); HAL_StatusTypeDef HAL_SPI_Receive(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout); -HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, +HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, const uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, uint32_t Timeout); HAL_StatusTypeDef HAL_SPI_Transmit_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); HAL_StatusTypeDef HAL_SPI_Receive_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c index 18a6ce9d72..e868490033 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal_spi.c @@ -493,7 +493,7 @@ __weak void HAL_SPI_MspDeInit(SPI_HandleTypeDef *hspi) * @param Timeout: Timeout duration * @retval HAL status */ -HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout) +HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, const uint8_t *pData, uint16_t Size, uint32_t Timeout) { uint32_t tickstart = 0U; HAL_StatusTypeDef errorcode = HAL_OK; @@ -882,7 +882,7 @@ error : * @param Timeout: Timeout duration * @retval HAL status */ -HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, +HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, const uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, uint32_t Timeout) { uint32_t tmp = 0U, tmp1 = 0U; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 971deffc05..1ca97c798a 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -29,6 +29,7 @@ #include "common/maths.h" #include "common/utils.h" +#include "drivers/bus_spi.h" #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" @@ -217,6 +218,23 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } +bool mpuGyroReadSPI(gyroDev_t *gyro) +{ + static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + uint8_t data[7]; + + const bool ack = spiBusTransfer(&gyro->bus, data, dataToSend, 7); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]); + gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]); + gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]); + + return true; +} + bool mpuCheckDataReady(gyroDev_t* gyro) { bool ret; diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 2da4ce1097..445af4dfe0 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -199,6 +199,7 @@ void mpuGyroInit(struct gyroDev_s *gyro); struct accDev_s; bool mpuAccRead(struct accDev_s *acc); bool mpuGyroRead(struct gyroDev_s *gyro); +bool mpuGyroReadSPI(struct gyroDev_s *gyro); void mpuDetect(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro); void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn); diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index ee3077421c..31edef7c34 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -351,7 +351,7 @@ bool bmi160GyroRead(gyroDev_t *gyro) }; uint8_t bmi160_rec_buf[BUFFER_SIZE]; - uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; + static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; IOLo(gyro->bus.spi.csnPin); spiTransfer(gyro->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 762ac1a61b..438b67b05e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -140,7 +140,7 @@ bool icm20689SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = icm20689GyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 94a9e2e561..2413174b7e 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -244,7 +244,7 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = mpu6000SpiGyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 8f2f4db210..eae203e653 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -136,7 +136,7 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = mpu6500SpiGyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 1df2200467..000d18a6f9 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -210,7 +210,7 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro) } gyro->initFn = mpu9250SpiGyroInit; - gyro->readFn = mpuGyroRead; + gyro->readFn = mpuGyroReadSPI; gyro->intStatusFn = mpuCheckDataReady; // 16.4 dps/lsb scalefactor diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 009a424bc1..6be78dc11c 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -240,6 +240,14 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len return true; } +bool spiBusTransfer(const busDevice_t *bus, uint8_t *rxData, const uint8_t *txData, int length) +{ + IOLo(bus->spi.csnPin); + spiTransfer(bus->spi.instance, rxData, txData, length); + IOHi(bus->spi.csnPin); + return true; +} + void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { #define BR_CLEAR_MASK 0xFFC7 diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 3f3a507044..799ffe7450 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,6 +17,7 @@ #pragma once +#include "drivers/bus.h" #include "drivers/io_types.h" #include "drivers/bus.h" #include "drivers/rcc_types.h" @@ -93,6 +94,8 @@ uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); +bool spiBusTransfer(const busDevice_t *bus, uint8_t *rxData, const uint8_t *txData, int length); + #if defined(USE_HAL_DRIVER) SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 356fc5709c..ae25ccdfab 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -23,6 +23,7 @@ #ifdef USE_SPI +#include "drivers/bus.h" #include "drivers/bus_spi.h" #include "drivers/bus_spi_impl.h" #include "drivers/dma.h" @@ -275,7 +276,7 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len } else // Tx and Rx { - status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT); + status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, in, out, len, SPI_DEFAULT_TIMEOUT); } if( status != HAL_OK) @@ -310,6 +311,17 @@ static uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t in) return in; } +bool spiBusTransfer(const busDevice_t *bus, uint8_t *rxData, const uint8_t *txData, int len) +{ + IOLo(bus->spi.csnPin); + const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(bus->spi.handle, txData, rxData, len, SPI_DEFAULT_TIMEOUT); + IOHi(bus->spi.csnPin); + if (status != HAL_OK) { + spiTimeoutUserCallback(bus->spi.instance); + } + return true; +} + void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { SPIDevice device = spiDeviceByInstance(instance);