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.
This commit is contained in:
parent
2b79f2b1a4
commit
40d195aeae
|
@ -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)
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue