From 40d195aeae935aa188831f32044fcb6c3abac196 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 9 Apr 2019 10:58:04 -0400 Subject: [PATCH] Fix gyro/acc initialization when more than one MPU6000 (or MPU9250) sensor is connected Logic was not expecting there to be multiple MPU6000's connected simultaneously. There are currently no boards manufactured with this config, but it's possible to do using boards that have an onboard MPU6000 and an external gyro box containing a second MPU6000. --- src/main/drivers/accgyro/accgyro_spi_mpu6000.c | 7 ------- src/main/drivers/accgyro/accgyro_spi_mpu9250.c | 7 ------- 2 files changed, 14 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 65e353ef6..b16ab6f11 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -47,7 +47,6 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro); -static bool mpuSpi6000InitDone = false; // Bits @@ -171,10 +170,6 @@ uint8_t mpu6000SpiDetect(const busDevice_t *bus) static void mpu6000AccAndGyroInit(gyroDev_t *gyro) { - if (mpuSpi6000InitDone) { - return; - } - spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); // Device Reset @@ -218,8 +213,6 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_FAST); delayMicroseconds(1); - - mpuSpi6000InitDone = true; } bool mpu6000SpiAccDetect(accDev_t *acc) diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 798786ed5..e1c0b23c8 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -49,7 +49,6 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro); -static bool mpuSpi9250InitDone = false; bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { @@ -120,10 +119,6 @@ bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { - if (mpuSpi9250InitDone) { - return; - } - spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed for writing to slow registers mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); @@ -145,8 +140,6 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { #endif spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_FAST); - - mpuSpi9250InitDone = true; //init done } uint8_t mpu9250SpiDetect(const busDevice_t *bus)