diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index 1432b6882b..60c2f517c6 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -55,6 +55,7 @@ typedef enum {
     FEATURE_VTX = 1 << 24,
     FEATURE_RX_SPI = 1 << 25,
     FEATURE_SOFTSPI = 1 << 26,
+    FEATURE_CANVAS = 1 << 27,
 } features_e;
 
 void beeperOffSet(uint32_t mask);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 4edd2c4a6e..4f35eb4cb4 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -101,15 +101,6 @@
 #include "config/config_master.h"
 #include "config/feature.h"
 
-#ifdef USE_DPRINTF
-#include "common/printf.h"
-#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
-extern serialPort_t *debugSerialPort;
-#define dprintf(x) if (debugSerialPort) printf x
-#else
-#define dprintf(x)
-#endif
-
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c
index db80e003e0..97b12f03d9 100644
--- a/src/main/io/canvas.c
+++ b/src/main/io/canvas.c
@@ -68,7 +68,6 @@ int canvasWrite(uint8_t col, uint8_t row, char *string)
 }
 
 screenFnVTable_t canvasVTable = {
-    canvasGetDevParam,
     canvasBegin,
     canvasEnd,
     canvasClear,
@@ -77,18 +76,19 @@ screenFnVTable_t canvasVTable = {
     NULL,
 };
 
-displayPort_t canvasDisplayPort = {
-    .rows = 13,
-    .cols = 30,
-    .pBuftime = 23,          // = 256/(115200/10)
-    .pBufsize = 192,         // 256 * 3/4 (Be conservative)
-    .VTable = canvasVTable,
-};
+void canvasCmsInit(displayPort_t *pPort)
+{
+    pPort->rows = 13;
+    pPort->cols = 30;
+    pPort->buftime = 23;          // = 256/(115200/10)
+    pPort->bufsize = 192;         // 256 * 3/4 (Be conservative)
+    pPort->VTable = &canvasVTable;
+}
 
-displayPort_t *canvasInit(void)
+void canvasInit()
 {
     mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp
 
-    return &canvasDisplayPort;
+    cmsDeviceRegister(canvasCmsInit);
 }
 #endif
diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h
index f6a574f03f..c7f96207ca 100644
--- a/src/main/io/canvas.h
+++ b/src/main/io/canvas.h
@@ -1,3 +1,3 @@
 #pragma once
-
-displayPort_t *canvasInit(void);
+void canvasInit(void);
+void canvasCmsInit(displayPort_t *);
diff --git a/src/main/io/cms.c b/src/main/io/cms.c
index 1580cbd0d5..e83ac375f9 100644
--- a/src/main/io/cms.c
+++ b/src/main/io/cms.c
@@ -37,7 +37,9 @@
 
 #include "io/cms.h"
 #include "io/cms_types.h"
+#ifdef CANVAS
 #include "io/canvas.h"
+#endif
 
 #include "io/flashfs.h"
 #include "io/osd.h"
@@ -57,9 +59,50 @@
 
 #include "build/debug.h"
 
+// Device management
+
+#define CMS_MAX_DEVICE 4
+
+cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE];
+int cmsDeviceCount;
+int cmsCurrentDevice = -1;
+int cmsLastDevice = -1;
+
+bool cmsDeviceRegister(cmsDeviceInitFuncPtr func)
+{
+    if (cmsDeviceCount == CMS_MAX_DEVICE)
+        return false;
+
+    cmsDeviceInitFunc[cmsDeviceCount++] = func;
+
+    return true;
+}
+
+cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
+{
+    if (cmsDeviceCount == 0)
+        return NULL;
+
+    if (cmsCurrentDevice < 0)
+        cmsCurrentDevice = 0;
+
+    return cmsDeviceInitFunc[cmsCurrentDevice];
+}
+
+cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
+{
+    if (cmsDeviceCount == 0)
+        return NULL;
+
+    cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay
+
+    return cmsDeviceInitFunc[cmsCurrentDevice];
+}
+
 #define CMS_UPDATE_INTERVAL 50 // msec
 
