diff --git a/src/config b/src/config index 96eebb9999..535b2472cf 160000 --- a/src/config +++ b/src/config @@ -1 +1 @@ -Subproject commit 96eebb999926be0769394f3105065f4e10472433 +Subproject commit 535b2472cffdc9891241a815eb9a2947b330d2c2 diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b4d7f02e7e..1492282d83 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1687,7 +1687,7 @@ static bool blackboxWriteSysinfo(void) gyroConfig()->gyro_soft_notch_hz_2); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, gyroConfig()->gyro_soft_notch_cutoff_2); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_TO_USE, "%d", gyroConfig()->gyro_to_use); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_ENABLE_MASK, "%d", gyroConfig()->gyro_enabled_bitmask); BLACKBOX_PRINT_HEADER_LINE("gyro_debug_axis", "%d", gyroConfig()->gyro_filter_debug_axis); #ifdef USE_DYN_NOTCH_FILTER BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MAX_HZ, "%d", dynNotchConfig()->dyn_notch_max_hz); diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 6a8aac7538..bdfb9eb82b 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -3675,18 +3675,12 @@ static void cliDumpGyroRegisters(const char *cmdName, char *cmdline) UNUSED(cmdName); UNUSED(cmdline); -#ifdef USE_MULTI_GYRO - if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) { - cliPrintLinef("\r\n# Gyro 1"); - cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1); + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyroConfig()->gyro_enabled_bitmask & GYRO_MASK(i)) { + cliPrintLinef("\r\n# Gyro %d", i + 1); + cliPrintGyroRegisters(i); // assuming this takes a 0-based gyro index + } } - if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) { - cliPrintLinef("\r\n# Gyro 2"); - cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_2); - } -#else - cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1); -#endif } #endif @@ -5329,7 +5323,7 @@ static void showDma(void) cliPrintLine("Currently active DMA:"); cliRepeat('-', 20); #endif - for (int i = 1; i <= DMA_LAST_HANDLER; i++) { + for (int i = DMA_FIRST_HANDLER; i <= DMA_LAST_HANDLER; i++) { const resourceOwner_t *owner = dmaGetOwner(i); cliPrintf(DMA_OUTPUT_STRING, DMA_DEVICE_NO(i), DMA_DEVICE_INDEX(i)); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index d2b1c49158..9e6c0ddbf2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -244,12 +244,6 @@ static const char * const lookupTableAlignment[] = { "CUSTOM", }; -#ifdef USE_MULTI_GYRO -static const char * const lookupTableGyro[] = { - "FIRST", "SECOND", "BOTH" -}; -#endif - #ifdef USE_GPS static const char * const lookupTableGpsProvider[] = { "NMEA", "UBLOX", "MSP", "VIRTUAL" @@ -679,9 +673,6 @@ const lookupTableEntry_t lookupTables[] = { #endif #ifdef USE_LED_STRIP LOOKUP_TABLE_ENTRY(lookupLedStripFormatRGB), -#endif -#ifdef USE_MULTI_GYRO - LOOKUP_TABLE_ENTRY(lookupTableGyro), #endif LOOKUP_TABLE_ENTRY(lookupTableThrottleLimitType), #if defined(USE_VIDEO_SYSTEM) @@ -774,9 +765,30 @@ const clivalue_t valueTable[] = { { "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) }, #endif -#ifdef USE_MULTI_GYRO - { PARAM_NAME_GYRO_TO_USE, VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, +#if GYRO_COUNT > 1 + { PARAM_NAME_GYRO_ENABLE_MASK, VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, (1 << GYRO_COUNT) - 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, + { "gyro_1_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 0, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, + { "gyro_2_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 1, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, #endif +#if GYRO_COUNT > 2 + { "gyro_3_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 2, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, +#endif +#if GYRO_COUNT > 3 + { "gyro_4_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 3, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, +#endif +#if GYRO_COUNT > 4 + { "gyro_5_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 4, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, +#endif +#if GYRO_COUNT > 5 + { "gyro_6_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 5, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, +#endif +#if GYRO_COUNT > 6 + { "gyro_7_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 6, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, +#endif +#if GYRO_COUNT > 7 + { "gyro_8_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 7, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) }, +#endif // GYRO_COUNT + #if defined(USE_DYN_NOTCH_FILTER) { PARAM_NAME_DYN_NOTCH_COUNT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_NOTCH_COUNT_MAX }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_count) }, { PARAM_NAME_DYN_NOTCH_Q, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_q) }, @@ -1826,23 +1838,43 @@ const clivalue_t valueTable[] = { #endif // PG_GYRO_DEVICE_CONFIG - { "gyro_1_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, busType) }, - { "gyro_1_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, spiBus) }, - { "gyro_1_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cBus) }, - { "gyro_1_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) }, - { "gyro_1_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, alignment) }, - { "gyro_1_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.roll) }, - { "gyro_1_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.pitch) }, - { "gyro_1_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.yaw) }, -#ifdef USE_MULTI_GYRO - { "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, busType) }, - { "gyro_2_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, spiBus) }, - { "gyro_2_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cBus) }, - { "gyro_2_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) }, - { "gyro_2_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, alignment) }, - { "gyro_2_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.roll) }, - { "gyro_2_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.pitch) }, - { "gyro_2_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.yaw) }, +#define GYRO_DEVICE_RECORDS(N, IDX) \ + /* bustype */ \ + { "gyro_" STR(N) "_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, busType) },\ + /* spibus */ \ + { "gyro_" STR(N) "_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, spiBus) },\ + /* i2cBus */ \ + { "gyro_" STR(N) "_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, i2cBus) },\ + /* i2c addr */ \ + { "gyro_" STR(N) "_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, i2cAddress) },\ + /* align lookup */ \ + { "gyro_" STR(N) "_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, alignment) },\ + /* custom roll/pitch/yaw */ \ + { "gyro_" STR(N) "_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, customAlignment.roll) },\ + { "gyro_" STR(N) "_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, customAlignment.pitch) },\ + { "gyro_" STR(N) "_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, customAlignment.yaw) }\ + + GYRO_DEVICE_RECORDS(1, 0), +#if GYRO_COUNT > 1 + GYRO_DEVICE_RECORDS(2, 1), +#endif +#if GYRO_COUNT > 2 + GYRO_DEVICE_RECORDS(3, 2), +#endif +#if GYRO_COUNT > 3 + GYRO_DEVICE_RECORDS(4, 3), +#endif +#if GYRO_COUNT > 4 + GYRO_DEVICE_RECORDS(5, 4), +#endif +#if GYRO_COUNT > 5 + GYRO_DEVICE_RECORDS(6, 5), +#endif +#if GYRO_COUNT > 6 + GYRO_DEVICE_RECORDS(7, 6), +#endif +#if GYRO_COUNT > 7 + GYRO_DEVICE_RECORDS(8, 7), #endif #ifdef I2C_FULL_RECONFIGURABILITY #ifdef USE_I2C_DEVICE_0 diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index b3222f8b94..8c79b54aa2 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -94,9 +94,6 @@ typedef enum { #endif #ifdef USE_LED_STRIP TABLE_RGB_GRB, -#endif -#ifdef USE_MULTI_GYRO - TABLE_GYRO, #endif TABLE_THROTTLE_LIMIT_TYPE, #if defined(USE_VIDEO_SYSTEM) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 2c48e6106d..8a7440ba5c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -76,12 +76,6 @@ static uint8_t rateProfileIndex; static char rateProfileIndexString[MAX_RATE_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE]; static controlRateConfig_t rateProfile; -#ifdef USE_MULTI_GYRO -static const char * const osdTableGyroToUse[] = { - "FIRST", "SECOND", "BOTH" -}; -#endif - static void setProfileIndexString(char *profileString, int profileIndex, const char *profileName) { int charIndex = 0; @@ -752,7 +746,6 @@ static uint16_t gyroConfig_gyro_soft_notch_hz_1; static uint16_t gyroConfig_gyro_soft_notch_cutoff_1; static uint16_t gyroConfig_gyro_soft_notch_hz_2; static uint16_t gyroConfig_gyro_soft_notch_cutoff_2; -static uint8_t gyroConfig_gyro_to_use; static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp) { @@ -764,7 +757,6 @@ static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp) gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1; gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2; gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2; - gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use; return NULL; } @@ -780,7 +772,6 @@ static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *s gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1; gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2; gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2; - gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use; return NULL; } @@ -797,9 +788,6 @@ static const OSD_Entry cmsx_menuFilterGlobalEntries[] = { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 } }, { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 } }, { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } }, -#ifdef USE_MULTI_GYRO - { "GYRO TO USE", OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gyroConfig_gyro_to_use, 2, osdTableGyroToUse} }, -#endif { "BACK", OME_Back, NULL, NULL }, { NULL, OME_END, NULL, NULL} diff --git a/src/main/config/config.c b/src/main/config/config.c index 16250fa3eb..27e6f8375f 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -293,10 +293,10 @@ static void validateAndFixConfig(void) #if defined(USE_MAG) buildAlignmentFromStandardAlignment(&compassConfigMutable()->mag_customAlignment, compassConfig()->mag_alignment); #endif - buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(0)->customAlignment, gyroDeviceConfig(0)->alignment); -#if defined(USE_MULTI_GYRO) - buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(1)->customAlignment, gyroDeviceConfig(1)->alignment); -#endif + + for (int i = 0; i < GYRO_COUNT; i++) { + buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(i)->customAlignment, gyroDeviceConfig(i)->alignment); + } #ifdef USE_ACC if (accelerometerConfig()->accZero.values.roll != 0 || diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index afdc21e2fa..60ce63d5dc 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -190,17 +190,25 @@ static void setUserBank(const extDevice_t *dev, const uint8_t user_bank) #if defined(USE_GYRO_CLKIN) static pwmOutputPort_t pwmGyroClk = {0}; +static int findByExtDevice(const extDevice_t *dev) { + for (int i = 0; i < GYRO_COUNT; i++) { + if (&gyro.gyroSensor[i].gyroDev.dev == dev) { + return i; + } + } + return -1; +} + + static bool initExternalClock(const extDevice_t *dev) { - int cfg; - if (&gyro.gyroSensor1.gyroDev.dev == dev) { - cfg = 0; - } else if (&gyro.gyroSensor2.gyroDev.dev == dev) { - cfg = 1; - } else { - // only gyroSensor device supported + const int cfg = findByExtDevice(dev); + + if (cfg < 0) { + // Could not find a valid sensor return false; } + const ioTag_t tag = gyroDeviceConfig(cfg)->clkIn; const IO_t io = IOGetByTag(tag); if (pwmGyroClk.enabled) { diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 39ce91552c..688f93e858 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -58,7 +58,7 @@ typedef struct dmaChannelDescriptor_s { #endif } dmaChannelDescriptor_t; -#define DMA_IDENTIFIER_TO_INDEX(x) ((x) - 1) +#define DMA_IDENTIFIER_TO_INDEX(x) ((x) - DMA_FIRST_HANDLER) void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId); diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 8d9c812183..9dd99ed845 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -25,7 +25,7 @@ #define PARAM_NAME_GYRO_LPF1_STATIC_HZ "gyro_lpf1_static_hz" #define PARAM_NAME_GYRO_LPF2_TYPE "gyro_lpf2_type" #define PARAM_NAME_GYRO_LPF2_STATIC_HZ "gyro_lpf2_static_hz" -#define PARAM_NAME_GYRO_TO_USE "gyro_to_use" +#define PARAM_NAME_GYRO_ENABLE_MASK "gyro_enable_bitmask" #define PARAM_NAME_DYN_NOTCH_MAX_HZ "dyn_notch_max_hz" #define PARAM_NAME_DYN_NOTCH_COUNT "dyn_notch_count" #define PARAM_NAME_DYN_NOTCH_Q "dyn_notch_q" diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 3087069b9d..b5fe879f1a 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1812,21 +1812,7 @@ case MSP_NAME: break; case MSP_SENSOR_ALIGNMENT: { - uint8_t gyroAlignment; -#ifdef USE_MULTI_GYRO - switch (gyroConfig()->gyro_to_use) { - case GYRO_CONFIG_USE_GYRO_2: - gyroAlignment = gyroDeviceConfig(1)->alignment; - break; - case GYRO_CONFIG_USE_GYRO_BOTH: - // for dual-gyro in "BOTH" mode we only read/write gyro 0 - default: - gyroAlignment = gyroDeviceConfig(0)->alignment; - break; - } -#else - gyroAlignment = gyroDeviceConfig(0)->alignment; -#endif + uint8_t gyroAlignment = gyroDeviceConfig(firstEnabledGyro())->alignment; sbufWriteU8(dst, gyroAlignment); sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same #if defined(USE_MAG) @@ -1836,32 +1822,17 @@ case MSP_NAME: #endif // 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, gyroConfig()->gyro_to_use); - sbufWriteU8(dst, gyroDeviceConfig(0)->alignment); - sbufWriteU8(dst, gyroDeviceConfig(1)->alignment); -#else - sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1); - sbufWriteU8(dst, gyroDeviceConfig(0)->alignment); - sbufWriteU8(dst, ALIGN_DEFAULT); -#endif - // Added in MSP API 1.47 - switch (gyroConfig()->gyro_to_use) { -#ifdef USE_MULTI_GYRO - case GYRO_CONFIG_USE_GYRO_2: - sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.roll); - sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.pitch); - sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.yaw); - break; -#endif - case GYRO_CONFIG_USE_GYRO_BOTH: - // for dual-gyro in "BOTH" mode we only read/write gyro 0 - default: - sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.roll); - sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.pitch); - sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.yaw); - break; + sbufWriteU8(dst, getGyroDetectedFlags()); + sbufWriteU8(dst, gyroConfig()->gyro_enabled_bitmask); // deprecates gyro_to_use + // Added support for more then two IMUs in MSP API 1.47 + for (int i = 0; i < 8; i++) { + sbufWriteU8(dst, i < GYRO_COUNT ? gyroDeviceConfig(i)->alignment : ALIGN_DEFAULT); + } + + for (int i = 0; i < 8; i++) { + for (unsigned j; j < ARRAYLEN(gyroDeviceConfig(i)->customAlignment.raw); j++) { + sbufWriteU16(dst, i < GYRO_COUNT ? gyroDeviceConfig(i)->customAlignment.raw[j] : 0); + } } #ifdef USE_MAG @@ -1885,7 +1856,7 @@ case MSP_NAME: sbufWriteU16(dst, motorConfig()->motorIdle); sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz sbufWriteU8(dst, motorConfig()->dev.motorInversion); - sbufWriteU8(dst, gyroConfig()->gyro_to_use); + sbufWriteU8(dst, 0); // deprecated gyro_to_use sbufWriteU8(dst, gyroConfig()->gyro_high_fsr); sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold); sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration); @@ -3014,8 +2985,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, break; case MSP_SET_SENSOR_ALIGNMENT: { - // maintain backwards compatibility for API < 1.41 - const uint8_t gyroAlignment = sbufReadU8(src); + sbufReadU8(src); sbufReadU8(src); // discard deprecated acc_align #if defined(USE_MAG) compassConfigMutable()->mag_alignment = sbufReadU8(src); @@ -3023,53 +2993,33 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); #endif - if (sbufBytesRemaining(src) >= 3) { - // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2 -#ifdef USE_MULTI_GYRO - gyroConfigMutable()->gyro_to_use = sbufReadU8(src); - gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src); - gyroDeviceConfigMutable(1)->alignment = sbufReadU8(src); -#else - sbufReadU8(src); // unused gyro_to_use - gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src); - sbufReadU8(src); // unused gyro_2_sensor_align -#endif - } else { - // maintain backwards compatibility for API < 1.41 -#ifdef USE_MULTI_GYRO - switch (gyroConfig()->gyro_to_use) { - case GYRO_CONFIG_USE_GYRO_2: - gyroDeviceConfigMutable(1)->alignment = gyroAlignment; - break; - case GYRO_CONFIG_USE_GYRO_BOTH: - // For dual-gyro in "BOTH" mode we'll only update gyro 0 - default: - gyroDeviceConfigMutable(0)->alignment = gyroAlignment; - break; - } -#else - gyroDeviceConfigMutable(0)->alignment = gyroAlignment; -#endif - } + gyroConfigMutable()->gyro_enabled_bitmask = sbufReadU8(src); // Added in API 1.47 - if (sbufBytesRemaining(src) >= 6) { - switch (gyroConfig()->gyro_to_use) { -#ifdef USE_MULTI_GYRO - case GYRO_CONFIG_USE_GYRO_2: - gyroDeviceConfigMutable(1)->customAlignment.roll = sbufReadU16(src); - gyroDeviceConfigMutable(1)->customAlignment.pitch = sbufReadU16(src); - gyroDeviceConfigMutable(1)->customAlignment.yaw = sbufReadU16(src); - break; -#endif - case GYRO_CONFIG_USE_GYRO_BOTH: - // For dual-gyro in "BOTH" mode we'll only update gyro 0 - default: - gyroDeviceConfigMutable(0)->customAlignment.roll = sbufReadU16(src); - gyroDeviceConfigMutable(0)->customAlignment.pitch = sbufReadU16(src); - gyroDeviceConfigMutable(0)->customAlignment.yaw = sbufReadU16(src); - break; + if (sbufBytesRemaining(src) >= 8 * 7) { + for (int i = 0; i < 8; i++) { + const uint8_t alignment = sbufReadU8(src); + if (i < GYRO_COUNT) { + gyroDeviceConfigMutable(i)->alignment = alignment; + } + } + + for (int i = 0; i < 8; i++) { + if (i < GYRO_COUNT) { + sensorAlignment_t customAlignment; + for (unsigned j = 0; j < ARRAYLEN(customAlignment.raw); j++) { + customAlignment.raw[j] = (int16_t)sbufReadU16(src); + } + if (i < GYRO_COUNT) { + gyroDeviceConfigMutable(i)->customAlignment = customAlignment; + } + } else { + sbufReadU16(src); // skip unused custom alignment roll + sbufReadU16(src); // skip unused custom alignment pitch + sbufReadU16(src); // skip unused custom alignment yaw + } } } + if (sbufBytesRemaining(src) >= 6) { #ifdef USE_MAG compassConfigMutable()->mag_customAlignment.roll = sbufReadU16(src); @@ -3081,6 +3031,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif } + break; } @@ -3100,7 +3051,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, motorConfigMutable()->dev.motorInversion = sbufReadU8(src); } if (sbufBytesRemaining(src) >= 8) { - gyroConfigMutable()->gyro_to_use = sbufReadU8(src); + sbufReadU8(src); // deprecated gyro_to_use gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src); gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src); gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src); diff --git a/src/main/pg/gyrodev.c b/src/main/pg/gyrodev.c index 67d2d7d442..0a4fabf826 100644 --- a/src/main/pg/gyrodev.c +++ b/src/main/pg/gyrodev.c @@ -35,6 +35,10 @@ #include "drivers/sensor.h" #include "sensors/gyro.h" +#ifdef SIMULATOR_BUILD +#define GYRO_COUNT 1 +#endif + #ifndef GYRO_1_CS_PIN #define GYRO_1_CS_PIN NONE #endif @@ -43,6 +47,10 @@ #define GYRO_1_EXTI_PIN NONE #endif +#ifndef GYRO_1_CLKIN_PIN +#define GYRO_1_CLKIN_PIN NONE +#endif + #ifndef GYRO_2_CS_PIN #define GYRO_2_CS_PIN NONE #endif @@ -51,14 +59,34 @@ #define GYRO_2_EXTI_PIN NONE #endif -#ifndef GYRO_1_CLKIN_PIN -#define GYRO_1_CLKIN_PIN NONE -#endif - #ifndef GYRO_2_CLKIN_PIN #define GYRO_2_CLKIN_PIN NONE #endif +#ifndef GYRO_3_CS_PIN +#define GYRO_3_CS_PIN NONE +#endif + +#ifndef GYRO_3_EXTI_PIN +#define GYRO_3_EXTI_PIN NONE +#endif + +#ifndef GYRO_3_CLKIN_PIN +#define GYRO_3_CLKIN_PIN NONE +#endif + +#ifndef GYRO_4_CS_PIN +#define GYRO_4_CS_PIN NONE +#endif + +#ifndef GYRO_4_EXTI_PIN +#define GYRO_4_EXTI_PIN NONE +#endif + +#ifndef GYRO_4_CLKIN_PIN +#define GYRO_4_CLKIN_PIN NONE +#endif + #ifdef MPU_ADDRESS #define GYRO_I2C_ADDRESS MPU_ADDRESS #else @@ -78,7 +106,7 @@ #ifndef GYRO_1_ALIGN_YAW #define GYRO_1_ALIGN_YAW 0 #endif -#ifndef GYRO_1_CUSTOM_ALIGN +#ifndef GYRO_1_CUSTOM_ALIGN #define GYRO_1_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_1_ALIGN_ROLL / 10, GYRO_1_ALIGN_PITCH / 10, GYRO_1_ALIGN_YAW / 10 ) #else #error "GYRO_1_ALIGN_x and GYRO_1_CUSTOM_ALIGN are mutually exclusive" @@ -102,6 +130,40 @@ #endif #endif // GYRO_2_ALIGN_ROLL || GYRO_2_ALIGN_PITCH || GYRO_2_ALIGN_YAW +#if defined(GYRO_3_ALIGN_ROLL) || defined(GYRO_3_ALIGN_PITCH) || defined(GYRO_3_ALIGN_YAW) +#ifndef GYRO_3_ALIGN_ROLL +#define GYRO_3_ALIGN_ROLL 0 +#endif +#ifndef GYRO_3_ALIGN_PITCH +#define GYRO_3_ALIGN_PITCH 0 +#endif +#ifndef GYRO_3_ALIGN_YAW +#define GYRO_3_ALIGN_YAW 0 +#endif +#ifndef GYRO_3_CUSTOM_ALIGN +#define GYRO_3_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_3_ALIGN_ROLL / 10, GYRO_3_ALIGN_PITCH / 10, GYRO_3_ALIGN_YAW / 10 ) +#else +#error "GYRO_3_ALIGN_x and GYRO_3_CUSTOM_ALIGN are mutually exclusive" +#endif +#endif // GYRO_3_ALIGN_ROLL || GYRO_3_ALIGN_PITCH || GYRO_3_ALIGN_YAW + +#if defined(GYRO_4_ALIGN_ROLL) || defined(GYRO_4_ALIGN_PITCH) || defined(GYRO_4_ALIGN_YAW) +#ifndef GYRO_4_ALIGN_ROLL +#define GYRO_4_ALIGN_ROLL 0 +#endif +#ifndef GYRO_4_ALIGN_PITCH +#define GYRO_4_ALIGN_PITCH 0 +#endif +#ifndef GYRO_4_ALIGN_YAW +#define GYRO_4_ALIGN_YAW 0 +#endif +#ifndef GYRO_4_CUSTOM_ALIGN +#define GYRO_4_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_4_ALIGN_ROLL / 10, GYRO_4_ALIGN_PITCH / 10, GYRO_4_ALIGN_YAW / 10 ) +#else +#error "GYRO_4_ALIGN_x and GYRO_4_CUSTOM_ALIGN are mutually exclusive" +#endif +#endif // GYRO_4_ALIGN_ROLL || GYRO_4_ALIGN_PITCH || GYRO_4_ALIGN_YAW + #ifndef GYRO_1_ALIGN #ifdef GYRO_1_CUSTOM_ALIGN #define GYRO_1_ALIGN ALIGN_CUSTOM @@ -130,6 +192,34 @@ STATIC_ASSERT(GYRO_1_ALIGN == ALIGN_CUSTOM, "GYRO_1_ALIGN and GYRO_1_CUSTOM_ALIG STATIC_ASSERT(GYRO_2_ALIGN == ALIGN_CUSTOM, "GYRO_2_ALIGN and GYRO_2_CUSTOM_ALIGN mixed"); #endif // GYRO_2_CUSTOM_ALIGN +#ifndef GYRO_3_ALIGN +#ifdef GYRO_3_CUSTOM_ALIGN +#define GYRO_3_ALIGN ALIGN_CUSTOM +#else +#define GYRO_3_ALIGN CW0_DEG +#endif +#endif // GYRO_3_ALIGN + +#ifndef GYRO_3_CUSTOM_ALIGN +#define GYRO_3_CUSTOM_ALIGN SENSOR_ALIGNMENT_FROM_STD(GYRO_3_ALIGN) +#else +STATIC_ASSERT(GYRO_3_ALIGN == ALIGN_CUSTOM, "GYRO_3_ALIGN and GYRO_3_CUSTOM_ALIGN mixed"); +#endif // GYRO_3_CUSTOM_ALIGN + +#ifndef GYRO_4_ALIGN +#ifdef GYRO_4_CUSTOM_ALIGN +#define GYRO_4_ALIGN ALIGN_CUSTOM +#else +#define GYRO_4_ALIGN CW0_DEG +#endif +#endif // GYRO_4_ALIGN + +#ifndef GYRO_4_CUSTOM_ALIGN +#define GYRO_4_CUSTOM_ALIGN SENSOR_ALIGNMENT_FROM_STD(GYRO_4_ALIGN) +#else +STATIC_ASSERT(GYRO_4_ALIGN == ALIGN_CUSTOM, "GYRO_4_ALIGN and GYRO_4_CUSTOM_ALIGN mixed"); +#endif // GYRO_4_CUSTOM_ALIGN + #if defined(USE_SPI_GYRO) && (defined(GYRO_1_SPI_INSTANCE) || defined(GYRO_2_SPI_INSTANCE)) static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, ioTag_t clkInTag, uint8_t alignment, sensorAlignment_t customAlignment) { @@ -143,7 +233,7 @@ static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *i } #endif -#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO) +#if defined(USE_I2C_GYRO) && !(GYRO_COUNT > 1) static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment) { devconf->busType = BUS_TYPE_I2C; @@ -162,25 +252,58 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf) devconf[0].index = 0; // All multi-gyro boards use SPI based gyros. #ifdef USE_SPI_GYRO + +#define GYRO_RESET(index, num) \ + gyroResetSpiDeviceConfig(&devconf[index], GYRO_##num##_SPI_INSTANCE, \ + IO_TAG(GYRO_##num##_CS_PIN), IO_TAG(GYRO_##num##_EXTI_PIN), \ + IO_TAG(GYRO_##num##_CLKIN_PIN), GYRO_##num##_ALIGN, GYRO_##num##_CUSTOM_ALIGN) + #ifdef GYRO_1_SPI_INSTANCE - gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), IO_TAG(GYRO_1_CLKIN_PIN), GYRO_1_ALIGN, GYRO_1_CUSTOM_ALIGN); + GYRO_RESET(0, 1); #else devconf[0].busType = BUS_TYPE_NONE; #endif -#ifdef USE_MULTI_GYRO + +#if GYRO_COUNT > 1 devconf[1].index = 1; #ifdef GYRO_2_SPI_INSTANCE // TODO: CLKIN gyro 2 on separate pin is not supported yet. need to implement it - gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), IO_TAG(GYRO_2_CLKIN_PIN), GYRO_2_ALIGN, GYRO_2_CUSTOM_ALIGN); + GYRO_RESET(1, 2); #else devconf[1].busType = BUS_TYPE_NONE; #endif +#endif // GYRO_COUNT 2 -#endif // USE_MULTI_GYRO +#if GYRO_COUNT > 2 + devconf[2].index = 2; +#ifdef GYRO_3_SPI_INSTANCE + // TODO: CLKIN gyro 3 on separate pin is not supported yet. need to implement it + GYRO_RESET(2, 3); +#else + devconf[2].busType = BUS_TYPE_NONE; +#endif +#endif // GYRO_COUNT 3 + +#if GYRO_COUNT > 3 + devconf[3].index = 3; +#ifdef GYRO_4_SPI_INSTANCE + // TODO: CLKIN gyro 4 on separate pin is not supported yet. need to implement it + GYRO_RESET(3, 4); +#else + devconf[3].busType = BUS_TYPE_NONE; +#endif +#endif // GYRO_COUNT 4 + +#if GYRO_COUNT > 4 + // reset all gyro greater than gyro 5 + for (int i = 4; i < 8; i++) { + devconf[i].busType = BUS_TYPE_NONE; + } +#endif #endif // USE_SPI_GYRO // I2C gyros appear as a sole gyro in single gyro boards. -#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO) +#if defined(USE_I2C_GYRO) && !(GYRO_COUNT > 1) devconf[0].i2cBus = I2C_DEV_TO_CFG(I2CINVALID); // XXX Not required? gyroResetI2cDeviceConfig(&devconf[0], I2C_DEVICE, IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN, customAlignment1); #endif diff --git a/src/main/pg/gyrodev.h b/src/main/pg/gyrodev.h index b46d98ecb7..5959c97f7a 100644 --- a/src/main/pg/gyrodev.h +++ b/src/main/pg/gyrodev.h @@ -26,13 +26,8 @@ #include "common/sensor_alignment.h" #include "drivers/io_types.h" -#ifdef USE_MULTI_GYRO -#define MAX_GYRODEV_COUNT 2 -#define MAX_ACCDEV_COUNT 2 -#else -#define MAX_GYRODEV_COUNT 1 -#define MAX_ACCDEV_COUNT 1 -#endif +#define MAX_GYRODEV_COUNT GYRO_COUNT +#define MAX_ACCDEV_COUNT GYRO_COUNT typedef struct gyroDeviceConfig_s { int8_t index; diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index 46a9943379..eb0fea3a3b 100644 --- a/src/main/sensors/acceleration_init.c +++ b/src/main/sensors/acceleration_init.c @@ -367,15 +367,9 @@ bool accInit(uint16_t accSampleRateHz) // Copy alignment from active gyro, as all production boards use acc-gyro-combi chip. // Exception is STM32F411DISCOVERY, and (may be) handled in future enhancement. - sensor_align_e alignment = gyroDeviceConfig(0)->alignment; - const sensorAlignment_t* customAlignment = &gyroDeviceConfig(0)->customAlignment; + sensor_align_e alignment = gyroDeviceConfig(firstEnabledGyro())->alignment; + const sensorAlignment_t* customAlignment = &gyroDeviceConfig(firstEnabledGyro())->customAlignment; -#ifdef USE_MULTI_GYRO - if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) { - alignment = gyroDeviceConfig(1)->alignment; - customAlignment = &gyroDeviceConfig(1)->customAlignment; - } -#endif acc.dev.accAlign = alignment; buildRotationMatrixFromAngles(&acc.dev.rotationMatrix, customAlignment); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 26b8d6a26c..b7d35c2b48 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -86,8 +86,8 @@ FAST_DATA uint8_t activePidLoopDenom = 1; static bool firstArmingCalibrationWasStarted = false; #ifdef UNIT_TEST -STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyro.gyroSensor1; -STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev; +STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyro.gyroSensor[0]; +STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor[0].gyroDev; #endif #define DEBUG_GYRO_CALIBRATION_VALUE 3 @@ -97,8 +97,9 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev; PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9); -#ifndef DEFAULT_GYRO_TO_USE -#define DEFAULT_GYRO_TO_USE GYRO_CONFIG_USE_GYRO_1 +#ifndef DEFAULT_GYRO_ENABLED +// enable the first gyro if none are enabled +#define DEFAULT_GYRO_ENABLED GYRO_MASK(0) #endif void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) @@ -115,7 +116,6 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->gyro_lpf2_type = FILTER_PT1; gyroConfig->gyro_lpf2_static_hz = GYRO_LPF2_HZ_DEFAULT; gyroConfig->gyro_high_fsr = false; - gyroConfig->gyro_to_use = DEFAULT_GYRO_TO_USE; gyroConfig->gyro_soft_notch_hz_1 = 0; gyroConfig->gyro_soft_notch_cutoff_1 = 0; gyroConfig->gyro_soft_notch_hz_2 = 0; @@ -130,6 +130,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->gyro_lpf1_dyn_expo = 5; gyroConfig->simplified_gyro_filter = true; gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT; + gyroConfig->gyro_enabled_bitmask = DEFAULT_GYRO_ENABLED; } static bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) @@ -139,20 +140,15 @@ static bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) bool gyroIsCalibrationComplete(void) { - switch (gyro.gyroToUse) { - default: - case GYRO_CONFIG_USE_GYRO_1: { - return isGyroSensorCalibrationComplete(&gyro.gyroSensor1); + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) { + if (!isGyroSensorCalibrationComplete(&gyro.gyroSensor[i])) { + return false; + } } -#ifdef USE_MULTI_GYRO - case GYRO_CONFIG_USE_GYRO_2: { - return isGyroSensorCalibrationComplete(&gyro.gyroSensor2); - } - case GYRO_CONFIG_USE_GYRO_BOTH: { - return isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2); - } -#endif } + + return true; } static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) @@ -187,10 +183,11 @@ void gyroStartCalibration(bool isFirstArmingCalibration) return; } - gyroSetCalibrationCycles(&gyro.gyroSensor1); -#ifdef USE_MULTI_GYRO - gyroSetCalibrationCycles(&gyro.gyroSensor2); -#endif + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) { + gyroSetCalibrationCycles(&gyro.gyroSensor[i]); + } + } if (isFirstArmingCalibration) { firstArmingCalibrationWasStarted = true; @@ -410,34 +407,27 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor) FAST_CODE void gyroUpdate(void) { - switch (gyro.gyroToUse) { - case GYRO_CONFIG_USE_GYRO_1: - gyroUpdateSensor(&gyro.gyroSensor1); - if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) { - gyro.gyroADC[X] = gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale; - gyro.gyroADC[Y] = gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale; - gyro.gyroADC[Z] = gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale; + // ensure that gyroADC don't contain a stale value + float adcSum[XYZ_AXIS_COUNT] = {0}; + + float active = 0; + + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) { + gyroUpdateSensor(&gyro.gyroSensor[i]); + if (isGyroSensorCalibrationComplete(&gyro.gyroSensor[i])) { + adcSum[X] += gyro.gyroSensor[i].gyroDev.gyroADC.x * gyro.gyroSensor[i].gyroDev.scale; + adcSum[Y] += gyro.gyroSensor[i].gyroDev.gyroADC.y * gyro.gyroSensor[i].gyroDev.scale; + adcSum[Z] += gyro.gyroSensor[i].gyroDev.gyroADC.z * gyro.gyroSensor[i].gyroDev.scale; + active++; + } } - break; -#ifdef USE_MULTI_GYRO - case GYRO_CONFIG_USE_GYRO_2: - gyroUpdateSensor(&gyro.gyroSensor2); - if (isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) { - gyro.gyroADC[X] = gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale; - gyro.gyroADC[Y] = gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale; - gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale; - } - break; - case GYRO_CONFIG_USE_GYRO_BOTH: - gyroUpdateSensor(&gyro.gyroSensor1); - gyroUpdateSensor(&gyro.gyroSensor2); - if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) { - gyro.gyroADC[X] = ((gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)) / 2.0f; - gyro.gyroADC[Y] = ((gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)) / 2.0f; - gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)) / 2.0f; - } - break; -#endif + } + + if (active != 0) { + gyro.gyroADC[X] = adcSum[X] / active; + gyro.gyroADC[Y] = adcSum[Y] / active; + gyro.gyroADC[Z] = adcSum[Z] / active; } if (gyro.downsampleFilterEnabled) { @@ -485,36 +475,24 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs) #endif if (gyro.useDualGyroDebugging) { - switch (gyro.gyroToUse) { - case GYRO_CONFIG_USE_GYRO_1: - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale)); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale)); - break; + int debugIndex = 0; -#ifdef USE_MULTI_GYRO - case GYRO_CONFIG_USE_GYRO_2: - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)); - break; + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) { + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0 + debugIndex, gyro.gyroSensor[i].gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1 + debugIndex, gyro.gyroSensor[i].gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0 + debugIndex, lrintf(gyro.gyroSensor[i].gyroDev.gyroADC.x * gyro.gyroSensor[i].gyroDev.scale)); + DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1 + debugIndex, lrintf(gyro.gyroSensor[i].gyroDev.gyroADC.y * gyro.gyroSensor[i].gyroDev.scale)); + // grab the difference between each gyro and the fused gyro output, gyro.gyroADCf (it hasn't had filters applied yet) + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0 + debugIndex, lrintf((gyro.gyroSensor[i].gyroDev.gyroADC.x * gyro.gyroSensor[i].gyroDev.scale) - gyro.gyroADCf[0])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1 + debugIndex, lrintf((gyro.gyroSensor[i].gyroDev.gyroADC.y * gyro.gyroSensor[i].gyroDev.scale) - gyro.gyroADCf[1])); - case GYRO_CONFIG_USE_GYRO_BOTH: - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale)); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale)); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)); - DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)); - DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale))); - DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale))); - DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale))); - break; -#endif + debugIndex += 2; + if (debugIndex >= 8) { + break; + // only iterate over the first 4 enabled gyros, debug can't hold more + } + } } } @@ -556,21 +534,15 @@ static int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor) void gyroReadTemperature(void) { - switch (gyro.gyroToUse) { - case GYRO_CONFIG_USE_GYRO_1: - gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor1); - break; + int16_t max_temp = INT16_MIN; -#ifdef USE_MULTI_GYRO - case GYRO_CONFIG_USE_GYRO_2: - gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor2); - break; - - case GYRO_CONFIG_USE_GYRO_BOTH: - gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyro.gyroSensor1), gyroReadSensorTemperature(gyro.gyroSensor2)); - break; -#endif // USE_MULTI_GYRO + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) { + max_temp = MAX(max_temp, gyroReadSensorTemperature(gyro.gyroSensor[i])); + } } + + gyroSensorTemperature = max_temp; } int16_t gyroGetTemperature(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 8af82bebfa..522f2d7358 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -51,6 +51,8 @@ #define GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ 200 +#define GYRO_MASK(x) BIT(x) + typedef union gyroLowpassFilter_u { pt1Filter_t pt1FilterState; biquadFilter_t biquadFilterState; @@ -58,16 +60,6 @@ typedef union gyroLowpassFilter_u { pt3Filter_t pt3FilterState; } gyroLowpassFilter_t; -typedef enum gyroDetectionFlags_e { - GYRO_NONE_MASK = 0, - GYRO_1_MASK = BIT(0), -#if defined(USE_MULTI_GYRO) - 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; - typedef struct gyroCalibration_s { float sum[XYZ_AXIS_COUNT]; stdev_t var[XYZ_AXIS_COUNT]; @@ -90,10 +82,7 @@ typedef struct gyro_s { float sampleSum[XYZ_AXIS_COUNT]; // summed samples used for downsampling bool downsampleFilterEnabled; // if true then downsample using gyro lowpass 2, otherwise use averaging - gyroSensor_t gyroSensor1; -#ifdef USE_MULTI_GYRO - gyroSensor_t gyroSensor2; -#endif + gyroSensor_t gyroSensor[GYRO_COUNT]; gyroDev_t *rawSensorDev; // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW @@ -113,7 +102,7 @@ typedef struct gyro_s { biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; uint16_t accSampleRateHz; - uint8_t gyroToUse; + uint8_t gyroEnabledBitmask; uint8_t gyroDebugMode; bool gyroHasOverflowProtection; bool useDualGyroDebugging; @@ -155,10 +144,6 @@ typedef enum { YAW_SPIN_RECOVERY_AUTO } yawSpinRecoveryMode_e; -#define GYRO_CONFIG_USE_GYRO_1 0 -#define GYRO_CONFIG_USE_GYRO_2 1 -#define GYRO_CONFIG_USE_GYRO_BOTH 2 - enum { FILTER_LPF1 = 0, FILTER_LPF2 @@ -168,7 +153,6 @@ 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. uint8_t gyro_hardware_lpf; // gyro DLPF setting uint8_t gyro_high_fsr; - uint8_t gyro_to_use; uint16_t gyro_lpf1_static_hz; uint16_t gyro_lpf2_static_hz; @@ -194,10 +178,12 @@ typedef struct gyroConfig_s { uint8_t gyro_filter_debug_axis; - uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup. + uint8_t gyrosDetected; // What gyros should be shown as part of the cli status command. uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter uint8_t simplified_gyro_filter; uint8_t simplified_gyro_filter_multiplier; + + uint8_t gyro_enabled_bitmask; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 7a2de5f353..957762f765 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -73,18 +73,12 @@ #include "gyro_init.h" -#ifdef USE_MULTI_GYRO -#define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1) -#else -#define ACTIVE_GYRO (&gyro.gyroSensor1) -#endif - // The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer // This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other // gyro types are supported with SPI DMA. #define GYRO_BUF_SIZE 32 -static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK; +static uint8_t gyroDetectedFlags = 0; static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz) { @@ -579,10 +573,9 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config) void gyroPreInit(void) { - gyroPreInitSensor(gyroDeviceConfig(0)); -#ifdef USE_MULTI_GYRO - gyroPreInitSensor(gyroDeviceConfig(1)); -#endif + for (int i = 0; i < GYRO_COUNT; i++) { + gyroPreInitSensor(gyroDeviceConfig(i)); + } } bool gyroInit(void) @@ -617,95 +610,104 @@ bool gyroInit(void) break; } - gyroDetectionFlags = GYRO_NONE_MASK; + gyroDetectedFlags = 0; uint8_t gyrosToScan = gyroConfig()->gyrosDetected; + // scan all gyros if gyrosDetected is zero + if (gyrosToScan == 0) { + gyrosToScan = 0xff; + } + // always scan gyro_enabled_bitmask + gyrosToScan |= gyroConfig()->gyro_enabled_bitmask; - gyro.gyroToUse = gyroConfig()->gyro_to_use; gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis; - if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) { - gyroDetectionFlags |= GYRO_1_MASK; + for (int i = 0; i < GYRO_COUNT; i++) { + // Only attempt to detect a gyro if it's enabled or we're doing an auto-scan + if (gyrosToScan & GYRO_MASK(i)) { + if (gyroDetectSensor(&gyro.gyroSensor[i], gyroDeviceConfig(i))) { + // If we detected a gyro, make sure it's in the enabled bitmask + // This ensures that during first boot, all detected gyros are enabled + gyroDetectedFlags |= GYRO_MASK(i); + } + } } -#if defined(USE_MULTI_GYRO) - if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) { - gyroDetectionFlags |= GYRO_2_MASK; - } -#endif - - if (gyroDetectionFlags == GYRO_NONE_MASK) { + if (gyroDetectedFlags == 0) { return false; } bool eepromWriteRequired = false; - if (!gyrosToScan) { - gyroConfigMutable()->gyrosDetected = gyroDetectionFlags; + if (gyrosToScan != gyroConfigMutable()->gyrosDetected) { + gyroConfigMutable()->gyrosDetected = gyroDetectedFlags; eepromWriteRequired = true; } -#if defined(USE_MULTI_GYRO) - 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; + // check if all enabled sensors are detected + gyro.gyroEnabledBitmask = gyroDetectedFlags & gyroConfig()->gyro_enabled_bitmask; + + if (gyroConfigMutable()->gyro_enabled_bitmask != gyro.gyroEnabledBitmask) { + gyroConfigMutable()->gyro_enabled_bitmask = gyro.gyroEnabledBitmask; + eepromWriteRequired = true; + } + + // Only allow using multiple gyros simultaneously if they are the same hardware type. + // Or allow using if they have the same sample rate and scale. + bool gyro_hardware_compatible = true; + uint16_t gyro_sample_rate = 0; + float gyro_scale = 0.0f; + bool firstFound = false; + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) { + if (!firstFound) { + firstFound = true; + gyro_sample_rate = gyro.gyroSensor[i].gyroDev.gyroSampleRateHz; + gyro_scale = gyro.gyroSensor[i].gyroDev.scale; + } else if ((gyro_sample_rate != gyro.gyroSensor[i].gyroDev.gyroSampleRateHz) || + (gyro_scale != gyro.gyroSensor[i].gyroDev.scale)) { + gyro_hardware_compatible = false; + } } + } - gyroConfigMutable()->gyro_to_use = gyro.gyroToUse; + if (!gyro_hardware_compatible) { + // If the user enabled multiple IMU and they are not compatible types, then reset to using only the first IMU. + gyro.gyroEnabledBitmask = gyro.gyroEnabledBitmask & -gyro.gyroEnabledBitmask; + gyroConfigMutable()->gyro_enabled_bitmask = gyro.gyroEnabledBitmask; eepromWriteRequired = true; } - // Only allow using both gyros simultaneously if they are the same hardware type. - 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; - } + static DMA_DATA uint8_t gyroBuf[GYRO_COUNT][2][GYRO_BUF_SIZE / 2]; - if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE]; - // SPI DMA buffer required per device - gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2; - gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2]; + for (int i = 0; i < GYRO_COUNT; i++) { + if (gyroDetectedFlags & GYRO_MASK(i)) { // Only initialize detected gyros + // SPI DMA buffer required per device + gyro.gyroSensor[i].gyroDev.dev.txBuf = gyroBuf[i][0]; + gyro.gyroSensor[i].gyroDev.dev.rxBuf = gyroBuf[i][1]; - gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1)); - gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection; - detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware; + gyroInitSensor(&gyro.gyroSensor[i], gyroDeviceConfig(i)); + + gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection + && gyro.gyroSensor[i].gyroDev.gyroHasOverflowProtection; + } } -#endif if (eepromWriteRequired) { writeEEPROM(); } - if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) { - static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE]; - // SPI DMA buffer required per device - gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1; - gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2]; - gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0)); - gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection; - detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware; + // Use the first enabled gyro for our scale and raw sensor dev + int firstGyro = firstEnabledGyro(); + if (firstGyro >= 0) { + detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor[firstGyro].gyroDev.gyroHardware; + gyro.scale = gyro.gyroSensor[firstGyro].gyroDev.scale; + gyro.rawSensorDev = &gyro.gyroSensor[firstGyro].gyroDev; + } else { + // no gyros enabled + detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; + gyro.scale = 1.0f; + gyro.rawSensorDev = NULL; } - // Copy the sensor's scale to the high-level gyro object. If running in "BOTH" mode - // then logic above requires both sensors to be the same so we'll use sensor1's scale. - // This will need to be revised if we ever allow different sensor types to be used simultaneously. - // Likewise determine the appropriate raw data for use in DEBUG_GYRO_RAW - gyro.scale = gyro.gyroSensor1.gyroDev.scale; - gyro.rawSensorDev = &gyro.gyroSensor1.gyroDev; -#if defined(USE_MULTI_GYRO) - if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) { - gyro.scale = gyro.gyroSensor2.gyroDev.scale; - gyro.rawSensorDev = &gyro.gyroSensor2.gyroDev; - } -#endif - if (gyro.rawSensorDev) { gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz; gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz; @@ -717,9 +719,9 @@ bool gyroInit(void) return true; } -gyroDetectionFlags_t getGyroDetectionFlags(void) +uint8_t getGyroDetectedFlags(void) { - return gyroDetectionFlags; + return gyroDetectedFlags; } void gyroSetTargetLooptime(uint8_t pidDenom) @@ -736,34 +738,41 @@ void gyroSetTargetLooptime(uint8_t pidDenom) gyroDev_t *gyroActiveDev(void) { - return &ACTIVE_GYRO->gyroDev; + return gyro.rawSensorDev; } const mpuDetectionResult_t *gyroMpuDetectionResult(void) { - return &ACTIVE_GYRO->gyroDev.mpuDetectionResult; + return &gyro.rawSensorDev->mpuDetectionResult; } int16_t gyroRateDps(int axis) { - return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale); + return lrintf(gyro.gyroADCf[axis] / gyro.rawSensorDev->scale); } #ifdef USE_GYRO_REGISTER_DUMP -static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor) +static extDevice_t *gyroExtDevice(uint8_t idx) { -#ifdef USE_MULTI_GYRO - if (whichSensor == GYRO_CONFIG_USE_GYRO_2) { - return &gyro.gyroSensor2.gyroDev.dev; - } -#else - UNUSED(whichSensor); -#endif - return &gyro.gyroSensor1.gyroDev.dev; + return &gyro.gyroSensor[idx].gyroDev.dev; } uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg) { - return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg); + if (whichSensor < GYRO_COUNT) { + return mpuGyroReadRegister(gyroExtDevice(whichSensor), reg); + } else { + return 0; + } } #endif // USE_GYRO_REGISTER_DUMP + +int firstEnabledGyro(void) +{ + if (gyro.gyroEnabledBitmask != 0) { + return llog2(gyro.gyroEnabledBitmask & -gyro.gyroEnabledBitmask); + } else { + // no gyro is enabled + return -1; + } +} diff --git a/src/main/sensors/gyro_init.h b/src/main/sensors/gyro_init.h index eb06280747..3fba53065e 100644 --- a/src/main/sensors/gyro_init.h +++ b/src/main/sensors/gyro_init.h @@ -28,9 +28,10 @@ void gyroPreInit(void); bool gyroInit(void); void gyroInitFilters(void); void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config); -gyroDetectionFlags_t getGyroDetectionFlags(void); +uint8_t getGyroDetectedFlags(void); gyroDev_t *gyroActiveDev(void); struct mpuDetectionResult_s; const struct mpuDetectionResult_s *gyroMpuDetectionResult(void); int16_t gyroRateDps(int axis); uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); +int firstEnabledGyro(void); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ad68eee069..63ec5048eb 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -703,3 +703,4 @@ extern struct linker_symbol __config_end; #define USE_PIN_PULL_UP_DOWN #endif #endif // USE_PINIO + diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index ed96a9f48f..626ddb4e02 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -372,7 +372,6 @@ #define USE_AIRMODE_LPF #define USE_GYRO_DLPF_EXPERIMENTAL -#define USE_MULTI_GYRO #define USE_SENSOR_NAMES #define USE_UNCOMMON_MIXERS #define USE_SIGNATURE @@ -494,3 +493,66 @@ #if defined(USE_POSITION_HOLD) && !defined(USE_GPS) #error "USE_POSITION_HOLD requires USE_GPS to be defined" #endif + +// backwards compatibility for older config.h targets +#ifndef GYRO_CONFIG_USE_GYRO_1 +#define GYRO_CONFIG_USE_GYRO_1 0 +#endif + +#ifndef GYRO_CONFIG_USE_GYRO_2 +#define GYRO_CONFIG_USE_GYRO_2 1 +#endif + +#ifndef GYRO_CONFIG_USE_GYRO_BOTH +#define GYRO_CONFIG_USE_GYRO_BOTH 2 +#endif + +#ifdef DEFAULT_GYRO_TO_USE + #ifndef DEFAULT_GYRO_ENABLED + #if DEFAULT_GYRO_TO_USE == GYRO_CONFIG_USE_GYRO_1 + #define DEFAULT_GYRO_ENABLED GYRO_MASK(0) + #elif DEFAULT_GYRO_TO_USE == GYRO_CONFIG_USE_GYRO_2 + #define DEFAULT_GYRO_ENABLED GYRO_MASK(1) + #elif DEFAULT_GYRO_TO_USE == GYRO_CONFIG_USE_GYRO_BOTH + #define DEFAULT_GYRO_ENABLED (GYRO_MASK(0) | GYRO_MASK(1)) + #endif + #endif +#elif !defined(DEFAULT_GYRO_ENABLED) + // assume one gyro + #define DEFAULT_GYRO_ENABLED GYRO_MASK(0) +#endif + +#ifndef GYRO_COUNT + #ifdef GYRO_1_CS_PIN + #define GYRO_1_DEFINED 1 + #else + #define GYRO_1_DEFINED 0 + #endif + + #ifdef GYRO_2_CS_PIN + #define GYRO_2_DEFINED 1 + #else + #define GYRO_2_DEFINED 0 + #endif + + #ifdef GYRO_3_CS_PIN + #define GYRO_3_DEFINED 1 + #else + #define GYRO_3_DEFINED 0 + #endif + + #ifdef GYRO_4_CS_PIN + #define GYRO_4_DEFINED 1 + #else + #define GYRO_4_DEFINED 0 + #endif + + #define GYRO_COUNT_RAW (GYRO_1_DEFINED + GYRO_2_DEFINED + GYRO_3_DEFINED + GYRO_4_DEFINED) + + // Ensure GYRO_COUNT is at least 1 + #if GYRO_COUNT_RAW > 0 + #define GYRO_COUNT GYRO_COUNT_RAW + #else + #define GYRO_COUNT 1 + #endif +#endif diff --git a/src/platform/APM32/include/platform/dma.h b/src/platform/APM32/include/platform/dma.h index 0b20062250..727a0d5741 100644 --- a/src/platform/APM32/include/platform/dma.h +++ b/src/platform/APM32/include/platform/dma.h @@ -32,7 +32,8 @@ typedef enum { DMA_NONE = 0, - DMA1_ST0_HANDLER = 1, + DMA_FIRST_HANDLER = 1, + DMA1_ST0_HANDLER = DMA_FIRST_HANDLER, DMA1_ST1_HANDLER, DMA1_ST2_HANDLER, DMA1_ST3_HANDLER, @@ -55,7 +56,6 @@ typedef enum { #define DMA_DEVICE_INDEX(x) ((((x)-1) % 8)) #define DMA_OUTPUT_INDEX 0 #define DMA_OUTPUT_STRING "DMA%d Stream %d:" -#define DMA_INPUT_STRING "DMA%d_ST%d" #define DEFINE_DMA_CHANNEL(d, s, f) { \ .dma = d, \ diff --git a/src/platform/AT32/include/platform/dma.h b/src/platform/AT32/include/platform/dma.h index adb08d3843..a19cffbf00 100644 --- a/src/platform/AT32/include/platform/dma.h +++ b/src/platform/AT32/include/platform/dma.h @@ -32,7 +32,8 @@ typedef enum { DMA_NONE = 0, - DMA1_CH1_HANDLER = 1, + DMA_FIRST_HANDLER = 1, + DMA1_CH1_HANDLER = DMA_FIRST_HANDLER, DMA1_CH2_HANDLER, DMA1_CH3_HANDLER, DMA1_CH4_HANDLER, @@ -56,7 +57,6 @@ uint32_t dmaGetChannel(const uint8_t channel); #define DMA_OUTPUT_INDEX 0 #define DMA_OUTPUT_STRING "DMA%d Channel %d:" -#define DMA_INPUT_STRING "DMA%d_CH%d" #define DEFINE_DMA_CHANNEL(d, c, f) { \ .dma = d, \ diff --git a/src/platform/SIMULATOR/include/platform/dma.h b/src/platform/SIMULATOR/include/platform/dma.h index f199deeed6..5767ef5514 100644 --- a/src/platform/SIMULATOR/include/platform/dma.h +++ b/src/platform/SIMULATOR/include/platform/dma.h @@ -23,5 +23,6 @@ typedef enum { DMA_NONE = 0, + DMA_FIRST_HANDLER = DMA_NONE, DMA_LAST_HANDLER = DMA_NONE } dmaIdentifier_e; diff --git a/src/platform/STM32/include/platform/dma.h b/src/platform/STM32/include/platform/dma.h index fbd9f5f1cb..37b026b2a4 100644 --- a/src/platform/STM32/include/platform/dma.h +++ b/src/platform/STM32/include/platform/dma.h @@ -40,7 +40,8 @@ typedef enum { DMA_NONE = 0, - DMA1_ST0_HANDLER = 1, + DMA_FIRST_HANDLER = 1, + DMA1_ST0_HANDLER = DMA_FIRST_HANDLER, DMA1_ST1_HANDLER, DMA1_ST2_HANDLER, DMA1_ST3_HANDLER, @@ -63,7 +64,6 @@ typedef enum { #define DMA_DEVICE_INDEX(x) ((((x)-1) % 8)) #define DMA_OUTPUT_INDEX 0 #define DMA_OUTPUT_STRING "DMA%d Stream %d:" -#define DMA_INPUT_STRING "DMA%d_ST%d" #define DEFINE_DMA_CHANNEL(d, s, f) { \ .dma = d, \ @@ -101,7 +101,8 @@ void dmaMuxEnable(dmaIdentifier_e identifier, uint32_t dmaMuxId); typedef enum { DMA_NONE = 0, - DMA1_CH1_HANDLER = 1, + DMA_FIRST_HANDLER = 1, + DMA1_CH1_HANDLER = DMA_FIRST_HANDLER, DMA1_CH2_HANDLER, DMA1_CH3_HANDLER, DMA1_CH4_HANDLER, @@ -129,7 +130,8 @@ uint32_t dmaGetChannel(const uint8_t channel); typedef enum { DMA_NONE = 0, - DMA1_CH1_HANDLER = 1, + DMA_FIRST_HANDLER = 1, + DMA1_CH1_HANDLER = DMA_FIRST_HANDLER, DMA1_CH2_HANDLER, DMA1_CH3_HANDLER, DMA1_CH4_HANDLER, @@ -146,7 +148,6 @@ typedef enum { #define DMA_OUTPUT_INDEX 0 #define DMA_OUTPUT_STRING "DMA%d Channel %d:" -#define DMA_INPUT_STRING "DMA%d_CH%d" #define DEFINE_DMA_CHANNEL(d, c, f) { \ .dma = d, \ diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 8f59e69f76..04577831ec 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -125,6 +125,7 @@ typedef struct #define SPIDEV_COUNT 0 #define I2CDEV_COUNT 0 +#define GYRO_COUNT 1 #define WS2811_DMA_TC_FLAG (void *)1 #define WS2811_DMA_HANDLER_IDENTIFER 0