From e186a2d70921c985cddb67aa89041dee360f5d99 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Sun, 2 Apr 2017 01:26:49 +0900
Subject: [PATCH 1/3] Further fix CMS displayPort registration

1. Avoid OSD over MSP_DISPLAYPORT case (not ready yet).
2. osdInit to take care NULL displayPortToUse.
---
 src/main/fc/fc_init.c | 10 ++++------
 src/main/io/osd.c     |  3 +++
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index af9fdcc33a..36eca73556 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -444,18 +444,16 @@ void init(void)
 #if defined(USE_MAX7456)
         // if there is a max7456 chip for the OSD then use it, otherwise use MSP
         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(osdDisplayPort);
     }
 #endif
 
-#if defined(USE_MSP_DISPLAYPORT) && defined(CMS)
-    // If BFOSD is active, then register it as CMS device, else register MSP.
-    if (osdDisplayPort)
-        cmsDisplayPortRegister(osdDisplayPort);
-    else
+#if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
+    // If BFOSD is not active, then register as register MSP_DISPLAYPORT.
+    if (!osdDisplayPort)
         cmsDisplayPortRegister(displayPortMspInit());
 #endif
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 995fea8c3f..b24010caef 100644
--- a/src/main/io/osd.c
+++ b/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;

From 3df4a84c1cbc72d7d8e2f7e9a59ae63c20186712 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Sun, 2 Apr 2017 11:04:52 +0900
Subject: [PATCH 2/3] comment tidy

---
 src/main/fc/fc_init.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 36eca73556..143c655921 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -442,7 +442,7 @@ 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_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
         osdDisplayPort = displayPortMspInit();
@@ -452,7 +452,7 @@ void init(void)
 #endif
 
 #if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
-    // If BFOSD is not active, then register as register MSP_DISPLAYPORT.
+    // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
     if (!osdDisplayPort)
         cmsDisplayPortRegister(displayPortMspInit());
 #endif

From 5d92b62393f5a0e790e4dfd427a35cf0ae596564 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Sun, 2 Apr 2017 22:42:35 +0900
Subject: [PATCH 3/3] Resurrect multi-display cycling

---
 src/main/cms/cms.c    |  5 +++++
 src/main/fc/fc_init.c | 48 ++++++++++++++++++++++++++-----------------
 2 files changed, 34 insertions(+), 19 deletions(-)

diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index a741656657..f7e106a3e1 100644
--- a/src/main/cms/cms.c
+++ b/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;
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 143c655921..7a346321ec 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -378,25 +378,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);
@@ -435,6 +416,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
+
     displayPort_t *osdDisplayPort = NULL;
 
 #ifdef OSD
@@ -447,6 +448,7 @@ void init(void)
 #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
@@ -457,6 +459,14 @@ void init(void)
         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();