-// XXX Why is this here? Something is wrong?
+// XXX Why is this here? Something wrong?
+// XXX We need something like Drawing Context that holds all state variables?
 int8_t lastCursorPos;
 
 void cmsScreenClear(displayPort_t *instance)
@@ -97,21 +140,9 @@ void cmsScreenResync(displayPort_t *instance)
         instance->VTable->resync();
 }
 
-displayPort_t *cmsScreenInit(void)
+void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
 {
-#ifdef OSD
-    return osdCmsInit();
-#endif
-
-#ifdef CANVAS
-    return canvasCmsInit();
-#endif
-
-#ifdef OLEDCMS
-    return displayCmsInit();
-#endif
-
-    return NULL;
+    cmsDeviceInitFunc(pDisp);
 }
 
 
@@ -407,38 +438,31 @@ void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime)
 
 // XXX Needs separation
 OSD_Entry menuPid[];
+void cmsx_PidRead(void);
+void cmsx_PidWriteback(void);
 OSD_Entry menuRateExpo[];
-uint8_t tempPid[4][3];
-controlRateConfig_t rateProfile;
+void cmsx_RateExpoRead(void);
+void cmsx_RateExpoWriteback(void);
 
 void cmsMenuChange(displayPort_t *pDisplay, void *ptr)
 {
-    uint8_t i;
     if (ptr) {
-        // hack - save profile to temp
-        // XXX (jflyper) This hack could be avoided by adding pre- and post-
-        // (onEntry and onExit?) functions to OSD_Entry, that are called
-        // before and after the menu item is displayed.
-        if (ptr == &menuPid[0]) {
-            for (i = 0; i < 3; i++) {
-                tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i];
-                tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i];
-                tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i];
-            }
-            tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL];
-            tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL];
-            tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL];
-        }
-
+        // XXX (jflyper): This can be avoided by adding pre- and post-
+        // XXX (or onEnter and onExit) functions?
+        if (ptr == &menuPid[0])
+            cmsx_PidRead();
         if (ptr == &menuRateExpo[0])
-            memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t));
+            cmsx_RateExpoRead();
 
-        menuStack[menuStackIdx] = currentMenu;
-        menuStackHistory[menuStackIdx] = currentCursorPos;
-        menuStackIdx++;
-
-        currentMenu = (OSD_Entry *)ptr;
-        currentCursorPos = 0;
+        if ((OSD_Entry *)ptr != currentMenu) {
+            // Stack it and move to a new menu.
+            // (ptr == curretMenu case occurs when reopening for display sw)
+            menuStack[menuStackIdx] = currentMenu;
+            menuStackHistory[menuStackIdx] = currentCursorPos;
+            menuStackIdx++;
+            currentMenu = (OSD_Entry *)ptr;
+            currentCursorPos = 0;
+        }
         cmsScreenClear(pDisplay);
         cmsUpdateMaxRows(pDisplay);
     }
@@ -446,25 +470,14 @@ void cmsMenuChange(displayPort_t *pDisplay, void *ptr)
 
 void cmsMenuBack(displayPort_t *pDisplay)
 {
-    uint8_t i;
-
     // becasue pids and rates meybe stored in profiles we need some thicks to manipulate it
     // hack to save pid profile
-    if (currentMenu == &menuPid[0]) {
-        for (i = 0; i < 3; i++) {
-            masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0];
-            masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1];
-            masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2];
-        }
-
-        masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0];
-        masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1];
-        masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2];
-    }
+    if (currentMenu == &menuPid[0])
+        cmsx_PidWriteback();
 
     // hack - save rate config for current profile
     if (currentMenu == &menuRateExpo[0])
-        memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t));
+        cmsx_RateExpoWriteback();
 
     if (menuStackIdx) {
         cmsScreenClear(pDisplay);
@@ -477,29 +490,43 @@ void cmsMenuBack(displayPort_t *pDisplay)
     }
 }
 
+// XXX This should go to device
+void cmsComputeBatchsize(displayPort_t *pDisplay)
+{
+    pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime;
+}
+
 // XXX Separation
 void cmsx_FeatureRead(void);
 void cmsx_FeatureWriteback(void);
 void cmsx_InfoInit(void);
 
