diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 2d8a8193b0..c69738c7fa 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -102,15 +102,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
@@ -1946,16 +1937,7 @@ mspResult_e mspFcProcessCommand(mspPort_t *mspPort, mspPostProcessFnPtr *mspPost
     return ret;
 }
 
-/*
- * Return a pointer to the process command function
- */
-mspProcessCommandFnPtr mspFcInit(void)
-{
-    initActiveBoxIds();
-    return mspFcProcessCommand;
-}
-
-void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len)
+void mspServerPush(mspPort_t *mspPort, uint8_t cmd, uint8_t *data, int len)
 {
     currentPort = mspPort;
     mspPort->cmdMSP = cmd;
@@ -1968,3 +1950,17 @@ void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len)
 
     tailSerialReply();
 }
+
+/*
+ * Return a pointer to the process command function
+ */
+mspProcessCommandFnPtr mspFcInit(void)
+{
+    initActiveBoxIds();
+    return mspFcProcessCommand;
+}
+
+mspPushCommandFnPtr mspFcPushInit(void)
+{
+    return mspServerPush;
+}
diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h
index 745fd551a9..be575e72e4 100644
--- a/src/main/fc/fc_msp.h
+++ b/src/main/fc/fc_msp.h
@@ -20,3 +20,4 @@
 #include "msp/msp.h"
 
 mspProcessCommandFnPtr mspFcInit(void);
+mspPushCommandFnPtr mspFcPushInit(void);
diff --git a/src/main/io/cms.h b/src/main/io/cms.h
index e6d9887938..61b059b1a3 100644
--- a/src/main/io/cms.h
+++ b/src/main/io/cms.h
@@ -1 +1,2 @@
+void cmsInit(void);
 void cmsHandler(uint32_t);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 94b4d68f4b..71d5dc8a67 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -98,8 +98,9 @@ void osdResetAlarms(void);
 // specific functions; max7456XXX(), canvasXXX(), oledXXX(), ...
 //
 
-#include "io/serial_msp.h"
+#include "fc/fc_msp.h"
 #include "msp/msp_protocol.h"
+#include "msp/msp_serial.h"
 
 void canvasBegin(void)
 {
@@ -146,6 +147,12 @@ void canvasWrite(int col, int row, char *string)
 
     mspSerialPush(MSP_CANVAS, (uint8_t *)buf, len + 4);
 }
+
+// Called once at startup to initialize push function in msp
+void canvasInit(void)
+{
+    mspSerialPushInit(mspFcPushInit());
+}
 #endif
 
 // Force draw all elements if true
@@ -226,6 +233,13 @@ void cmsScreenResync(void)
 #endif
 }
 
+void cmsScreenInit(void)
+{
+#ifdef CANVAS
+    canvasInit();
+#endif
+}
+
 //
 // Lots of things not separated yet.
 //
@@ -1348,6 +1362,11 @@ void cmsHandler(uint32_t currentTime)
 
 }
 
+void cmsInit(void)
+{
+    cmsScreenInit();
+}
+
 // Does this belong here?
 
 #ifdef USE_FLASHFS
diff --git a/src/main/main.c b/src/main/main.c
index 6bf9b1e7df..4cf62bf83f 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -88,6 +88,7 @@
 #include "io/transponder_ir.h"
 #include "io/osd.h"
 #include "io/vtx.h"
+#include "io/cms.h"
 
 #include "scheduler/scheduler.h"
 
@@ -424,6 +425,10 @@ void init(void)
     }
 #endif
 
+#ifdef CMS
+    cmsInit();
+#endif
+
     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
             masterConfig.acc_hardware,
             masterConfig.mag_hardware,
diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h
index 3f3216df29..268c621433 100644
--- a/src/main/msp/msp.h
+++ b/src/main/msp/msp.h
@@ -29,3 +29,4 @@ struct serialPort_s;
 typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
 struct mspPort_s;
 typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn);
+typedef void (*mspPushCommandFnPtr)(struct mspPort_s *, uint8_t, uint8_t *, int);
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 3bb28db637..29d23ce8be 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -35,6 +35,7 @@
 
 
 static mspProcessCommandFnPtr mspProcessCommandFn;
+static mspPushCommandFnPtr mspPushCommandFn;
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 bufWriter_t *writer;
 
@@ -128,34 +129,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
     return mspPostProcessFn;
 }
 
-#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
-
-void mspSerialPush(int cmd, uint8_t *data, int buflen)
-{
-    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
-        mspPort_t * const mspPort = &mspPorts[portIndex];
-        if (!mspPort->port) {
-            continue;
-        }
-
-        // Big enough for a OSD line
-        uint8_t buf[sizeof(bufWriter_t) + 30];
-
-        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
-
-        mspServerPush(mspPort, cmd, data, buflen);
-
-        bufWriterFlush(writer);
-    }
-}
-
 /*
  * Process MSP commands from serial ports configured as MSP ports.
  *
@@ -204,3 +177,27 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
     memset(mspPorts, 0, sizeof(mspPorts));
     mspSerialAllocatePorts();
 }
+
+void mspSerialPush(uint8_t cmd, uint8_t *data, int buflen)
+{
+    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t * const mspPort = &mspPorts[portIndex];
+        if (!mspPort->port) {
+            continue;
+        }
+
+        // Big enough for a OSD line
+        uint8_t buf[sizeof(bufWriter_t) + 30];
+
+        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
+
+        mspPushCommandFn(mspPort, cmd, data, buflen);
+
+        bufWriterFlush(writer);
+    }
+}
+
+void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse)
+{
+    mspPushCommandFn = mspPushCommandFnToUse;
+}
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index b6b3b319f4..19ab30dead 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -60,4 +60,6 @@ void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
-void mspSerialPush(int, uint8_t *, int);
+
+void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn);
+void mspSerialPush(uint8_t, uint8_t *, int);