From 1db460f164cf8be4b7b7016b30db83ef6a6c7af4 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 16 Feb 2020 13:28:55 +0900 Subject: [PATCH] [G4] MCO support --- src/main/cli/settings.c | 9 +++++- src/main/drivers/mco.c | 64 +++++++++++++++++++++++++++++++++++++---- src/main/drivers/mco.h | 12 +++++++- src/main/fc/init.c | 13 ++++++++- src/main/pg/mco.c | 7 +---- src/main/pg/mco.h | 6 ++-- 6 files changed, 94 insertions(+), 17 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ce60b29fa29..fcb9119247d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -38,6 +38,7 @@ #include "drivers/dshot_command.h" #include "drivers/camera_control.h" #include "drivers/light_led.h" +#include "drivers/mco.h" #include "drivers/pinio.h" #include "drivers/sdio.h" #include "drivers/vtx_common.h" @@ -1556,7 +1557,13 @@ const clivalue_t valueTable[] = { #endif #endif #ifdef USE_MCO - { "mco2_on_pc9", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MCO_CONFIG, offsetof(mcoConfig_t, enabled[1]) }, +#ifdef STM32G4 + { "mco_on_pa8", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MCO_CONFIG, PG_ARRAY_ELEMENT_OFFSET(mcoConfig_t, 0, enabled) }, + { "mco_source", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, MCO_SOURCE_COUNT - 1 }, PG_MCO_CONFIG, PG_ARRAY_ELEMENT_OFFSET(mcoConfig_t, 0, source) }, + { "mco_divider", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, MCO_DIVIDER_COUNT - 1 }, PG_MCO_CONFIG, PG_ARRAY_ELEMENT_OFFSET(mcoConfig_t, 0, divider) }, +#else + { "mco2_on_pc9", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MCO_CONFIG, PG_ARRAY_ELEMENT_OFFSET(mcoConfig_t, 1, enabled) }, +#endif #endif #ifdef USE_RX_SPEKTRUM { "spektrum_spi_protocol", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, protocol) }, diff --git a/src/main/drivers/mco.c b/src/main/drivers/mco.c index 7273d6d166c..81d5bf67c02 100644 --- a/src/main/drivers/mco.c +++ b/src/main/drivers/mco.c @@ -27,26 +27,78 @@ #ifdef USE_MCO #include "drivers/io.h" +#include "drivers/mco.h" #include "pg/mco.h" -void mcoInit(const mcoConfig_t *mcoConfig) +#ifdef STM32G4 + +// Notes +// - MCO output stability +// MCO output should not be too high. +// For example +// it is required to use DIV4 for SYSCLK = 254MHz (derives 62.5MHz) +// it is required to use DIV2 for SYSCLK = 170MHz (derives 85MHz) +// +// - MCO frequenchy can be more flexible if PLLR is made configurable. + +const uint32_t mcoSources[MCO_SOURCE_COUNT] = { + RCC_MCO1SOURCE_NOCLOCK, + RCC_MCO1SOURCE_SYSCLK, + RCC_MCO1SOURCE_HSI, + RCC_MCO1SOURCE_HSE, + RCC_MCO1SOURCE_PLLCLK, // PLLR on G4 + RCC_MCO1SOURCE_LSI, + RCC_MCO1SOURCE_LSE, + RCC_MCO1SOURCE_HSI48, +}; + +const uint32_t mcoDividers[MCO_DIVIDER_COUNT] = { + RCC_MCO_DIV1, + RCC_MCO_DIV2, + RCC_MCO_DIV4, + RCC_MCO_DIV8, + RCC_MCO_DIV16, +}; +#endif + +void mcoConfigure(MCODevice_e device, const mcoConfig_t *config) { + if (!config->enabled) { + return; + } + + IO_t io; + +#if defined(STM32F4) || defined(STM32F7) // Only configure MCO2 with PLLI2SCLK as source for now. // Other MCO1 and other sources can easily be added. - // For all F4 and F7 varianets, MCO1 is on PA8 and MCO2 is on PC9. - if (mcoConfig->enabled[1]) { - IO_t io = IOGetByTag(DEFIO_TAG_E(PC9)); + switch(device) { + case MCODEV_1: // MCO1 on PA8 + return; // Not supported (yet) + + case MCODEV_2: // MCO2 on PC9 + io = IOGetByTag(DEFIO_TAG_E(PC9)); IOInit(io, OWNER_MCO, 2); #if defined(STM32F7) HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_PLLI2SCLK, RCC_MCODIV_4); IOConfigGPIOAF(io, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_NOPULL), GPIO_AF0_MCO); -#elif defined(STM32F40_41xxx) || defined(STM32F411xE) || defined(STM32F446xx) +#else + // All F4s RCC_MCO2Config(RCC_MCO2Source_PLLI2SCLK, RCC_MCO2Div_4); IOConfigGPIOAF(io, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL), GPIO_AF_MCO); +#endif + break; + } +#elif defined(STM32G4) + // G4 only supports one MCO on PA8 + UNUSED(device); + + io = IOGetByTag(DEFIO_TAG_E(PA8)); + IOInit(io, OWNER_MCO, 1); + HAL_RCC_MCOConfig(RCC_MCO, mcoSources[config->source], mcoDividers[config->divider]); #else #error Unsupported MCU #endif - } } #endif diff --git a/src/main/drivers/mco.h b/src/main/drivers/mco.h index a9a5b06922b..72ced7e0e80 100644 --- a/src/main/drivers/mco.h +++ b/src/main/drivers/mco.h @@ -22,4 +22,14 @@ #include "pg/mco.h" -void mcoInit(const mcoConfig_t *mcoConfig); +typedef enum { + MCODEV_1 = 0, + MCODEV_2, +} MCODevice_e; + +#ifdef STM32G4 +#define MCO_SOURCE_COUNT 8 +#define MCO_DIVIDER_COUNT 5 +#endif + +void mcoConfigure(MCODevice_e device, const mcoConfig_t *config); diff --git a/src/main/fc/init.c b/src/main/fc/init.c index e3ca377eb3a..6d8abc15b1a 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -539,8 +539,19 @@ void init(void) // Configure MCO output after config is stable #ifdef USE_MCO - mcoInit(mcoConfig()); + // Note that mcoConfigure must be augmented with an additional argument to + // indicate which device instance to configure when MCO and MCO2 are both supported + +#if defined(STM32F4) || defined(STM32F7) + // F4 and F7 support MCO on PA8 and MCO2 on PC9, but only MCO2 is supported for now + mcoConfigure(MCODEV_2, mcoConfig(MCODEV_2)); +#elif defined(STM32G4) + // G4 only supports one MCO on PA8 + mcoConfigure(MCODEV_1, mcoConfig(MCODEV_1)); +#else +#error Unsupported MCU #endif +#endif // USE_MCO #ifdef USE_TIMER timerInit(); // timer must be initialized before any channel is allocated diff --git a/src/main/pg/mco.c b/src/main/pg/mco.c index e95f3e9b740..66ca4e8f3e6 100644 --- a/src/main/pg/mco.c +++ b/src/main/pg/mco.c @@ -30,10 +30,5 @@ #include "pg/pg_ids.h" #include "pg/mco.h" -PG_REGISTER_WITH_RESET_TEMPLATE(mcoConfig_t, mcoConfig, PG_MCO_CONFIG, 0); - -PG_RESET_TEMPLATE(mcoConfig_t, mcoConfig, - .enabled[0] = 0, - .enabled[1] = 0, -); +PG_REGISTER_ARRAY(mcoConfig_t, 2, mcoConfig, PG_MCO_CONFIG, 0); #endif // USE_MCO diff --git a/src/main/pg/mco.h b/src/main/pg/mco.h index d4fa86e3e60..49fb641ed63 100644 --- a/src/main/pg/mco.h +++ b/src/main/pg/mco.h @@ -27,7 +27,9 @@ #include "drivers/io_types.h" typedef struct mcoConfig_s { - uint8_t enabled[2]; + uint8_t enabled; + uint8_t source; + uint8_t divider; } mcoConfig_t; -PG_DECLARE(mcoConfig_t, mcoConfig); +PG_DECLARE_ARRAY(mcoConfig_t, 2, mcoConfig);