diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 82ff00fd1a..1d919f83ae 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -32,8 +32,9 @@
 
 #ifdef CMS
 
-#include "build/version.h"
+#include "build/build_config.h"
 #include "build/debug.h"
+#include "build/version.h"
 
 #include "cms/cms.h"
 #include "cms/cms_menu_builtin.h"
@@ -124,7 +125,7 @@ static displayPort_t *cmsDisplayPortSelectNext(void)
 
 static bool cmsInMenu = false;
 
-static CMS_Menu *currentMenu;    // Points to top entry of the current page
+STATIC_UNIT_TESTED CMS_Menu *currentMenu;    // Points to top entry of the current page
 
 // XXX Does menu backing support backing into second page???
 
@@ -489,7 +490,7 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
     return 0;
 }
 
-static long cmsMenuBack(displayPort_t *pDisplay)
+STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay)
 {
     // Let onExit function decide whether to allow exit or not.
 
@@ -520,7 +521,7 @@ static long cmsMenuBack(displayPort_t *pDisplay)
     return 0;
 }
 
-static void cmsMenuOpen(void)
+STATIC_UNIT_TESTED void cmsMenuOpen(void)
 {
     if (!cmsInMenu) {
         // New open
@@ -595,7 +596,7 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
     return 0;
 }
 
-static uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
+STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
 {
     uint16_t res = BUTTON_TIME;
     OSD_Entry *p;
@@ -853,6 +854,8 @@ void cmsHandler(uint32_t currentTime)
 // Can it be done with the current main()?
 void cmsInit(void)
 {
+    cmsDeviceCount = 0;
+    cmsCurrentDevice = -1;
 }
 
 #endif // CMS
diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h
index de46c34ea8..fd60cb5480 100644
--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@@ -55,7 +55,7 @@ typedef enum
 
 typedef struct
 {
-    char *text;
+    const char *text;
     OSD_MenuElement type;
     CMSEntryFuncPtr func;
     void *data;
@@ -92,7 +92,7 @@ typedef long (*CMSMenuOnExitPtr)(OSD_Entry *self);
 typedef struct
 {
     // These two are debug aids for menu content creators.
-    char *GUARD_text;
+    const char *GUARD_text;
     OSD_MenuElement GUARD_type;
 
     CMSMenuFuncPtr onEnter;
diff --git a/src/test/Makefile b/src/test/Makefile
index 6581e7ae2b..d8eb183f34 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -110,6 +110,14 @@ $(OBJECT_DIR)/common/maths.o : \
 	@mkdir -p $(dir $@)
 	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
 
+$(OBJECT_DIR)/common/typeconversion.o : \
+	$(USER_DIR)/common/typeconversion.c \
+	$(USER_DIR)/common/typeconversion.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/typeconversion.c -o $@
+
 $(OBJECT_DIR)/common/filter.o : \
 	$(USER_DIR)/common/filter.c \
 	$(USER_DIR)/common/filter.h \
@@ -574,6 +582,39 @@ $(OBJECT_DIR)/alignsensor_unittest : \
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
 
+$(OBJECT_DIR)/drivers/display.o : \
+	$(USER_DIR)/drivers/display.c \
+	$(USER_DIR)/drivers/display.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/display.c -o $@
+
+$(OBJECT_DIR)/cms/cms.o : \
+	$(USER_DIR)/cms/cms.c \
+	$(USER_DIR)/cms/cms.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/cms/cms.c -o $@
+
+$(OBJECT_DIR)/cms_unittest.o : \
+	$(TEST_DIR)/cms_unittest.cc \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/cms_unittest.cc -o $@
+
+$(OBJECT_DIR)/cms_unittest : \
+	$(OBJECT_DIR)/cms_unittest.o \
+	$(OBJECT_DIR)/common/typeconversion.o \
+	$(OBJECT_DIR)/drivers/display.o \
+	$(OBJECT_DIR)/cms/cms.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
 ## test        : Build and run the Unit Tests
 test: $(TESTS:%=test-%)
 
diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc
new file mode 100644
index 0000000000..a417e8b7ee
--- /dev/null
+++ b/src/test/unit/cms_unittest.cc
@@ -0,0 +1,207 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+#include <limits.h>
+
+#include <math.h>
+
+#define BARO
+
+extern "C" {
+    #include "target.h"
+    #include "drivers/display.h"
+    #include "cms/cms.h"
+    #include "cms/cms_types.h"
+    void cmsMenuOpen(void);
+    long cmsMenuBack(displayPort_t *pDisplay);
+    uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key);
+    extern CMS_Menu *currentMenu;    // Points to top entry of the current page
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+static displayPort_t testDisplayPort;
+static int displayPortTestGrab(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestRelease(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestClear(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static int displayPortTestWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
+{
+    UNUSED(displayPort);
+    UNUSED(x);
+    UNUSED(y);
+    UNUSED(s);
+    return 0;
+}
+
+static int displayPortTestHeartbeat(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static void displayPortTestResync(displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+}
+
+static uint32_t displayPortTestTxBytesFree(const displayPort_t *displayPort)
+{
+    UNUSED(displayPort);
+    return 0;
+}
+
+static const displayPortVTable_t testDisplayPortVTable = {
+    .grab = displayPortTestGrab,
+    .release = displayPortTestRelease,
+    .clear = displayPortTestClear,
+    .write = displayPortTestWrite,
+    .heartbeat = displayPortTestHeartbeat,
+    .resync = displayPortTestResync,
+    .txBytesFree = displayPortTestTxBytesFree
+};
+
+displayPort_t *displayPortTestInit(void)
+{
+    testDisplayPort.vTable = &testDisplayPortVTable;
+    testDisplayPort.rows = 10;
+    testDisplayPort.cols = 40;
+    testDisplayPort.isGrabbed = false;
+    return &testDisplayPort;
+}
+
+TEST(CMSUnittest, TestCmsDisplayPortRegister)
+{
+    cmsInit();
+    displayPort_t *displayPort = displayPortTestInit();
+    for (int ii = 0; ii < CMS_MAX_DEVICE; ++ii) {
+        const bool registered = cmsDisplayPortRegister(displayPort);
+        EXPECT_EQ(true, registered);
+    }
+    const bool registered = cmsDisplayPortRegister(displayPort);
+    EXPECT_EQ(false, registered);
+}
+
+TEST(CMSUnittest, TestCmsMenuOpen)
+{
+    cmsInit();
+    displayPort_t *displayPort = displayPortTestInit();
+    displayGrab(displayPort);
+    cmsDisplayPortRegister(displayPort);
+
+    cmsMenuOpen();
+}
+
+TEST(CMSUnittest, TestCmsMenuExit0)
+{
+    cmsInit();
+    displayPort_t *displayPort = displayPortTestInit();
+    cmsDisplayPortRegister(displayPort);
+
+    cmsMenuOpen();
+    long exit = cmsMenuExit(displayPort, (void*)0);
+    EXPECT_EQ(0, exit);
+}
+
+TEST(CMSUnittest, TestCmsMenuExit1)
+{
+    cmsInit();
+    displayPort_t *displayPort = displayPortTestInit();
+    cmsDisplayPortRegister(displayPort);
+
+    cmsMenuOpen();
+    long exit = cmsMenuExit(displayPort, (void*)0);
+    EXPECT_EQ(0, exit);
+}
+
+TEST(CMSUnittest, TestCmsMenuBack)
+{
+    cmsInit();
+    displayPort_t *displayPort = displayPortTestInit();
+    cmsDisplayPortRegister(displayPort);
+
+    cmsMenuOpen();
+    long exit = cmsMenuBack(displayPort);
+    EXPECT_EQ(0, exit);
+}
+
+TEST(CMSUnittest, TestCmsMenuKey)
+{
+#define KEY_ENTER   0
+#define KEY_UP      1
+#define KEY_DOWN    2
+#define KEY_LEFT    3
+#define KEY_RIGHT   4
+#define KEY_ESC     5
+#define BUTTON_TIME   250 // msec
+#define BUTTON_PAUSE  500 // msec
+    cmsInit();
+    displayPort_t *displayPort = &testDisplayPort;
+    cmsDisplayPortRegister(displayPort);
+
+    cmsMenuOpen();
+    uint16_t result = cmsHandleKey(displayPort, KEY_ESC);
+    EXPECT_EQ(BUTTON_PAUSE, result);
+}
+// STUBS
+
+extern "C" {
+static OSD_Entry menuMainEntries[] =
+{
+    {"-- MAIN MENU --", OME_Label, NULL, NULL, 0},
+    {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},
+    {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0},
+    {NULL, OME_END, NULL, NULL, 0}
+};
+CMS_Menu menuMain = {
+    "MENUMAIN",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuMainEntries,
+};
+uint8_t armingFlags;
+int16_t debug[4];
+int16_t rcData[18];
+void delay(uint32_t) {}
+uint32_t micros(void) { return 0; }
+uint32_t millis(void) { return 0; }
+void saveConfigAndNotify(void) {}
+void stopMotors(void) {}
+void stopPwmAllMotors(void) {}
+void systemReset(void) {}
+}
\ No newline at end of file
diff --git a/src/test/unit/target.h b/src/test/unit/target.h
index b7899e43a2..c2ebf479ed 100644
--- a/src/test/unit/target.h
+++ b/src/test/unit/target.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+#define CMS
+#define CMS_MAX_DEVICE 4
 #define MAG
 #define BARO
 #define GPS