-void cmsMenuOpen(displayPort_t *pDisplay)
+displayPort_t currentDisplay;
+
+void cmsMenuOpen(void)
 {
-    if (cmsInMenu)
+    cmsDeviceInitFuncPtr initfunc;
+
+    if (!cmsInMenu) {
+        // New open
+        cmsInMenu = true;
+        DISABLE_ARMING_FLAG(OK_TO_ARM);
+        initfunc = cmsDeviceSelectCurrent(); 
+        cmsx_FeatureRead();
+        currentMenu = &menuMain[0];
+    } else {
+        // Switch display
+        cmsScreenEnd(&currentDisplay);
+        initfunc = cmsDeviceSelectNext();
+    }
+
+    if (!initfunc)
         return;
 
-    cmsx_FeatureRead();
-
-    if (!pDisplay)
-        return;
-
-    pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime;
-
-    cmsInMenu = true;
-    DISABLE_ARMING_FLAG(OK_TO_ARM);
-
-    cmsScreenBegin(pDisplay);
-    currentMenu = &menuMain[0];
-    cmsMenuChange(pDisplay, currentMenu);
+    cmsScreenInit(&currentDisplay, initfunc);
+    cmsComputeBatchsize(&currentDisplay);
+    cmsScreenBegin(&currentDisplay);
+    cmsMenuChange(&currentDisplay, currentMenu);
 }
 
 void cmsMenuExit(displayPort_t *pDisplay, void *ptr)
@@ -715,19 +742,20 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
 
     uint8_t key = 0;
 
-    // Detect menu invocation
-    if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
-        // XXX Double enter!?
-        cmsMenuOpen(pDisplay);
-        rcDelay = BUTTON_PAUSE;    // Tends to overshoot if BUTTON_TIME
-        lastCalled = currentTime;
-        return;
-    }
-
-    if (cmsInMenu) {
+    if (!cmsInMenu) {
+        // Detect menu invocation
+        if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
+            cmsMenuOpen();
+            rcDelay = BUTTON_PAUSE;    // Tends to overshoot if BUTTON_TIME
+        }
+    } else {
         if (rcDelay > 0) {
-            rcDelay -= currentTime - lastCalled;
-            debug[0] = rcDelay;
+            rcDelay -= (currentTime - lastCalled);
+        }
+        else if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
+            // Double enter = display switching
+            cmsMenuOpen();
+            rcDelay = BUTTON_PAUSE;
         }
         else if (IS_HI(PITCH)) {
             key = KEY_UP;
@@ -751,10 +779,10 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
             rcDelay = BUTTON_TIME;
         }
 
-        lastCalled = currentTime;
+        //lastCalled = currentTime;
 
         if (key && !currentElement) {
-            rcDelay = cmsHandleKey(pDisplay, key);
+            rcDelay = cmsHandleKey(&currentDisplay, key);
             return;
         }
 
@@ -763,33 +791,31 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
         if (currentTime > lastCmsHeartBeat + 500) {
             // Heart beat for external CMS display device @ 500msec
             // (Timeout @ 1000msec)
-            cmsScreenHeartBeat(pDisplay);
+            cmsScreenHeartBeat(&currentDisplay);
             lastCmsHeartBeat = currentTime;
         }
     }
+    lastCalled = currentTime;
 }
 
-displayPort_t *currentDisplay = NULL;
-
 void cmsHandler(uint32_t unusedTime)
 {
     UNUSED(unusedTime);
 
-    if (!currentDisplay)
+    if (cmsDeviceCount < 0)
         return;
 
     static uint32_t lastCalled = 0;
     uint32_t now = millis();
 
     if (now - lastCalled >= CMS_UPDATE_INTERVAL) {
-        cmsUpdate(currentDisplay, now);
+        cmsUpdate(&currentDisplay, now);
         lastCalled = now;
     }
 }
 
 void cmsInit(void)
 {
-    currentDisplay = cmsScreenInit();
     cmsx_InfoInit();
 }
 
@@ -803,6 +829,8 @@ void cmsInit(void)
 
 OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1};
 
+uint8_t tempPid[4][3];
+
 static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1};
 static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1};
 static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1};
