From 78b1c1ab1a87070c4b4e61bd6f4b589676e0f3d3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 12 Jul 2017 15:11:39 +0100 Subject: [PATCH] Gyro SPI detect function now returns gyro type --- src/main/drivers/accgyro/accgyro_mpu.c | 29 ++++++++++++------- src/main/drivers/accgyro/accgyro_spi_bmi160.c | 12 ++++---- src/main/drivers/accgyro/accgyro_spi_bmi160.h | 2 +- .../drivers/accgyro/accgyro_spi_icm20689.c | 6 ++-- .../drivers/accgyro/accgyro_spi_icm20689.h | 2 +- .../drivers/accgyro/accgyro_spi_mpu6000.c | 8 ++--- .../drivers/accgyro/accgyro_spi_mpu6000.h | 2 +- .../drivers/accgyro/accgyro_spi_mpu9250.c | 10 +++---- .../drivers/accgyro/accgyro_spi_mpu9250.h | 2 +- 9 files changed, 40 insertions(+), 33 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index d4b11b123..cfab4f402 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -252,6 +252,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); @@ -259,8 +262,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; @@ -275,10 +279,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; @@ -293,8 +297,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; @@ -310,8 +315,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; @@ -326,8 +332,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 f9ad9aa3c..0dd4bfc05 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; } @@ -393,7 +393,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; } @@ -406,7 +406,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 ad5c6e1e2..d1a9ea660 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 438b67b05..3e4cc8217 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 164f51dec..57cc827f8 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 2413174b7..432d903e3 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 8435cebad..7682a2872 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 000d18a6f..b7516b3db 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 a9d9b1b0d..e7d3958c6 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);