Merge pull request #2800 from jflyper/bfdef-cms-resurrect-display-cycling

[CMS] Resurrect display cycling

src/main/cms/cms.c

@@ -629,6 +629,11 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
     if (!currentMenu)
         return res;
 
+    if (key == KEY_MENU) {
+        cmsMenuOpen();
+        return BUTTON_PAUSE;
+    }
+
     if (key == KEY_ESC) {
         cmsMenuBack(pDisplay);
         return BUTTON_PAUSE;

src/main/fc/fc_init.c

@@ -382,25 +382,6 @@ void init(void)
 
     initBoardAlignment(boardAlignment());
 
-#ifdef CMS
-    cmsInit();
-#endif
-
-#ifdef USE_DASHBOARD
-    if (feature(FEATURE_DASHBOARD)) {
-        dashboardInit();
-    }
-#endif
-
-#ifdef USE_RTC6705
-    if (feature(FEATURE_VTX)) {
-        rtc6705_soft_spi_init();
-        current_vtx_channel = vtxConfig()->vtx_channel;
-        rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
-        rtc6705_soft_spi_set_rf_power(vtxConfig()->vtx_power);
-    }
-#endif
-
     if (!sensorsAutodetect()) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
@@ -439,6 +420,26 @@ void init(void)
 
     rxInit();
 
+/*
+ * VTX
+ */
+
+#ifdef USE_RTC6705
+    if (feature(FEATURE_VTX)) {
+        rtc6705_soft_spi_init();
+        current_vtx_channel = vtxConfig()->vtx_channel;
+        rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
+        rtc6705_soft_spi_set_rf_power(vtxConfig()->vtx_power);
+    }
+#endif
+
+/*
+ * CMS, display devices and OSD
+ */
+#ifdef CMS
+    cmsInit();
+#endif
+
 #if ( defined(OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(CMS)) )
     displayPort_t *osdDisplayPort = NULL;
 #endif
@@ -448,23 +449,30 @@ void init(void)
 
     if (feature(FEATURE_OSD)) {
 #if defined(USE_MAX7456)
-        // if there is a max7456 chip for the OSD then use it, otherwise use MSP
+        // If there is a max7456 chip for the OSD then use it
         osdDisplayPort = max7456DisplayPortInit(vcdProfile());
-#elif defined(USE_MSP_DISPLAYPORT)
+#elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
         osdDisplayPort = displayPortMspInit();
 #endif
+        // osdInit  will register with CMS by itself.
         osdInit(osdDisplayPort);
     }
 #endif
 
-#if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
+#if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
-    // If BFOSD is active, then register it as CMS device, else register MSP.
+    // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
-    if (osdDisplayPort)
+    if (!osdDisplayPort)
-        cmsDisplayPortRegister(osdDisplayPort);
-    else
         cmsDisplayPortRegister(displayPortMspInit());
 #endif
 
+#ifdef USE_DASHBOARD
+    // Dashbord will register with CMS by itself.
+    if (feature(FEATURE_DASHBOARD)) {
+        dashboardInit();
+    }
+#endif
+
+
 #ifdef GPS
     if (feature(FEATURE_GPS)) {
         gpsInit();

src/main/io/osd.c

@@ -582,6 +582,9 @@ static void osdDrawLogo(int x, int y)
 
 void osdInit(displayPort_t *osdDisplayPortToUse)
 {
+    if (!osdDisplayPortToUse)
+        return;
+
     BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
 
     osdDisplayPort = osdDisplayPortToUse;