@@ -815,6 +843,35 @@ static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1};
 static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1};
 static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1};
 
+void cmsx_PidRead(void)
+{
+    uint8_t i;
+
+    for (i = 0; i < 3; i++) {
+        tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i];
+        tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i];
+        tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i];
+    }
+    tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL];
+    tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL];
+    tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL];
+}
+
+void cmsx_PidWriteback(void)
+{
+    uint8_t i;
+
+    for (i = 0; i < 3; i++) {
+        masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0];
+        masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1];
+        masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2];
+    }
+
+    masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0];
+    masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1];
+    masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2];
+}
+
 OSD_Entry menuPid[] =
 {
     {"--- PID ---", OME_Label, NULL, NULL, 0},
@@ -834,6 +891,11 @@ OSD_Entry menuPid[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
+//
+// Rate & Expo
+//
+controlRateConfig_t rateProfile;
+
 static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10};
 static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10};
 static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10};
@@ -846,6 +908,16 @@ static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10
 static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10};
 static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10};
 
+void cmsx_RateExpoRead()
+{
+    memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t));
+}
+
+void cmsx_RateExpoWriteback()
+{
+    memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t));
+}
+
 OSD_Entry menuRateExpo[] =
 {
     {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0},
@@ -931,6 +1003,29 @@ OSD_Entry menuImu[] =
 //
 // Black box
 //
+
+//
+// Should goto flashfs eventually.
+//
+#ifdef USE_FLASHFS
+void cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr)
+{
+    UNUSED(ptr);
+
+    cmsScreenClear(pDisplay);
+    cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH...");
+    cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing?
+
+    flashfsEraseCompletely();
+    while (!flashfsIsReady()) {
+        delay(100);
+    }
+
+    cmsScreenClear(pDisplay);
+    cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?
+}
+#endif // USE_FLASHFS
+
 uint8_t featureBlackbox = 0;
 
 OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1};
@@ -1056,28 +1151,6 @@ OSD_Entry menuLedstrip[] =
 };
 #endif // LED_STRIP
 
-//
-// Should goto flashfs eventually.
-//
-#ifdef USE_FLASHFS
-void cmsx_EraseFlash(void *ptr)
-{
-    UNUSED(ptr);
-
-    cmsScreenClear(currentDisplay);
-    cmsScreenWrite(currentDisplay5, 3, "ERASING FLASH...");
-    cmsScreenResync(currentDisplay); // Was max7456RefreshAll(); Why at this timing?
-
-    flashfsEraseCompletely();
-    while (!flashfsIsReady()) {
-        delay(100);
-    }
-
-    cmsScreenClear(currentDisplay);
-    cmsScreenResync(currentDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?
-}
-#endif // USE_FLASHFS
-
 #ifdef OSD
 //
 // OSD specific menu pages and items
@@ -1221,10 +1294,13 @@ void cmsx_FeatureWriteback(void)
     else
         featureClear(FEATURE_BLACKBOX);
 
+#ifdef LED_STRIP
     if (featureLedstrip)
         featureSet(FEATURE_LED_STRIP);
     else
         featureClear(FEATURE_LED_STRIP);
+#endif
+
 #if defined(VTX) || defined(USE_RTC6705)
     if (featureVtx)
         featureSet(FEATURE_VTX);
@@ -1244,4 +1320,5 @@ void cmsx_FeatureWriteback(void)
     saveConfigAndNotify();
 }
 
+
 #endif // CMS
diff --git a/src/main/io/cms.h b/src/main/io/cms.h
index 3228af396f..79ea1461ab 100644
--- a/src/main/io/cms.h
+++ b/src/main/io/cms.h
@@ -21,9 +21,15 @@ typedef struct displayPort_s {
     bool cleared;
 } displayPort_t;
 
+// Device management
+typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *);
+bool cmsDeviceRegister(cmsDeviceInitFuncPtr);
+
+// For main.c and scheduler
 void cmsInit(void);
 void cmsHandler(uint32_t);
 
+
 #if 0
 void cmsOpenMenu();
 void cmsUpdate(uint32_t);
