From e44f75b4a7e7554b11d3197650ece64288d837b2 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 3 Apr 2019 00:41:06 +1300 Subject: [PATCH] Fixed gyro detection for multi gyro setups. --- src/main/drivers/accgyro/accgyro_mpu.c | 18 ++-- src/main/drivers/accgyro/accgyro_mpu.h | 2 +- src/main/msp/msp.c | 3 +- src/main/sensors/gyro.c | 142 +++++++++++++------------ src/main/sensors/gyro.h | 21 +++- src/main/target/common_defaults_post.h | 16 +-- src/test/unit/sensor_gyro_unittest.cc | 1 - 7 files changed, 111 insertions(+), 92 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index d669653834..6306763926 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -222,7 +222,8 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config) { SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus)); - if (!instance) { + + if (!instance || !config->csnTag) { return false; } spiBusSetInstance(&gyro->bus, instance); @@ -248,7 +249,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro spiPreinitByTag(config->csnTag); - return false; + return true; } #endif @@ -261,13 +262,13 @@ void mpuPreInit(const struct gyroDeviceConfig_s *config) #endif } -void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) +bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) { // MPU datasheet specifies 30ms. delay(35); if (config->bustype == BUSTYPE_NONE) { - return; + return false; } if (config->bustype == BUSTYPE_GYRO_AUTO) { @@ -291,7 +292,7 @@ void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_3050; - return; + return true; } sig &= MPU_INQUIRY_MASK; @@ -301,14 +302,17 @@ void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) } else if (sig == MPU6500_WHO_AM_I_CONST) { gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; } - return; + return true; } } #endif #ifdef USE_SPI_GYRO gyro->bus.bustype = BUSTYPE_SPI; - detectSPISensorsAndUpdateDetectionResult(gyro, config); + + return detectSPISensorsAndUpdateDetectionResult(gyro, config); +#else + return false; #endif } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 4acc18eb09..a90fa2f011 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -218,7 +218,7 @@ void mpuGyroInit(struct gyroDev_s *gyro); bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroReadSPI(struct gyroDev_s *gyro); void mpuPreInit(const struct gyroDeviceConfig_s *config); -void mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config); +bool mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config); uint8_t mpuGyroDLPF(struct gyroDev_s *gyro); uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 0296c996e1..04353d0f64 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1357,13 +1357,12 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, compassConfig()->mag_align); // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment + sbufWriteU8(dst, getGyroDetectionFlags()); #ifdef USE_MULTI_GYRO - sbufWriteU8(dst, 1); // USE_MULTI_GYRO sbufWriteU8(dst, gyroConfig()->gyro_to_use); sbufWriteU8(dst, gyroDeviceConfig(0)->align); sbufWriteU8(dst, gyroDeviceConfig(1)->align); #else - sbufWriteU8(dst, 0); sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1); sbufWriteU8(dst, gyroDeviceConfig(0)->align); sbufWriteU8(dst, ALIGN_DEFAULT); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5327a26e8b..f844ab114e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -86,7 +86,7 @@ FAST_RAM_ZERO_INIT gyro_t gyro; static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode; -static uint8_t gyroToUse = 0; +static FAST_RAM_ZERO_INIT uint8_t gyroToUse; static FAST_RAM_ZERO_INIT bool overflowDetected; #ifdef USE_GYRO_OVERFLOW_CHECK @@ -167,6 +167,8 @@ STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2; #endif +static gyroDetectionFlags_t gyroDetectionFlags = NO_GYROS_DETECTED; + #ifdef UNIT_TEST STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1; STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev; @@ -393,7 +395,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) } if (gyroHardware != GYRO_NONE) { - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); } @@ -411,25 +412,31 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config) #endif } -static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) +static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) +{ +#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) + + if (!mpuDetect(&gyroSensor->gyroDev, config)) { + return false; + } +#else + UNUSED(config); +#endif + + const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); + gyroSensor->gyroDev.gyroHardware = gyroHardware; + + return gyroHardware != GYRO_NONE; +} + +static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) { gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis; gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; gyroSensor->gyroDev.gyroAlign = config->align; gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; -#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) - - mpuDetect(&gyroSensor->gyroDev, config); -#endif - - const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev); - gyroSensor->gyroDev.gyroHardware = gyroHardware; - if (gyroHardware == GYRO_NONE) { - return false; - } - // Must set gyro targetLooptime before gyroDev.init and initialisation of filters gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom); gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf; @@ -437,7 +444,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative. - switch (gyroHardware) { + switch (gyroSensor->gyroDev.gyroHardware) { case GYRO_NONE: // Won't ever actually get here, but included to account for all gyro types case GYRO_DEFAULT: case GYRO_FAKE: @@ -471,8 +478,6 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime); #endif - - return true; } void gyroPreInit(void) @@ -516,64 +521,65 @@ bool gyroInit(void) } firstArmingCalibrationWasStarted = false; - enum { - NO_GYROS_DETECTED = 0, - DETECTED_GYRO_1 = (1 << 0), - DETECTED_GYRO_2 = (1 << 1), - }; - - uint8_t detectionFlags = NO_GYROS_DETECTED; + gyroDetectionFlags = NO_GYROS_DETECTED; gyroToUse = gyroConfig()->gyro_to_use; - if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - if (gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0))) { - gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection; - detectionFlags |= DETECTED_GYRO_1; - } + if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) { + gyroDetectionFlags |= DETECTED_GYRO_1; } -#ifdef USE_MULTI_GYRO - if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - if (gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1))) { - gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection; - detectionFlags |= DETECTED_GYRO_2; - } - } - - if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && detectionFlags != (DETECTED_GYRO_1 | DETECTED_GYRO_2)) { - // at least one gyro is missing. - - if (detectionFlags & DETECTED_GYRO_1) { - gyroToUse = GYRO_CONFIG_USE_GYRO_1; - gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1; - detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware; - sensorsSet(SENSOR_GYRO); - return true; - } - - if (detectionFlags & DETECTED_GYRO_2) { - gyroToUse = GYRO_CONFIG_USE_GYRO_2; - gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_2; - detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware; - sensorsSet(SENSOR_GYRO); - return true; - } - } - - // Only allow using both gyros simultaneously if they are the same hardware type. - // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro. - if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - if (gyroSensor1.gyroDev.gyroHardware != gyroSensor2.gyroDev.gyroHardware) { - gyroToUse = GYRO_CONFIG_USE_GYRO_1; - gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1; - detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware; - sensorsSet(SENSOR_GYRO); - } +#if defined(USE_MULTI_GYRO) + if (gyroDetectSensor(&gyroSensor2, gyroDeviceConfig(1))) { + gyroDetectionFlags |= DETECTED_GYRO_2; } #endif - return detectionFlags != NO_GYROS_DETECTED; + if (gyroDetectionFlags == NO_GYROS_DETECTED) { + return false; + } + +#if defined(USE_MULTI_GYRO) + if ((gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !(gyroDetectionFlags & DETECTED_BOTH_GYROS)) + || (gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1)) + || (gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) { + if (gyroDetectionFlags & DETECTED_GYRO_1) { + gyroToUse = GYRO_CONFIG_USE_GYRO_1; + } else { + gyroToUse = GYRO_CONFIG_USE_GYRO_2; + } + + gyroConfigMutable()->gyro_to_use = gyroToUse; + } + + // Only allow using both gyros simultaneously if they are the same hardware type. + if ((gyroDetectionFlags & DETECTED_BOTH_GYROS) && gyroSensor1.gyroDev.gyroHardware == gyroSensor2.gyroDev.gyroHardware) { + gyroDetectionFlags |= DETECTED_DUAL_GYROS; + } else if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro. + gyroToUse = GYRO_CONFIG_USE_GYRO_1; + gyroConfigMutable()->gyro_to_use = gyroToUse; + } + + if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1)); + gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection; + detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware; + } +#endif + + if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { + gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0)); + gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection; + detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware; + } + + return true; +} + +gyroDetectionFlags_t getGyroDetectionFlags(void) +{ + return gyroDetectionFlags; } #ifdef USE_DYN_LPF diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e73572166b..490d743587 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -36,18 +36,18 @@ typedef struct gyro_s { extern gyro_t gyro; -typedef enum { +enum { GYRO_OVERFLOW_CHECK_NONE = 0, GYRO_OVERFLOW_CHECK_YAW, GYRO_OVERFLOW_CHECK_ALL_AXES -} gyroOverflowCheck_e; +}; enum { DYN_NOTCH_RANGE_HIGH = 0, DYN_NOTCH_RANGE_MEDIUM, DYN_NOTCH_RANGE_LOW, DYN_NOTCH_RANGE_AUTO -} ; +}; #define DYN_NOTCH_RANGE_HZ_HIGH 2000 #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333 @@ -63,10 +63,20 @@ enum { #define GYRO_CONFIG_USE_GYRO_2 1 #define GYRO_CONFIG_USE_GYRO_BOTH 2 -typedef enum { +enum { FILTER_LOWPASS = 0, FILTER_LOWPASS2 -} filterSlots; +}; + +typedef enum gyroDetectionFlags_e { + NO_GYROS_DETECTED = 0, + DETECTED_GYRO_1 = (1 << 0), +#if defined(USE_MULTI_GYRO) + DETECTED_GYRO_2 = (1 << 1), + DETECTED_BOTH_GYROS = (DETECTED_GYRO_1 | DETECTED_GYRO_2), + DETECTED_DUAL_GYROS = (1 << 7), // All gyros are of the same hardware type +#endif +} gyroDetectionFlags_t; typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. @@ -125,6 +135,7 @@ bool gyroOverflowDetected(void); bool gyroYawSpinDetected(void); uint16_t gyroAbsRateDps(int axis); uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); +gyroDetectionFlags_t getGyroDetectionFlags(void); #ifdef USE_DYN_LPF float dynThrottle(float throttle); void dynLpfGyroUpdate(float throttle); diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h index 4303d1f9ec..f85d0d5aa6 100644 --- a/src/main/target/common_defaults_post.h +++ b/src/main/target/common_defaults_post.h @@ -293,14 +293,6 @@ #endif #endif -// F4 and F7 single gyro boards -#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE) -#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE -#define GYRO_2_CS_PIN NONE -#define GYRO_2_ALIGN ALIGN_DEFAULT -#define GYRO_2_EXTI_PIN NONE -#endif - #if !defined(GYRO_1_SPI_INSTANCE) #define GYRO_1_SPI_INSTANCE NULL #endif @@ -317,6 +309,14 @@ #define GYRO_1_ALIGN ALIGN_DEFAULT #endif +// F4 and F7 single gyro boards +#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE) +#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE +#define GYRO_2_CS_PIN NONE +#define GYRO_2_ALIGN ALIGN_DEFAULT +#define GYRO_2_EXTI_PIN NONE +#endif + #if defined(MPU_ADDRESS) #define GYRO_I2C_ADDRESS MPU_ADDRESS #else diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index b132e44683..6da74aedc5 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -59,7 +59,6 @@ TEST(SensorGyro, Detect) { const gyroHardware_e detected = gyroDetect(gyroDevPtr); EXPECT_EQ(GYRO_FAKE, detected); - EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]); } TEST(SensorGyro, Init)