diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 7f5150c0b4..2323a095eb 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -240,6 +240,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI + uint8_t sensor = MPU_NONE; + UNUSED(sensor); + #ifdef USE_GYRO_SPI_MPU6000 #ifdef MPU6000_SPI_INSTANCE spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE); @@ -247,8 +250,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #ifdef MPU6000_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin; #endif - if (mpu6000SpiDetect(&gyro->bus)) { - gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; + sensor = mpu6000SpiDetect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; @@ -263,10 +267,10 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #ifdef MPU6500_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin; #endif - const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus); + sensor = 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; + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; @@ -281,8 +285,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #ifdef MPU9250_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin; #endif - if (mpu9250SpiDetect(&gyro->bus)) { - gyro->mpuDetectionResult.sensor = MPU_9250_SPI; + sensor = mpu9250SpiDetect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; @@ -298,8 +303,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #ifdef ICM20689_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin; #endif - if (icm20689SpiDetect(&gyro->bus)) { - gyro->mpuDetectionResult.sensor = ICM_20689_SPI; + sensor = icm20689SpiDetect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; @@ -314,8 +320,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) #ifdef BMI160_CS_PIN gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin; #endif - if (bmi160Detect(&gyro->bus)) { - gyro->mpuDetectionResult.sensor = BMI_160_SPI; + sensor = bmi160Detect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; gyro->mpuConfiguration.readFn = spiReadRegisterBuffer; gyro->mpuConfiguration.writeFn = spiWriteRegister; return true; diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index d6d65a15c1..9f90b536fd 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -93,10 +93,10 @@ static int32_t BMI160_do_foc(const busDevice_t *bus); static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data); -bool bmi160Detect(const busDevice_t *bus) +uint8_t bmi160Detect(const busDevice_t *bus) { if (BMI160Detected) { - return true; + return BMI_160_SPI; } IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); @@ -111,11 +111,11 @@ bool bmi160Detect(const busDevice_t *bus) /* Check the chip ID */ if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) { - return false; + return MPU_NONE; } BMI160Detected = true; - return true; + return BMI_160_SPI; } @@ -381,7 +381,7 @@ void bmi160SpiAccInit(accDev_t *acc) bool bmi160SpiAccDetect(accDev_t *acc) { - if (!bmi160Detect(acc->bus.spi.csnPin)) { + if (bmi160Detect(acc->bus.spi.csnPin) == MPU_NONE) { return false; } @@ -394,7 +394,7 @@ bool bmi160SpiAccDetect(accDev_t *acc) bool bmi160SpiGyroDetect(gyroDev_t *gyro) { - if (!bmi160Detect(gyro->bus.spi.csnPin)) { + if (bmi160Detect(gyro->bus.spi.csnPin) == MPU_NONE) { return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.h b/src/main/drivers/accgyro/accgyro_spi_bmi160.h index ad5c6e1e24..d1a9ea6601 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.h +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.h @@ -68,6 +68,6 @@ enum bmi160_gyro_range { BMI160_RANGE_2000DPS = 0x00, }; -bool bmi160Detect(const busDevice_t *bus); +uint8_t bmi160Detect(const busDevice_t *bus); bool bmi160SpiAccDetect(accDev_t *acc); bool bmi160SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 2f2c80cf10..a6ef57f9ab 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -53,7 +53,7 @@ static void icm20689SpiInit(const busDevice_t *bus) hardwareInitialised = true; } -bool icm20689SpiDetect(const busDevice_t *bus) +uint8_t icm20689SpiDetect(const busDevice_t *bus) { icm20689SpiInit(bus); @@ -69,13 +69,13 @@ bool icm20689SpiDetect(const busDevice_t *bus) break; } if (!attemptsRemaining) { - return false; + return MPU_NONE; } } while (attemptsRemaining--); spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD); - return true; + return ICM_20689_SPI; } diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.h b/src/main/drivers/accgyro/accgyro_spi_icm20689.h index 164f51decf..57cc827f89 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.h @@ -27,7 +27,7 @@ bool icm20689GyroDetect(gyroDev_t *gyro); void icm20689AccInit(accDev_t *acc); void icm20689GyroInit(gyroDev_t *gyro); -bool icm20689SpiDetect(const busDevice_t *bus); +uint8_t icm20689SpiDetect(const busDevice_t *bus); bool icm20689SpiAccDetect(accDev_t *acc); bool icm20689SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index e23c5fe18c..a4976e02fb 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -125,7 +125,7 @@ void mpu6000SpiAccInit(accDev_t *acc) acc->acc_1G = 512 * 8; } -bool mpu6000SpiDetect(const busDevice_t *bus) +uint8_t mpu6000SpiDetect(const busDevice_t *bus) { IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); @@ -144,7 +144,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus) break; } if (!attemptsRemaining) { - return false; + return MPU_NONE; } } while (attemptsRemaining--); @@ -166,10 +166,10 @@ bool mpu6000SpiDetect(const busDevice_t *bus) case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: - return true; + return MPU_60x0_SPI; } - return false; + return MPU_NONE; } static void mpu6000AccAndGyroInit(gyroDev_t *gyro) diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h index 8435cebad0..7682a28726 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.h @@ -17,7 +17,7 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -bool mpu6000SpiDetect(const busDevice_t *bus); +uint8_t mpu6000SpiDetect(const busDevice_t *bus); bool mpu6000SpiAccDetect(accDev_t *acc); bool mpu6000SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 8df48ff8dd..b397c2bb7c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -96,8 +96,8 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro) mpuGyroRead(gyro); - if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) { - spiResetErrorCounter(MPU9250_SPI_INSTANCE); + if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(gyro->bus.spi.instance) != 0) { + spiResetErrorCounter(gyro->bus.spi.instance); failureMode(FAILURE_GYRO_INIT_FAILED); } } @@ -166,7 +166,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { mpuSpi9250InitDone = true; //init done } -bool mpu9250SpiDetect(const busDevice_t *bus) +uint8_t mpu9250SpiDetect(const busDevice_t *bus) { IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); @@ -182,13 +182,13 @@ bool mpu9250SpiDetect(const busDevice_t *bus) break; } if (!attemptsRemaining) { - return false; + return MPU_NONE; } } while (attemptsRemaining--); spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST); - return true; + return MPU_9250_SPI; } bool mpu9250SpiAccDetect(accDev_t *acc) diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index a9d9b1b0d6..e7d3958c62 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -28,7 +28,7 @@ void mpu9250SpiResetGyro(void); -bool mpu9250SpiDetect(const busDevice_t *bus); +uint8_t mpu9250SpiDetect(const busDevice_t *bus); bool mpu9250SpiAccDetect(accDev_t *acc); bool mpu9250SpiGyroDetect(gyroDev_t *gyro);