diff --git a/src/main/io/display.c b/src/main/io/display.c
index 7e08ffd4bd..a620629932 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -707,6 +707,10 @@ void displayInit(rxConfig_t *rxConfigToUse)
     resetDisplay();
     delay(200);
 
+#ifdef CMS
+    cmsDeviceRegister(displayCmsInit);
+#endif
+
     rxConfig = rxConfigToUse;
 
     memset(&pageState, 0, sizeof(pageState));
@@ -747,28 +751,28 @@ void displayDisablePageCycling(void)
 #ifdef OLEDCMS
 #include "io/cms.h"
 
-int displayCMSBegin(void)
+int displayCmsBegin(void)
 {
     displayInCMS = true;
 
     return 0;
 }
 
-int displayCMSEnd(void)
+int displayCmsEnd(void)
 {
     displayInCMS = false;
 
     return 0;
 }
 
-int displayCMSClear(void)
+int displayCmsClear(void)
 {
     i2c_OLED_clear_display_quick();
 
     return 0;
 }
 
-int displayCMSWrite(uint8_t x, uint8_t y, char *s)
+int displayCmsWrite(uint8_t x, uint8_t y, char *s)
 {
     i2c_OLED_set_xy(x, y);
     i2c_OLED_send_string(s);
@@ -776,28 +780,23 @@ int displayCMSWrite(uint8_t x, uint8_t y, char *s)
     return 0;
 }
 
-screenFnVTable_t displayCMSVTable = {
-    displayCMSBegin,
-    displayCMSEnd,
-    displayCMSClear,
-    displayCMSWrite,
+screenFnVTable_t displayCmsVTable = {
+    displayCmsBegin,
+    displayCmsEnd,
+    displayCmsClear,
+    displayCmsWrite,
     NULL,
     NULL,
 };
 
-displayPort_t displayCMSDisplayPort = {
-    .rows = 8,
-    .cols = 21,
-    .buftime = 1,
-    .bufsize = 50000,
-    .VTable = &displayCMSVTable,
-};
-
-displayPort_t *displayCmsInit(void)
+void displayCmsInit(displayPort_t *pPort)
 {
-    return &displayCMSDisplayPort;
+    pPort->rows = 8;
+    pPort->cols = 21;
+    pPort->buftime = 1;
+    pPort->bufsize = 50000;
+    pPort->VTable = &displayCmsVTable;
 }
-
 #endif // OLEDCMS
 
 #endif
diff --git a/src/main/io/display.h b/src/main/io/display.h
index f9fd7e682e..e2c6c2b967 100644
--- a/src/main/io/display.h
+++ b/src/main/io/display.h
@@ -46,4 +46,6 @@ void displayDisablePageCycling(void);
 void displayResetPageCycling(void);
 void displaySetNextPageChangeAt(uint32_t futureMicros);
 
-displayPort_t *displayCmsInit(void);
+#ifdef CMS
+void displayCmsInit(displayPort_t *);
+#endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index c3954b7573..4b7df072e6 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -64,13 +64,6 @@
 
 #include "build/debug.h"
 
-#ifdef USE_DPRINTF
-extern serialPort_t *debugSerialPort;
-#define dprintf(x) if (debugSerialPort) printf x
-#else
-#define dprintf(x)
-#endif
-
 // Things in both OSD and CMS
 
 #define IS_HI(X)  (rcData[X] > 1750)
@@ -398,7 +391,7 @@ void osdInit(void)
     }
 
 #ifdef CMS
-    cmsInit();
+    cmsDeviceRegister(osdCmsInit);
 #endif
 
     sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING);
@@ -709,16 +702,12 @@ screenFnVTable_t osdVTable = {
     max7456RefreshAll,
 };
 
