diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 2d4dec5588..e4ab591cbd 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -878,12 +878,6 @@ void init(void) cmsInit(); #endif -#ifdef USE_TELEMETRY - if (featureIsEnabled(FEATURE_TELEMETRY)) { - telemetryInit(); - } -#endif - #if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS))) displayPort_t *osdDisplayPort = NULL; osdDisplayPortDevice_e osdDisplayPortDevice = OSD_DISPLAYPORT_DEVICE_NONE; @@ -969,6 +963,13 @@ void init(void) } #endif +#ifdef USE_TELEMETRY + // Telemetry will initialise displayport and register with CMS by itself. + if (featureIsEnabled(FEATURE_TELEMETRY)) { + telemetryInit(); + } +#endif + setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME); // On F4/F7 allocate SPI DMA streams before motor timers