Improved efficiency of gyro read for SPI

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

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;

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;

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

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

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

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;

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

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

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

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

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