MPU6500 conversion

src/main/drivers/accgyro/accgyro_mpu.c

@@ -248,14 +248,15 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
 #endif
 
 #ifdef USE_GYRO_SPI_MPU6500
+    gyro->bus.spi.instance = MPU6500_SPI_INSTANCE;
     gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
     const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus);
     // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
     if (mpu6500Sensor != MPU_NONE) {
         gyro->mpuDetectionResult.sensor = mpu6500Sensor;
         gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
-        gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
+        gyro->mpuConfiguration.readFn = spiReadRegister;
-        gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
+        gyro->mpuConfiguration.writeFn = spiWriteRegister;
         return true;
     }
 #endif

src/main/drivers/accgyro/accgyro_spi_mpu6500.c

@@ -34,39 +34,8 @@
 #include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6500.h"
 
-#define DISABLE_MPU6500(spiCsnPin)       IOHi(spiCsnPin)
-#define ENABLE_MPU6500(spiCsnPin)        IOLo(spiCsnPin)
-
 #define BIT_SLEEP                   0x40
 
-bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    ENABLE_MPU6500(bus->spi.csnPin);
-    spiTransferByte(MPU6500_SPI_INSTANCE, reg);
-    spiTransferByte(MPU6500_SPI_INSTANCE, data);
-    DISABLE_MPU6500(bus->spi.csnPin);
-
-    return true;
-}
-
-bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
-{
-    mpu6500SpiWriteRegister(bus, reg, data);
-    delayMicroseconds(10);
-
-    return true;
-}
-
-bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
-{
-    ENABLE_MPU6500(bus->spi.csnPin);
-    spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
-    spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
-    DISABLE_MPU6500(bus->spi.csnPin);
-
-    return true;
-}
-
 static void mpu6500SpiInit(const busDevice_t *bus)
 {
     static bool hardwareInitialised = false;
@@ -79,7 +48,7 @@ static void mpu6500SpiInit(const busDevice_t *bus)
     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
     IOHi(bus->spi.csnPin);
 
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
 
     hardwareInitialised = true;
 }
@@ -89,7 +58,7 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus)
     mpu6500SpiInit(bus);
 
     uint8_t tmp;
-    mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
+    spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
 
     uint8_t mpuDetected = MPU_NONE;
     switch (tmp) {
@@ -122,16 +91,16 @@ void mpu6500SpiAccInit(accDev_t *acc)
 
 void mpu6500SpiGyroInit(gyroDev_t *gyro)
 {
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_SLOW);
     delayMicroseconds(1);
 
     mpu6500GyroInit(gyro);
 
     // Disable Primary I2C Interface
-    mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
+    spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
     delay(100);
 
-    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
+    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
     delayMicroseconds(1);
 }
 

src/main/drivers/accgyro/accgyro_spi_mpu6500.h

@@ -24,9 +24,5 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus);
 bool mpu6500SpiAccDetect(accDev_t *acc);
 bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
 
-bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data);
-bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
-
 void mpu6500SpiGyroInit(gyroDev_t *gyro);
 void mpu6500SpiAccInit(accDev_t *acc);

src/main/drivers/compass/compass_ak8963.c

@@ -30,6 +30,7 @@
 
 #include "drivers/bus_i2c.h"
 #include "drivers/bus_spi.h"
+#include "drivers/io.h"
 #include "drivers/sensor.h"
 #include "drivers/time.h"
 
@@ -109,6 +110,27 @@ typedef enum {
 
 static queuedReadState_t queuedRead = { false, 0, 0};
 
+static bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
+{
+    IOLo(bus->spi.csnPin);
+    spiTransferByte(MPU6500_SPI_INSTANCE, reg);
+    spiTransferByte(MPU6500_SPI_INSTANCE, data);
+    IOHi(bus->spi.csnPin);
+    delayMicroseconds(10);
+
+    return true;
+}
+
+static bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
+{
+    IOLo(bus->spi.csnPin);
+    spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
+    spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
+    IOHi(bus->spi.csnPin);
+
+    return true;
+}
+
 static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
 {
     verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG);   // set I2C slave address for read