-displayPort_t osdDisplayPort = {
-    .buftime = 1,         // Very fast
-    .bufsize = 50000,     // Very large
-    .VTable = &osdVTable,
-};
-
-displayPort_t *osdCmsInit(void)
+void osdCmsInit(displayPort_t *pPort)
 {
-    osdDisplayPort.rows = max7456GetRowsCount();
-    osdDisplayPort.cols = 30;
-    return &osdDisplayPort;
+    pPort->rows = max7456GetRowsCount();
+    pPort->cols = 30;
+    pPort->buftime = 1;         // Very fast
+    pPort->bufsize = 50000;     // Very large
+    pPort->VTable = &osdVTable;
 }
 #endif // OSD
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 8fdf99dd00..09d3a21843 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -66,7 +66,10 @@ typedef struct {
 void updateOsd(uint32_t currentTime);
 void osdInit(void);
 void resetOsdConfig(osd_profile_t *osdProfile);
-displayPort_t *osdCmsInit(void);
+
+#ifdef CMS
+void osdCmsInit(displayPort_t *);
+#endif
 
 // Character coordinate and attributes
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index a07f72bf36..a9ccf40404 100755
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -227,7 +227,7 @@ static const char * const featureNames[] = {
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
     "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
-    " ", "VTX", "RX_SPI", "SOFTSPI", NULL
+    " ", "VTX", "RX_SPI", "SOFTSPI", "CANVAS", NULL
 };
 
 // sync this with rxFailsafeChannelMode_e
diff --git a/src/main/main.c b/src/main/main.c
index dac4339be3..618409f3a9 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -91,6 +91,7 @@
 #include "io/cms.h"
 #include "io/osd.h"
 #include "io/vtx.h"
+#include "io/canvas.h"
 
 #include "scheduler/scheduler.h"
 
@@ -398,6 +399,10 @@ void init(void)
 
     initBoardAlignment(&masterConfig.boardAlignment);
 
+#ifdef CMS
+    cmsInit();
+#endif
+
 #ifdef DISPLAY
     if (feature(FEATURE_DISPLAY)) {
         displayInit(&masterConfig.rxConfig);
@@ -419,10 +424,6 @@ void init(void)
     }
 #endif
 
-#ifdef CMS
-    cmsInit();
-#endif
-
     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
             masterConfig.acc_hardware,
             masterConfig.mag_hardware,
@@ -461,6 +462,12 @@ void init(void)
     mspFcInit();
     mspSerialInit();
 
+#ifdef CANVAS
+    if (feature(FEATURE_CANVAS)) {
+        canvasInit();
+    }
+#endif
+
 #ifdef USE_CLI
     cliInit(&masterConfig.serialConfig);
 #endif
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index 259fde884b..9bc30bf941 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -96,10 +96,10 @@
 #define CMS
 
 // Use external OSD to run CMS
-//#define CANVAS
+#define CANVAS
 
 // USE I2C OLED display to run CMS
-//#define OLEDCMS
+#define OLEDCMS
 
 // OSD define info:
 //   feature name (includes source) -> MAX_OSD, used in target.mk
diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk
index 01d31c3708..4ec2d2df37 100644
--- a/src/main/target/OMNIBUS/target.mk
+++ b/src/main/target/OMNIBUS/target.mk
@@ -15,4 +15,5 @@ TARGET_SRC = \
             io/transponder_ir.c \
             drivers/max7456.c \
             io/osd.c \
-            io/cms.c
+            io/cms.c \
+            io/canvas.c
diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h
index e4e23315b6..9a9f16e331 100644
--- a/src/main/target/SIRINFPV/target.h
+++ b/src/main/target/SIRINFPV/target.h
@@ -138,8 +138,16 @@
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 #define OSD
+
+// Configuratoin Menu System
 #define CMS
 
+// Use external OSD to run CMS
+#define CANVAS
+
+// USE I2C OLED display to run CMS
+#define OLEDCMS
+
 #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
 
 #define USE_SERVOS
diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk
index 2597e6e977..6481f94182 100644
--- a/src/main/target/SIRINFPV/target.mk
+++ b/src/main/target/SIRINFPV/target.mk
@@ -13,4 +13,5 @@ TARGET_SRC = \
             drivers/vtx_soft_spi_rtc6705.c \
             drivers/max7456.c \
             io/osd.c \
-            io/cms.c
+            io/cms.c \
+            io/canvas.c
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 6ab414085a..0ded226cf6 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -140,4 +140,4 @@
 #define CANVAS
 
 // USE I2C OLED display to run CMS
-//#define OLEDCMS
+#define OLEDCMS