Fix gyro/acc initialization when more than one MPU6000 (or MPU9250) s…

…ensor 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

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

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)