From 489156602f8f1f69a153e056454a852735e15498 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 7 Apr 2020 01:03:51 +1200 Subject: [PATCH] Fixed unnecessarily long startup time due to gyro detection. --- src/main/cli/cli.c | 13 +++++++++ src/main/sensors/gyro.h | 12 ++++---- src/main/sensors/gyro_init.c | 41 ++++++++++++++++++--------- src/test/unit/cli_unittest.cc | 2 ++ src/test/unit/sensor_gyro_unittest.cc | 1 + 5 files changed, 51 insertions(+), 18 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 1a4b2c941..ae3563e27 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4644,6 +4644,19 @@ static void cliStatus(const char *cmdName, char *cmdline) cliPrintLinef("Config size: %d, Max available config: %d", getEEPROMConfigSize(), getEEPROMStorageSize()); // Sensors + cliPrint("Gyros detected:"); + bool found = false; + for (unsigned pos = 0; pos < 7; pos++) { + if (gyroConfig()->gyrosDetected & BIT(pos)) { + if (found) { + cliPrint(","); + } else { + found = true; + } + cliPrintf(" gyro %d", pos + 1); + } + } + cliPrintLinefeed(); #if defined(USE_SENSOR_NAMES) const uint32_t detectedSensorsMask = sensorsMask(); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 43fad15e6..653915078 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -49,12 +49,12 @@ typedef union gyroLowpassFilter_u { } gyroLowpassFilter_t; typedef enum gyroDetectionFlags_e { - NO_GYROS_DETECTED = 0, - DETECTED_GYRO_1 = (1 << 0), + GYRO_NONE_MASK = 0, + GYRO_1_MASK = BIT(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 + GYRO_2_MASK = BIT(1), + GYRO_ALL_MASK = (GYRO_1_MASK | GYRO_2_MASK), + GYRO_IDENTICAL_MASK = BIT(7), // All gyros are of the same hardware type #endif } gyroDetectionFlags_t; @@ -195,6 +195,8 @@ typedef struct gyroConfig_s { uint16_t dyn_notch_min_hz; uint8_t gyro_filter_debug_axis; + + uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup. } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 393c9a785..1a3475e18 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -32,6 +32,8 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/config.h" + #include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/accgyro/accgyro_mpu.h" @@ -80,7 +82,7 @@ #define ACTIVE_GYRO (&gyro.gyroSensor1) #endif -static gyroDetectionFlags_t gyroDetectionFlags = NO_GYROS_DETECTED; +static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK; static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz) { @@ -565,45 +567,54 @@ bool gyroInit(void) break; } - gyroDetectionFlags = NO_GYROS_DETECTED; + gyroDetectionFlags = GYRO_NONE_MASK; + uint8_t gyrosToScan = gyroConfig()->gyrosDetected; gyro.gyroToUse = gyroConfig()->gyro_to_use; gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis; - if (gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) { - gyroDetectionFlags |= DETECTED_GYRO_1; + if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) { + gyroDetectionFlags |= GYRO_1_MASK; } #if defined(USE_MULTI_GYRO) - if (gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) { - gyroDetectionFlags |= DETECTED_GYRO_2; + if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) { + gyroDetectionFlags |= GYRO_2_MASK; } #endif - if (gyroDetectionFlags == NO_GYROS_DETECTED) { + if (gyroDetectionFlags == GYRO_NONE_MASK) { return false; } + bool eepromWriteRequired = false; + if (!gyrosToScan) { + gyroConfigMutable()->gyrosDetected = gyroDetectionFlags; + eepromWriteRequired = true; + } + #if defined(USE_MULTI_GYRO) - if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & DETECTED_BOTH_GYROS) == DETECTED_BOTH_GYROS)) - || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1)) - || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) { - if (gyroDetectionFlags & DETECTED_GYRO_1) { + if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK)) + || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK)) + || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) { + if (gyroDetectionFlags & GYRO_1_MASK) { gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1; } else { gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_2; } gyroConfigMutable()->gyro_to_use = gyro.gyroToUse; + eepromWriteRequired = true; } // Only allow using both gyros simultaneously if they are the same hardware type. - if (((gyroDetectionFlags & DETECTED_BOTH_GYROS) == DETECTED_BOTH_GYROS) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) { - gyroDetectionFlags |= DETECTED_DUAL_GYROS; + if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) { + gyroDetectionFlags |= GYRO_IDENTICAL_MASK; } else if (gyro.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. gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1; gyroConfigMutable()->gyro_to_use = gyro.gyroToUse; + eepromWriteRequired = true; } if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { @@ -613,6 +624,10 @@ bool gyroInit(void) } #endif + if (eepromWriteRequired) { + writeEEPROM(); + } + if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0)); gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection; diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc index 0dd702afd..22dee6367 100644 --- a/src/test/unit/cli_unittest.cc +++ b/src/test/unit/cli_unittest.cc @@ -52,6 +52,7 @@ extern "C" { #include "rx/rx.h" #include "scheduler/scheduler.h" #include "sensors/battery.h" + #include "sensors/gyro.h" void cliSet(const char *cmdName, char *cmdline); int cliGetSettingIndex(char *name, uint8_t length); @@ -85,6 +86,7 @@ extern "C" { PG_REGISTER_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs, PG_RX_CHANNEL_RANGE_CONFIG, 0); PG_REGISTER_ARRAY(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs, PG_RX_FAILSAFE_CHANNEL_CONFIG, 0); PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0); + PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(int8_t, unitTestData, PG_RESERVED_FOR_TESTING_1, 0); } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 32aa9030f..96828c466 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -162,4 +162,5 @@ timeDelta_t getGyroUpdateRate(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(taskId_e) {} int getArmingDisableFlags(void) {return 0;} +void writeEEPROM(void) {} }