From bee2bacb667478e37a7ceed5d305d0299f2c3991 Mon Sep 17 00:00:00 2001 From: Andrey Mironov Date: Fri, 20 Apr 2018 00:13:28 +0300 Subject: [PATCH] Fixed ownership of MPU_CS resource in case of multiple gyros (#5715) --- src/main/drivers/accgyro/accgyro_l3gd20.c | 2 ++ src/main/drivers/accgyro/accgyro_spi_bmi160.c | 2 ++ src/main/drivers/accgyro/accgyro_spi_icm20649.c | 2 ++ src/main/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 | 3 +++ src/main/sensors/gyro.c | 4 ++-- 8 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_l3gd20.c b/src/main/drivers/accgyro/accgyro_l3gd20.c index c4d84ad34d..f9e824a7f2 100644 --- a/src/main/drivers/accgyro/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro/accgyro_l3gd20.c @@ -79,10 +79,12 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) UNUSED(SPIx); // FIXME mpul3gd20CsPin = IOGetByTag(IO_TAG(L3GD20_CS_PIN)); +#ifndef USE_DUAL_GYRO IOInit(mpul3gd20CsPin, OWNER_MPU_CS, 0); IOConfigGPIO(mpul3gd20CsPin, SPI_IO_CS_CFG); DISABLE_L3GD20; +#endif spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD); } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 3cfe653317..643cfad57f 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -97,9 +97,11 @@ uint8_t bmi160Detect(const busDevice_t *bus) return BMI_160_SPI; } +#ifndef USE_DUAL_GYRO IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->busdev_u.spi.csnPin); +#endif spiSetDivisor(bus->busdev_u.spi.instance, BMI160_SPI_DIVISOR); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index 0e5ee50e18..4ceee78c2c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -42,9 +42,11 @@ static void icm20649SpiInit(const busDevice_t *bus) return; } +#ifndef USE_DUAL_GYRO IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->busdev_u.spi.csnPin); +#endif // all registers can be read/written at full speed (7MHz +-10%) // TODO verify that this works at 9MHz and 10MHz on non F7 diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 71056d1f9a..659c337851 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -42,9 +42,11 @@ static void icm20689SpiInit(const busDevice_t *bus) return; } +#ifndef USE_DUAL_GYRO IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->busdev_u.spi.csnPin); +#endif spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index aab29f4b25..c57d593127 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -125,9 +125,11 @@ void mpu6000SpiAccInit(accDev_t *acc) uint8_t mpu6000SpiDetect(const busDevice_t *bus) { +#ifndef USE_DUAL_GYRO IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->busdev_u.spi.csnPin); +#endif spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index db7b4da3a5..dbdd633c68 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -38,9 +38,11 @@ static void mpu6500SpiInit(const busDevice_t *bus) { +#ifndef USE_DUAL_GYRO IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); IOHi(bus->busdev_u.spi.csnPin); +#endif spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_FAST); } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 61c698465c..0af5650d31 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -162,8 +162,11 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { uint8_t mpu9250SpiDetect(const busDevice_t *bus) { +#ifndef USE_DUAL_GYRO IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); +#endif spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2d450b0e67..3d6371960f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -493,7 +493,7 @@ bool gyroInit(void) #if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN) if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_1_CS_PIN)); - IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(0)); IOHi(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. IOConfigGPIO(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); } @@ -502,7 +502,7 @@ bool gyroInit(void) #if defined(USE_DUAL_GYRO) && defined(GYRO_2_CS_PIN) if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_2_CS_PIN)); - IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, 1); + IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(1)); IOHi(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus. IOConfigGPIO(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG); }