[G4] MCO support

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) },

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

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);

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

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

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);