diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 228fcf16f..2ed813998 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -551,7 +551,7 @@ STATIC_UNIT_TESTED const void *cmsMenuBack(displayPort_t *pDisplay)
 {
     // Let onExit function decide whether to allow exit or not.
     if (currentCtx.menu->onExit) {
-        const void *result = currentCtx.menu->onExit(pageTop + currentCtx.cursorRow);
+        const void *result = currentCtx.menu->onExit(pDisplay, pageTop + currentCtx.cursorRow);
         if (result == MENU_CHAIN_BACK) {
             return result;
         }
@@ -655,7 +655,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
     }
 
     if (currentCtx.menu->onDisplayUpdate) {
-        const void *result = currentCtx.menu->onDisplayUpdate(pageTop + currentCtx.cursorRow);
+        const void *result = currentCtx.menu->onDisplayUpdate(pDisplay, pageTop + currentCtx.cursorRow);
         if (result == MENU_CHAIN_BACK) {
             cmsMenuBack(pDisplay);
 
@@ -712,19 +712,21 @@ const void *cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
     if (pMenu != currentCtx.menu) {
         saveMenuInhibited = false;
 
-        // Stack the current menu and move to a new menu.
-        if (menuStackIdx >= CMS_MENU_STACK_LIMIT - 1) {
-            // menu stack limit reached - prevent array overflow
-            return NULL;
+        if (currentCtx.menu) {
+            // If we are opening the initial top-level menu, then currentCtx.menu will be NULL and nothing to do.
+            // Otherwise stack the current menu before moving to the selected menu.
+            if (menuStackIdx >= CMS_MENU_STACK_LIMIT - 1) {
+                // menu stack limit reached - prevent array overflow
+                return NULL;
+            }
+            menuStack[menuStackIdx++] = currentCtx;
         }
 
-        menuStack[menuStackIdx++] = currentCtx;
-
         currentCtx.menu = pMenu;
         currentCtx.cursorRow = 0;
 
         if (pMenu->onEnter) {
-            const void *result = pMenu->onEnter();
+            const void *result = pMenu->onEnter(pDisplay);
             if (result == MENU_CHAIN_BACK) {
                 return cmsMenuBack(pDisplay);
             }
@@ -751,19 +753,22 @@ const void *cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
 
 void cmsMenuOpen(void)
 {
+    const CMS_Menu *startMenu;
     if (!cmsInMenu) {
         // New open
         pCurrentDisplay = cmsDisplayPortSelectCurrent();
         if (!pCurrentDisplay)
             return;
         cmsInMenu = true;
-        currentCtx = (cmsCtx_t){ &cmsx_menuMain, 0, 0 };
+        currentCtx = (cmsCtx_t){ NULL, 0, 0 };
+        startMenu = &cmsx_menuMain;
         menuStackIdx = 0;
         setArmingDisabled(ARMING_DISABLED_CMS_MENU);
         displayLayerSelect(pCurrentDisplay, DISPLAYPORT_LAYER_FOREGROUND); // make sure the foreground layer is active
     } else {
         // Switch display
         displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
+        startMenu = currentCtx.menu;
         if (pNextDisplay != pCurrentDisplay) {
             // DisplayPort has been changed.
             // Convert cursorRow to absolute value
@@ -801,7 +806,7 @@ void cmsMenuOpen(void)
     	maxMenuItems      = pCurrentDisplay->rows;
     }
 
-    cmsMenuChange(pCurrentDisplay, currentCtx.menu);
+    cmsMenuChange(pCurrentDisplay, startMenu);
 }
 
 static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
@@ -826,14 +831,14 @@ const void *cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
         cmsTraverseGlobalExit(&cmsx_menuMain);
 
         if (currentCtx.menu->onExit) {
-            currentCtx.menu->onExit((OSD_Entry *)NULL); // Forced exit
+            currentCtx.menu->onExit(pDisplay, (OSD_Entry *)NULL); // Forced exit
         }
 
         if ((exitType == CMS_POPUP_SAVE) || (exitType == CMS_POPUP_SAVEREBOOT)) {
             // traverse through the menu stack and call their onExit functions
             for (int i = menuStackIdx - 1; i >= 0; i--) {
                 if (menuStack[i].menu->onExit) {
-                    menuStack[i].menu->onExit((OSD_Entry *)NULL);
+                    menuStack[i].menu->onExit(pDisplay, (OSD_Entry *)NULL);
                 }
             }
         }
@@ -1323,4 +1328,13 @@ void inhibitSaveMenu(void)
     saveMenuInhibited = true;
 }
 
+void cmsAddMenuEntry(OSD_Entry *menuEntry, char *text, OSD_MenuElement type, CMSEntryFuncPtr func, void *data, uint8_t flags)
+{
+        menuEntry->text = text;
+        menuEntry->type = type;
+        menuEntry->func = func;
+        menuEntry->data = data;
+        menuEntry->flags = flags;
+}
+
 #endif // CMS
diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h
index c8d35a48b..b85fa3ac8 100644
--- a/src/main/cms/cms.h
+++ b/src/main/cms/cms.h
@@ -24,6 +24,8 @@
 
 #include "common/time.h"
 
+#include "cms/cms_types.h"
+
 typedef enum {
     CMS_KEY_NONE,
     CMS_KEY_UP,
@@ -52,6 +54,7 @@ const void *cmsMenuChange(displayPort_t *pPort, const void *ptr);
 const void *cmsMenuExit(displayPort_t *pPort, const void *ptr);
 void cmsSetExternKey(cms_key_e extKey);
 void inhibitSaveMenu(void);
+void cmsAddMenuEntry(OSD_Entry *menuEntry, char *text, OSD_MenuElement type, CMSEntryFuncPtr func, void *data, uint8_t flags);
 
 #define CMS_STARTUP_HELP_TEXT1 "MENU:THR MID"
 #define CMS_STARTUP_HELP_TEXT2     "+ YAW LEFT"
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index 11d53e74a..f2172b958 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@@ -178,8 +178,10 @@ static const void *cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
 }
 #endif // USE_FLASHFS
 
-static const void *cmsx_Blackbox_onEnter(void)
+static const void *cmsx_Blackbox_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     cmsx_Blackbox_GetDeviceStatus();
     cmsx_BlackboxDevice = blackboxConfig()->device;
 
@@ -188,8 +190,9 @@ static const void *cmsx_Blackbox_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_Blackbox_onExit(const OSD_Entry *self)
+static const void *cmsx_Blackbox_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     if (blackboxMayEditConfig()) {
diff --git a/src/main/cms/cms_menu_failsafe.c b/src/main/cms/cms_menu_failsafe.c
index e5642802e..59ab5eaaf 100644
--- a/src/main/cms/cms_menu_failsafe.c
+++ b/src/main/cms/cms_menu_failsafe.c
@@ -48,8 +48,10 @@ uint8_t failsafeConfig_failsafe_delay;
 uint8_t failsafeConfig_failsafe_off_delay;
 uint16_t failsafeConfig_failsafe_throttle;
 
-static const void *cmsx_Failsafe_onEnter(void)
+static const void *cmsx_Failsafe_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     failsafeConfig_failsafe_procedure = failsafeConfig()->failsafe_procedure;
     failsafeConfig_failsafe_delay = failsafeConfig()->failsafe_delay;
     failsafeConfig_failsafe_off_delay = failsafeConfig()->failsafe_off_delay;
@@ -58,8 +60,9 @@ static const void *cmsx_Failsafe_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_Failsafe_onExit(const OSD_Entry *self)
+static const void *cmsx_Failsafe_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     failsafeConfigMutable()->failsafe_procedure = failsafeConfig_failsafe_procedure;
diff --git a/src/main/cms/cms_menu_firmware.c b/src/main/cms/cms_menu_firmware.c
index 1b31a5a2f..dca7ed422 100644
--- a/src/main/cms/cms_menu_firmware.c
+++ b/src/main/cms/cms_menu_firmware.c
@@ -65,8 +65,9 @@ static char accCalibrationStatus[CALIBRATION_STATUS_MAX_LENGTH];
 static char baroCalibrationStatus[CALIBRATION_STATUS_MAX_LENGTH];
 #endif
 
-static const void *cmsx_CalibrationOnDisplayUpdate(const OSD_Entry *selected)
+static const void *cmsx_CalibrationOnDisplayUpdate(displayPort_t *pDisp, const OSD_Entry *selected)
 {
+    UNUSED(pDisp);
     UNUSED(selected);
 
     tfp_sprintf(gyroCalibrationStatus, sensors(SENSOR_GYRO) ? gyroIsCalibrationComplete() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF);
@@ -131,7 +132,7 @@ static const OSD_Entry menuCalibrateAccEntries[] = {
     { NULL, OME_END, NULL, NULL, 0 }
 };
 
-static CMS_Menu cmsx_menuCalibrateAcc = {
+CMS_Menu cmsx_menuCalibrateAcc = {
 #ifdef CMS_MENU_DEBUG
     .GUARD_text = "ACCCALIBRATION",
     .GUARD_type = OME_MENU,
@@ -142,7 +143,7 @@ static CMS_Menu cmsx_menuCalibrateAcc = {
     .entries = menuCalibrateAccEntries
 };
 
-static const void *cmsCalibrateAccMenu(displayPort_t *pDisp, const void *self)
+const void *cmsCalibrateAccMenu(displayPort_t *pDisp, const void *self)
 {
     UNUSED(self);
 
@@ -184,8 +185,10 @@ static CMS_Menu cmsx_menuCalibration = {
 static char infoGitRev[GIT_SHORT_REVISION_LENGTH + 1];
 static char infoTargetName[] = __TARGET__;
 
-static const void *cmsx_FirmwareInit(void)
+static const void *cmsx_FirmwareInit(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     unsigned i;
     for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
         if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f') {
diff --git a/src/main/cms/cms_menu_firmware.h b/src/main/cms/cms_menu_firmware.h
index 88d9d6747..c92d43b5b 100644
--- a/src/main/cms/cms_menu_firmware.h
+++ b/src/main/cms/cms_menu_firmware.h
@@ -23,3 +23,6 @@
 #include "cms/cms_types.h"
 
 extern CMS_Menu cmsx_menuFirmware;
+extern CMS_Menu cmsx_menuCalibrateAcc;
+
+const void *cmsCalibrateAccMenu(displayPort_t *pDisp, const void *self);
diff --git a/src/main/cms/cms_menu_gps_rescue.c b/src/main/cms/cms_menu_gps_rescue.c
index 6e0a700cc..654aa349f 100644
--- a/src/main/cms/cms_menu_gps_rescue.c
+++ b/src/main/cms/cms_menu_gps_rescue.c
@@ -59,8 +59,9 @@ static uint8_t gpsRescueConfig_altitudeMode;
 static uint16_t gpsRescueConfig_ascendRate;
 static uint16_t gpsRescueConfig_descendRate;
 
-static const void *cms_menuGpsRescuePidOnEnter(void)
+static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
 
     gpsRescueConfig_throttleP = gpsRescueConfig()->throttleP;
     gpsRescueConfig_throttleI = gpsRescueConfig()->throttleI;
@@ -75,8 +76,9 @@ static const void *cms_menuGpsRescuePidOnEnter(void)
     return NULL;
 }
 
-static const void *cms_menuGpsRescuePidOnExit(const OSD_Entry *self)
+static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     gpsRescueConfigMutable()->throttleP = gpsRescueConfig_throttleP;
@@ -121,8 +123,9 @@ CMS_Menu cms_menuGpsRescuePid = {
     .entries = cms_menuGpsRescuePidEntries,
 };
 
-static const void *cmsx_menuGpsRescueOnEnter(void)
+static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
 
     gpsRescueConfig_angle = gpsRescueConfig()->angle;
     gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
@@ -143,11 +146,11 @@ static const void *cmsx_menuGpsRescueOnEnter(void)
     return NULL;
 }
 
-static const void *cmsx_menuGpsRescueOnExit(const OSD_Entry *self)
+static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
-
     gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
     gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
     gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 6dc8a9910..53f963995 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -106,8 +106,10 @@ static void setProfileIndexString(char *profileString, int profileIndex, char *p
     profileString[charIndex] = '\0';
 }
 
-static const void *cmsx_menuImu_onEnter(void)
+static const void *cmsx_menuImu_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     pidProfileIndex = getCurrentPidProfileIndex();
     tmpPidProfileIndex = pidProfileIndex + 1;
 
@@ -117,8 +119,9 @@ static const void *cmsx_menuImu_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_menuImu_onExit(const OSD_Entry *self)
+static const void *cmsx_menuImu_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     changePidProfile(pidProfileIndex);
@@ -163,16 +166,19 @@ static const void *cmsx_PidRead(void)
     return NULL;
 }
 
-static const void *cmsx_PidOnEnter(void)
+static const void *cmsx_PidOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
     cmsx_PidRead();
 
     return NULL;
 }
 
-static const void *cmsx_PidWriteback(const OSD_Entry *self)
+static const void *cmsx_PidWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     pidProfile_t *pidProfile = currentPidProfile;
@@ -232,8 +238,9 @@ static const void *cmsx_RateProfileRead(void)
     return NULL;
 }
 
-static const void *cmsx_RateProfileWriteback(const OSD_Entry *self)
+static const void *cmsx_RateProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     memcpy(controlRateProfilesMutable(rateProfileIndex), &rateProfile, sizeof(controlRateConfig_t));
@@ -241,8 +248,10 @@ static const void *cmsx_RateProfileWriteback(const OSD_Entry *self)
     return NULL;
 }
 
-static const void *cmsx_RateProfileOnEnter(void)
+static const void *cmsx_RateProfileOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     setProfileIndexString(rateProfileIndexString, rateProfileIndex, controlRateProfilesMutable(rateProfileIndex)->profileName);
     cmsx_RateProfileRead();
 
@@ -295,8 +304,10 @@ static uint8_t cmsx_launchControlThrottlePercent;
 static uint8_t cmsx_launchControlAngleLimit;
 static uint8_t cmsx_launchControlGain;
 
-static const void *cmsx_launchControlOnEnter(void)
+static const void *cmsx_launchControlOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 
     cmsx_launchControlMode  = pidProfile->launchControlMode;
@@ -308,8 +319,9 @@ static const void *cmsx_launchControlOnEnter(void)
     return NULL;
 }
 
-static const void *cmsx_launchControlOnExit(const OSD_Entry *self)
+static const void *cmsx_launchControlOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
@@ -375,8 +387,10 @@ static uint8_t cmsx_ff_interpolate_sp;
 static uint8_t cmsx_ff_smooth_factor;
 #endif
 
-static const void *cmsx_profileOtherOnEnter(void)
+static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     setProfileIndexString(pidProfileIndexString, pidProfileIndex, currentPidProfile->profileName);
 
     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
@@ -417,8 +431,9 @@ static const void *cmsx_profileOtherOnEnter(void)
     return NULL;
 }
 
-static const void *cmsx_profileOtherOnExit(const OSD_Entry *self)
+static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
@@ -521,8 +536,10 @@ static uint16_t gyroConfig_gyro_soft_notch_hz_2;
 static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
 static uint8_t  gyroConfig_gyro_to_use;
 
-static const void *cmsx_menuGyro_onEnter(void)
+static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     gyroConfig_gyro_lowpass_hz =  gyroConfig()->gyro_lowpass_hz;
     gyroConfig_gyro_lowpass2_hz =  gyroConfig()->gyro_lowpass2_hz;
     gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
@@ -534,8 +551,9 @@ static const void *cmsx_menuGyro_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_menuGyro_onExit(const OSD_Entry *self)
+static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     gyroConfigMutable()->gyro_lowpass_hz =  gyroConfig_gyro_lowpass_hz;
@@ -596,8 +614,10 @@ static uint16_t dynFiltDtermMax;
 static uint8_t dynFiltDtermExpo;
 #endif
 
-static const void *cmsx_menuDynFilt_onEnter(void)
+static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
 #ifdef USE_GYRO_DATA_ANALYSE
     dynFiltNotchMaxHz   = gyroConfig()->dyn_notch_max_hz;
     dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent;
@@ -616,8 +636,9 @@ static const void *cmsx_menuDynFilt_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_menuDynFilt_onExit(const OSD_Entry *self)
+static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
 #ifdef USE_GYRO_DATA_ANALYSE
@@ -680,8 +701,10 @@ static uint16_t cmsx_dterm_notch_hz;
 static uint16_t cmsx_dterm_notch_cutoff;
 static uint16_t cmsx_yaw_lowpass_hz;
 
-static const void *cmsx_FilterPerProfileRead(void)
+static const void *cmsx_FilterPerProfileRead(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
 
     cmsx_dterm_lowpass_hz   = pidProfile->dterm_lowpass_hz;
@@ -693,8 +716,9 @@ static const void *cmsx_FilterPerProfileRead(void)
     return NULL;
 }
 
-static const void *cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
+static const void *cmsx_FilterPerProfileWriteback(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     pidProfile_t *pidProfile = currentPidProfile;
@@ -748,8 +772,10 @@ static const char * const cmsx_ProfileNames[] = {
 static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames };
 static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames };
 
-static const void *cmsx_menuCopyProfile_onEnter(void)
+static const void *cmsx_menuCopyProfile_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     cmsx_dstPidProfile = 0;
     cmsx_dstControlRateProfile = 0;
 
diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c
index e1cf9e850..4f5295716 100644
--- a/src/main/cms/cms_menu_ledstrip.c
+++ b/src/main/cms/cms_menu_ledstrip.c
@@ -65,8 +65,10 @@ const char * const ledProfileNames[LED_PROFILE_COUNT] = {
 #endif
 };
 
-static const void *cmsx_Ledstrip_OnEnter(void)
+static const void *cmsx_Ledstrip_OnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     cmsx_FeatureLedstrip = featureIsEnabled(FEATURE_LED_STRIP) ? 1 : 0;
     cmsx_ledProfile = getLedProfile();
     cmsx_ledRaceColor = ledStripConfig()->ledstrip_race_color;
@@ -80,8 +82,9 @@ static const void *cmsx_Ledstrip_OnEnter(void)
     return NULL;
 }
 
-static const void *cmsx_Ledstrip_OnExit(const OSD_Entry *self)
+static const void *cmsx_Ledstrip_OnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     if (cmsx_FeatureLedstrip) {
diff --git a/src/main/cms/cms_menu_main.c b/src/main/cms/cms_menu_main.c
index aa0635d19..89d507090 100644
--- a/src/main/cms/cms_menu_main.c
+++ b/src/main/cms/cms_menu_main.c
@@ -47,10 +47,26 @@
 
 #include "cms/cms_menu_vtx_common.h"
 
+#include "common/printf.h"
+
 #include "config/config.h"
 
+#include "fc/core.h"
+#include "fc/runtime_config.h"
+
+#include "sensors/acceleration.h"
+
 #include "cms_menu_main.h"
 
+#define CALIBRATION_STATUS_MAX_LENGTH 9
+
+#define CALIBRATION_STATUS_REQUIRED "REQUIRED"
+#define CALIBRATION_STATUS_ACTIVE   "  ACTIVE"
+#define CALIBRATION_STATUS_COMPLETE "COMPLETE"
+
+#if defined(USE_ACC)
+static char accCalibrationStatus[CALIBRATION_STATUS_MAX_LENGTH];
+#endif
 
 // Features
 
@@ -97,7 +113,64 @@ static const void *cmsx_SaveExitMenu(displayPort_t *pDisplay, const void *ptr)
     return NULL;
 }
 
+
+#define SETUP_POPUP_MAX_ENTRIES 1   // Increase as new entries are added
+
+static OSD_Entry setupPopupMenuEntries[SETUP_POPUP_MAX_ENTRIES + 3];
+
+static bool setupPopupMenuBuild(void)
+{
+    uint8_t menuIndex = 0;
+    updateArmingStatus();
+
+    cmsAddMenuEntry(&setupPopupMenuEntries[menuIndex], "-- SETUP MENU --", OME_Label, NULL, NULL, 0);
+
+    // Add menu entries for uncompleted setup tasks
+#if defined(USE_ACC)
+    if (sensors(SENSOR_ACC) && (getArmingDisableFlags() & ARMING_DISABLED_ACC_CALIBRATION)) {
+        cmsAddMenuEntry(&setupPopupMenuEntries[++menuIndex], "CALIBRATE ACC", OME_Funcall, cmsCalibrateAccMenu, accCalibrationStatus, DYNAMIC);
+    }
+#endif
+
+    cmsAddMenuEntry(&setupPopupMenuEntries[++menuIndex], "EXIT", OME_Back, NULL, NULL, DYNAMIC);
+    cmsAddMenuEntry(&setupPopupMenuEntries[++menuIndex], "NULL", OME_END, NULL, NULL, 0);
+
+    return (menuIndex > 2);  // return true if any setup items were added
+}
+
+static const void *setupPopupMenuOnDisplayUpdate(displayPort_t *pDisp, const OSD_Entry *selected)
+{
+    UNUSED(pDisp);
+    UNUSED(selected);
+
+#if defined(USE_ACC)
+    // Update the ACC calibration status message.
+    tfp_sprintf(accCalibrationStatus, accIsCalibrationComplete() ? accHasBeenCalibrated() ? CALIBRATION_STATUS_COMPLETE : CALIBRATION_STATUS_REQUIRED : CALIBRATION_STATUS_ACTIVE);
+#endif
+
+    return NULL;
+}
+
+CMS_Menu cmsx_menuSetupPopup = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "SETUPPOPUP",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = NULL,
+    .onExit = NULL,
+    .onDisplayUpdate = setupPopupMenuOnDisplayUpdate,
+    .entries = setupPopupMenuEntries,
+};
+
 // Main
+static const void *mainMenuOnEnter(displayPort_t *pDisp)
+{
+    if (setupPopupMenuBuild()) {
+        // If setup issues were found then switch to the dynamically constructed menu
+        cmsMenuChange(pDisp, &cmsx_menuSetupPopup);
+    }
+    return NULL;
+}
 
 static const OSD_Entry menuMainEntries[] =
 {
@@ -119,9 +192,10 @@ CMS_Menu cmsx_menuMain = {
     .GUARD_text = "MENUMAIN",
     .GUARD_type = OME_MENU,
 #endif
-    .onEnter = NULL,
+    .onEnter = mainMenuOnEnter,
     .onExit = NULL,
     .onDisplayUpdate = NULL,
     .entries = menuMainEntries,
 };
+
 #endif
diff --git a/src/main/cms/cms_menu_main.h b/src/main/cms/cms_menu_main.h
index 1f74b655d..a6be76c2e 100644
--- a/src/main/cms/cms_menu_main.h
+++ b/src/main/cms/cms_menu_main.h
@@ -23,3 +23,4 @@
 #include "cms/cms_types.h"
 
 extern CMS_Menu cmsx_menuMain;
+extern CMS_Menu cmsx_menuSetupPopup;
diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c
index ef7c60cbe..ce306b4bf 100644
--- a/src/main/cms/cms_menu_misc.c
+++ b/src/main/cms/cms_menu_misc.c
@@ -59,15 +59,19 @@
 // Misc
 //
 
-static const void *cmsx_menuRcOnEnter(void)
+static const void *cmsx_menuRcOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     inhibitSaveMenu();
 
     return NULL;
 }
 
-static const void *cmsx_menuRcConfirmBack(const OSD_Entry *self)
+static const void *cmsx_menuRcConfirmBack(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
+
     if (self && self->type == OME_Back) {
         return NULL;
     } else {
@@ -111,8 +115,10 @@ static uint16_t motorConfig_minthrottle;
 static uint8_t motorConfig_digitalIdleOffsetValue;
 static uint8_t rxConfig_fpvCamAngleDegrees;
 
-static const void *cmsx_menuMiscOnEnter(void)
+static const void *cmsx_menuMiscOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     motorConfig_minthrottle = motorConfig()->minthrottle;
     motorConfig_digitalIdleOffsetValue = motorConfig()->digitalIdleOffsetValue / 10;
     rxConfig_fpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
@@ -120,8 +126,9 @@ static const void *cmsx_menuMiscOnEnter(void)
     return NULL;
 }
 
-static const void *cmsx_menuMiscOnExit(const OSD_Entry *self)
+static const void *cmsx_menuMiscOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     motorConfigMutable()->minthrottle = motorConfig_minthrottle;
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index af946eadf..25a2662de 100644
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -48,14 +48,17 @@
 #ifdef USE_EXTENDED_CMS_MENUS
 static uint16_t osdConfig_item_pos[OSD_ITEM_COUNT];
 
-static const void *menuOsdActiveElemsOnEnter(void)
+static const void *menuOsdActiveElemsOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     memcpy(&osdConfig_item_pos[0], &osdElementConfig()->item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
     return NULL;
 }
 
-static const void *menuOsdActiveElemsOnExit(const OSD_Entry *self)
+static const void *menuOsdActiveElemsOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     memcpy(&osdElementConfigMutable()->item_pos[0], &osdConfig_item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
@@ -171,8 +174,10 @@ static uint16_t osdConfig_distance_alarm;
 static uint8_t batteryConfig_vbatDurationForWarning;
 static uint8_t batteryConfig_vbatDurationForCritical;
 
-static const void *menuAlarmsOnEnter(void)
+static const void *menuAlarmsOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     osdConfig_rssi_alarm = osdConfig()->rssi_alarm;
     osdConfig_link_quality_alarm = osdConfig()->link_quality_alarm;
     osdConfig_rssi_dbm_alarm = osdConfig()->rssi_dbm_alarm;
@@ -185,8 +190,9 @@ static const void *menuAlarmsOnEnter(void)
     return NULL;
 }
 
-static const void *menuAlarmsOnExit(const OSD_Entry *self)
+static const void *menuAlarmsOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm;
@@ -231,8 +237,10 @@ osd_timer_source_e timerSource[OSD_TIMER_COUNT];
 osd_timer_precision_e timerPrecision[OSD_TIMER_COUNT];
 uint8_t timerAlarm[OSD_TIMER_COUNT];
 
-static const void *menuTimersOnEnter(void)
+static const void *menuTimersOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
         const uint16_t timer = osdConfig()->timers[i];
         timerSource[i] = OSD_TIMER_SRC(timer);
@@ -243,8 +251,9 @@ static const void *menuTimersOnEnter(void)
     return NULL;
 }
 
-static const void *menuTimersOnExit(const OSD_Entry *self)
+static const void *menuTimersOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
@@ -291,8 +300,10 @@ static uint8_t displayPortProfileMax7456_whiteBrightness;
 static uint8_t osdConfig_osdProfileIndex;
 #endif
 
-static const void *cmsx_menuOsdOnEnter(void)
+static const void *cmsx_menuOsdOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
 #ifdef USE_OSD_PROFILES
     osdConfig_osdProfileIndex = osdConfig()->osdProfileIndex;
 #endif
@@ -306,8 +317,9 @@ static const void *cmsx_menuOsdOnEnter(void)
     return NULL;
 }
 
-static const void *cmsx_menuOsdOnExit(const OSD_Entry *self)
+static const void *cmsx_menuOsdOnExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
 #ifdef USE_OSD_PROFILES
diff --git a/src/main/cms/cms_menu_power.c b/src/main/cms/cms_menu_power.c
index c35f85886..5d7724c20 100644
--- a/src/main/cms/cms_menu_power.c
+++ b/src/main/cms/cms_menu_power.c
@@ -54,8 +54,10 @@ int16_t currentSensorVirtualConfig_scale;
 int16_t currentSensorVirtualConfig_offset;
 #endif
 
-static const void *cmsx_Power_onEnter(void)
+static const void *cmsx_Power_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     batteryConfig_voltageMeterSource = batteryConfig()->voltageMeterSource;
     batteryConfig_currentMeterSource = batteryConfig()->currentMeterSource;
 
@@ -74,8 +76,9 @@ static const void *cmsx_Power_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_Power_onExit(const OSD_Entry *self)
+static const void *cmsx_Power_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     batteryConfigMutable()->voltageMeterSource = batteryConfig_voltageMeterSource;
diff --git a/src/main/cms/cms_menu_vtx_common.c b/src/main/cms/cms_menu_vtx_common.c
index 867abcc6a..967a95e15 100644
--- a/src/main/cms/cms_menu_vtx_common.c
+++ b/src/main/cms/cms_menu_vtx_common.c
@@ -43,8 +43,10 @@
 static char statusLine1[MAX_STATUS_LINE_LENGTH] = "";
 static char statusLine2[MAX_STATUS_LINE_LENGTH] = "";
 
-static const void *setStatusMessage(void)
+static const void *setStatusMessage(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     vtxDevice_t *device = vtxCommonDevice();
 
     statusLine1[0] = 0;
diff --git a/src/main/cms/cms_menu_vtx_rtc6705.c b/src/main/cms/cms_menu_vtx_rtc6705.c
index e3f8698a8..e45892f16 100644
--- a/src/main/cms/cms_menu_vtx_rtc6705.c
+++ b/src/main/cms/cms_menu_vtx_rtc6705.c
@@ -78,8 +78,10 @@ static void cmsx_Vtx_ConfigWriteback(void)
     saveConfigAndNotify();
 }
 
-static const void *cmsx_Vtx_onEnter(void)
+static const void *cmsx_Vtx_onEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     cmsx_Vtx_ConfigRead();
 
     entryVtxBand.val = &cmsx_vtxBand;
@@ -97,8 +99,9 @@ static const void *cmsx_Vtx_onEnter(void)
     return NULL;
 }
 
-static const void *cmsx_Vtx_onExit(const OSD_Entry *self)
+static const void *cmsx_Vtx_onExit(displayPort_t *pDisp, const OSD_Entry *self)
 {
+    UNUSED(pDisp);
     UNUSED(self);
 
     vtxCommonSetPitMode(vtxCommonDevice(), cmsx_vtxPit);
diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c
index ddd478961..40295765a 100644
--- a/src/main/cms/cms_menu_vtx_smartaudio.c
+++ b/src/main/cms/cms_menu_vtx_smartaudio.c
@@ -468,7 +468,7 @@ static const char * const saCmsPitNames[] = {
 static OSD_TAB_t saCmsEntPitFMode = { &saCmsPitFMode, 1, saCmsPitFModeNames };
 static OSD_TAB_t saCmsEntPit = {&saCmsPit, 2, saCmsPitNames};
 
-static const void *sacms_SetupTopMenu(void); // Forward
+static const void *sacms_SetupTopMenu(displayPort_t *pDisp); // Forward
 
 static const void *saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self)
 {
@@ -484,7 +484,7 @@ static const void *saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *s
     // don't call 'saSetBandAndChannel()' / 'saSetFreq()' here,
     // wait until SET / 'saCmsCommence()' is activated
 
-    sacms_SetupTopMenu();
+    sacms_SetupTopMenu(pDisp);
 
     return NULL;
 }
@@ -540,8 +540,10 @@ static const void *saCmsCommence(displayPort_t *pDisp, const void *self)
     return MENU_CHAIN_BACK;
 }
 
-static const void *saCmsSetPORFreqOnEnter(void)
+static const void *saCmsSetPORFreqOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     if (saDevice.version == 1)
         return MENU_CHAIN_BACK;
 
@@ -584,8 +586,10 @@ static const char *saCmsUserFreqGetString(displayPort_t *pDisp, const void *self
     return pbuf;
 }
 
-static const void *saCmsSetUserFreqOnEnter(void)
+static const void *saCmsSetUserFreqOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     saCmsUserFreqNew = saCmsUserFreq;
 
     return NULL;
@@ -741,8 +745,10 @@ static const OSD_Entry saCmsMenuOfflineEntries[] =
 
 CMS_Menu cmsx_menuVtxSmartAudio; // Forward
 
-static const void *sacms_SetupTopMenu(void)
+static const void *sacms_SetupTopMenu(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     if (saCmsDeviceStatus) {
         if (saCmsFselModeNew == 0)
             cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries;
diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c
index c5271ef73..16782c90f 100644
--- a/src/main/cms/cms_menu_vtx_tramp.c
+++ b/src/main/cms/cms_menu_vtx_tramp.c
@@ -220,8 +220,10 @@ static bool trampCmsInitSettings(void)
     return true;
 }
 
-static const void *trampCmsOnEnter(void)
+static const void *trampCmsOnEnter(displayPort_t *pDisp)
 {
+    UNUSED(pDisp);
+
     if (!trampCmsInitSettings()) {
         return MENU_CHAIN_BACK;
     }
diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h
index 3bfe3bcd9..b07c1b3af 100644
--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@@ -58,11 +58,11 @@ typedef const void *(*CMSEntryFuncPtr)(displayPort_t *displayPort, const void *p
 
 typedef struct
 {
-    const char * const text;
-    const OSD_MenuElement type;
+    const char * text;
+    OSD_MenuElement type;
     CMSEntryFuncPtr func;
     void *data;
-    const uint8_t flags;
+    uint8_t flags;
 } __attribute__((packed)) OSD_Entry;
 
 // Bits in flags
@@ -82,7 +82,7 @@ typedef struct
 
 #define IS_DYNAMIC(p) ((p)->flags & DYNAMIC)
 
-typedef const void *(*CMSMenuFuncPtr)(void);
+typedef const void *(*CMSMenuFuncPtr)(displayPort_t *pDisp);
 
 // Special return value(s) for function chaining by CMSMenuFuncPtr
 extern int menuChainBack;
@@ -95,11 +95,9 @@ onExit function is called with self:
 (2) NULL if called from menu exit (forced exit at top level).
 */
 
-typedef const void *(*CMSMenuOnExitPtr)(const OSD_Entry *self);
+typedef const void *(*CMSMenuOnExitPtr)(displayPort_t *pDisp, const OSD_Entry *self);
 
-typedef const void * (*CMSMenuCheckRedirectPtr)(void);
-
-typedef const void *(*CMSMenuOnDisplayUpdatePtr)(const OSD_Entry *selected);
+typedef const void *(*CMSMenuOnDisplayUpdatePtr)(displayPort_t *pDisp, const OSD_Entry *selected);
 
 typedef struct
 {
diff --git a/src/test/unit/cms_unittest.cc b/src/test/unit/cms_unittest.cc
index 673887d09..836c45ead 100644
--- a/src/test/unit/cms_unittest.cc
+++ b/src/test/unit/cms_unittest.cc
@@ -118,7 +118,7 @@ TEST(CMSUnittest, TestCmsMenuKey)
 // STUBS
 
 extern "C" {
-static OSD_Entry menuMainEntries[] =
+static const OSD_Entry menuMainEntries[] =
 {
     {"-- MAIN MENU --", OME_Label, NULL, NULL, 0},
     {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},