From 7853782486a4b2080f6cc4069a3bbb61434fa2b8 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 3 Dec 2019 09:01:10 -0500 Subject: [PATCH] Move MSP/OSD/CMS initialization to be near the end of the init process Helps prevent the USB VCP device from being initialized until the firmware is ready to accept connections. Previously the USB device would be initialized early in the process and the firmware still had lots of initialization following until it would be ready to accept connections. May help prevent connection failures if attempting to connect as soon as the serial port appears. --- src/main/fc/init.c | 173 +++++++++++++++++++++++---------------------- 1 file changed, 87 insertions(+), 86 deletions(-) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 5ff9b09f7..e34943525 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -769,96 +769,10 @@ void init(void) imuInit(); - mspInit(); - mspSerialInit(); - failsafeInit(); rxInit(); -/* - * CMS, display devices and OSD - */ -#ifdef USE_CMS - cmsInit(); -#endif - -#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS))) - displayPort_t *osdDisplayPort = NULL; -#endif - -#if defined(USE_OSD) - //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets - - if (featureIsEnabled(FEATURE_OSD)) { - osdDisplayPortDevice_e device = osdConfig()->displayPortDevice; - - switch(device) { - - case OSD_DISPLAYPORT_DEVICE_AUTO: - FALLTHROUGH; - -#if defined(USE_FRSKYOSD) - // Test OSD_DISPLAYPORT_DEVICE_FRSKYOSD first, since an FC could - // have a builtin MAX7456 but also an FRSKYOSD connected to an - // uart. - case OSD_DISPLAYPORT_DEVICE_FRSKYOSD: - osdDisplayPort = frskyOsdDisplayPortInit(vcdProfile()->video_system); - if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) { - break; - } - FALLTHROUGH; -#endif - -#if defined(USE_MAX7456) - case OSD_DISPLAYPORT_DEVICE_MAX7456: - // If there is a max7456 chip for the OSD configured and detectd then use it. - osdDisplayPort = max7456DisplayPortInit(vcdProfile()); - if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MAX7456) { - break; - } - FALLTHROUGH; -#endif - -#if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) - case OSD_DISPLAYPORT_DEVICE_MSP: - osdDisplayPort = displayPortMspInit(); - if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MSP) { - break; - } - FALLTHROUGH; -#endif - - // Other device cases can be added here - - case OSD_DISPLAYPORT_DEVICE_NONE: - default: - break; - } - - // osdInit will register with CMS by itself. - osdInit(osdDisplayPort); - } -#endif // USE_OSD - -#if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) - // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device. - if (!osdDisplayPort) - cmsDisplayPortRegister(displayPortMspInit()); -#endif - -#ifdef USE_DASHBOARD - // Dashbord will register with CMS by itself. - if (featureIsEnabled(FEATURE_DASHBOARD)) { - dashboardInit(); - } -#endif - -#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL) - // Register the srxl Textgen telemetry sensor as a displayport device - cmsDisplayPortRegister(displayPortSrxlInit()); -#endif - #ifdef USE_GPS if (featureIsEnabled(FEATURE_GPS)) { gpsInit(); @@ -999,6 +913,93 @@ void init(void) statsInit(); #endif + // Initialize MSP + mspInit(); + mspSerialInit(); + +/* + * CMS, display devices and OSD + */ +#ifdef USE_CMS + cmsInit(); +#endif + +#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS))) + displayPort_t *osdDisplayPort = NULL; +#endif + +#if defined(USE_OSD) + //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets + + if (featureIsEnabled(FEATURE_OSD)) { + osdDisplayPortDevice_e device = osdConfig()->displayPortDevice; + + switch(device) { + + case OSD_DISPLAYPORT_DEVICE_AUTO: + FALLTHROUGH; + +#if defined(USE_FRSKYOSD) + // Test OSD_DISPLAYPORT_DEVICE_FRSKYOSD first, since an FC could + // have a builtin MAX7456 but also an FRSKYOSD connected to an + // uart. + case OSD_DISPLAYPORT_DEVICE_FRSKYOSD: + osdDisplayPort = frskyOsdDisplayPortInit(vcdProfile()->video_system); + if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) { + break; + } + FALLTHROUGH; +#endif + +#if defined(USE_MAX7456) + case OSD_DISPLAYPORT_DEVICE_MAX7456: + // If there is a max7456 chip for the OSD configured and detectd then use it. + osdDisplayPort = max7456DisplayPortInit(vcdProfile()); + if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MAX7456) { + break; + } + FALLTHROUGH; +#endif + +#if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT) + case OSD_DISPLAYPORT_DEVICE_MSP: + osdDisplayPort = displayPortMspInit(); + if (osdDisplayPort || device == OSD_DISPLAYPORT_DEVICE_MSP) { + break; + } + FALLTHROUGH; +#endif + + // Other device cases can be added here + + case OSD_DISPLAYPORT_DEVICE_NONE: + default: + break; + } + + // osdInit will register with CMS by itself. + osdInit(osdDisplayPort); + } +#endif // USE_OSD + +#if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) + // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device. + if (!osdDisplayPort) + cmsDisplayPortRegister(displayPortMspInit()); +#endif + +#ifdef USE_DASHBOARD + // Dashbord will register with CMS by itself. + if (featureIsEnabled(FEATURE_DASHBOARD)) { + dashboardInit(); + } +#endif + +#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL) + // Register the srxl Textgen telemetry sensor as a displayport device + cmsDisplayPortRegister(displayPortSrxlInit()); +#endif + setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME); tasksInit();