Introduce CMS_Menu

src/main/io/cms.c

@@ -19,6 +19,7 @@
  Created by Marcin Baliniak
  OSD-CMS separation by jflyper
  */
+#define CMS_MENU_DEBUG // For external menu content creators
 
 #include <stdbool.h>
 #include <stdint.h>
@@ -76,9 +77,9 @@
 #include "io/cms_ledstrip.h"
 
 // Forwards
-void cmsx_InfoInit(void);
+long cmsx_InfoInit(void);
-void cmsx_FeatureRead(void);
+long cmsx_FeatureRead(void);
-void cmsx_FeatureWriteback(void);
+long cmsx_FeatureWriteback(void);
 
 // Device management
 
@@ -154,19 +155,41 @@ displayPort_t currentDisplay;
 
 bool cmsInMenu = false;
 
-OSD_Entry menuMain[];
+CMS_Menu menuMain;
+CMS_Menu *currentMenu;          // Points to top entry of the current page
 
 // XXX Does menu backing support backing into second page???
 
-OSD_Entry *menuStack[10];        // Stack to save menu transition
+CMS_Menu *menuStack[10];         // Stack to save menu transition
 uint8_t menuStackHistory[10];    // cursorRow in a stacked menu
 uint8_t menuStackIdx = 0;
 
-OSD_Entry *currentMenu;          // Points to top entry of the current page
+OSD_Entry *pageTop;              // Points to top entry of the current page
-OSD_Entry *nextPage;             // Only 2 pages are allowed (for now)
+OSD_Entry *pageTopAlt;           // Only 2 pages are allowed (for now)
-uint8_t maxRow;                  // Max row in a page
+uint8_t maxRow;                  // Max row in the current page
 
-int8_t cursorRow;
+int8_t entryPos;                 // Absolute position of the cursor
+int8_t cursorRow;                // Position of the cursor relative to pageTop
+
+// Broken menu substitution
+
+char menuErrLabel[21 + 1];
+
+OSD_Entry menuErrEntries[] = {
+    { "BROKEN MENU", OME_Label, NULL, NULL, 0 },
+    { menuErrLabel, OME_String, NULL, NULL, 0 },
+    { "BACK", OME_Back, NULL, NULL, 0 },
+    { NULL, OME_END, NULL, NULL, 0 }
+};
+
+CMS_Menu menuErr = {
+    "MENU CONTENT BROKEN",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuErrEntries,
+};
 
 // Stick/key detection
 
@@ -190,7 +213,7 @@ void cmsUpdateMaxRow(displayPort_t *instance)
     OSD_Entry *ptr;
 
     maxRow = 0;
-    for (ptr = currentMenu; ptr->type != OME_END; ptr++)
+    for (ptr = pageTop; ptr->type != OME_END; ptr++)
         maxRow++;
 
     if (maxRow > MAX_MENU_ITEMS(instance))
@@ -355,6 +378,12 @@ int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool dr
     case OME_END:
     case OME_Back:
         break;
+    case OME_MENU:
+#ifdef CMS_MENU_DEBUG
+        // Shouldn't happen. Notify creator of this menu content.
+        cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT");
+#endif
+        break;
     }
 
     return cnt;
@@ -373,20 +402,20 @@ void cmsDrawMenu(displayPort_t *pDisplay)
 
     uint32_t room = displayTxBytesFree(pDisplay);
 
-    if (!currentMenu)
+    if (!pageTop)
         return;
 
     if (pDisplay->cleared) {
-        for (p = currentMenu, i= 0; p->type != OME_END; p++, i++) {
+        for (p = pageTop, i= 0; p->type != OME_END; p++, i++) {
             SET_PRINTLABEL(p);
             SET_PRINTVALUE(p);
         }
 
         if (i > MAX_MENU_ITEMS(pDisplay)) // max per page
         {
-            nextPage = currentMenu + MAX_MENU_ITEMS(pDisplay);
+            pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay);
-            if (nextPage->type == OME_END)
+            if (pageTopAlt->type == OME_END)
-                nextPage = NULL;
+                pageTopAlt = NULL;
         }
 
         pDisplay->cleared = false;
@@ -394,7 +423,7 @@ void cmsDrawMenu(displayPort_t *pDisplay)
 
     // Cursor manipulation
 
-    while ((currentMenu + cursorRow)->type == OME_Label) // skip label
+    while ((pageTop + cursorRow)->type == OME_Label) // skip label
         cursorRow++;
 
     if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) {
@@ -413,7 +442,7 @@ void cmsDrawMenu(displayPort_t *pDisplay)
         return;
 
     // Print text labels
-    for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
+    for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
         if (IS_PRINTLABEL(p)) {
             room -= displayWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text);
             CLR_PRINTLABEL(p);
@@ -427,7 +456,7 @@ void cmsDrawMenu(displayPort_t *pDisplay)
     // XXX Polled values at latter positions in the list may not be
     // XXX printed if not enough room in the middle of the list.
 
-    for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
+    for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
         if (IS_PRINTVALUE(p)) {
             room -= cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled);
             if (room < 30)
@@ -438,25 +467,39 @@ void cmsDrawMenu(displayPort_t *pDisplay)
 
 long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
 {
-    if (ptr) {
+    CMS_Menu *pMenu = (CMS_Menu *)ptr;
-        // XXX (jflyper): This can be avoided by adding pre- and post-
+
-        // XXX (or onEnter and onExit) functions?
+    if (pMenu) {
-        if (ptr == cmsx_menuPid)
+#ifdef CMS_MENU_DEBUG
-            cmsx_PidRead();
+        if (pMenu->GUARD_type != OME_MENU) {
-        if (ptr == cmsx_menuRateExpo)
+            // ptr isn't pointing to a CMS_Menu.
-            cmsx_RateExpoRead();
+            if (pMenu->GUARD_type < OME_MENU) {
+                strncpy(menuErrLabel, pMenu->GUARD_text, 21);
+            } else {
+                strncpy(menuErrLabel, "LABEL UNKNOWN", 21);
+            }
+            pMenu = &menuErr;
+        }
+#endif
 
         // Stack the current menu and move to a new menu.
-        // The (ptr == curretMenu) case occurs when reopening for display sw
+        // The (pMenu == curretMenu) case occurs when reopening for display sw
 
-        if ((OSD_Entry *)ptr != currentMenu) {
+        if (pMenu != currentMenu) {
             menuStack[menuStackIdx] = currentMenu;
+            cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value
             menuStackHistory[menuStackIdx] = cursorRow;
             menuStackIdx++;
-            currentMenu = (OSD_Entry *)ptr;
+
+            currentMenu = (CMS_Menu *)ptr;
             cursorRow = 0;
+
+            if (pMenu->onEnter)
+                pMenu->onEnter();
         }
 
+        pageTop = currentMenu->entries;
+
         displayClear(pDisplay);
         cmsUpdateMaxRow(pDisplay);
     }
@@ -466,22 +509,22 @@ long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
 
 long cmsMenuBack(displayPort_t *pDisplay)
 {
-    // becasue pids and rates may be stored in profiles we need some thicks to manipulate it
+    if (currentMenu->onExit)
-    // hack to save pid profile
+        currentMenu->onExit();
-    if (currentMenu == cmsx_menuPid)
-        cmsx_PidWriteback();
-
-    // hack - save rate config for current profile
-    if (currentMenu == cmsx_menuRateExpo)
-        cmsx_RateExpoWriteback();
 
     if (menuStackIdx) {
         displayClear(pDisplay);
         menuStackIdx--;
-        nextPage = NULL;
         currentMenu = menuStack[menuStackIdx];
         cursorRow = menuStackHistory[menuStackIdx];
+        pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow()
         cmsUpdateMaxRow(pDisplay);
+        if (cursorRow > maxRow) {
+            pageTopAlt = currentMenu->entries;
+            pageTop = pageTopAlt + maxRow + 1;
+            cursorRow -= (maxRow + 1);
+            cmsUpdateMaxRow(pDisplay);
+        }
     }
 
     return 0;
@@ -496,8 +539,7 @@ void cmsMenuOpen(void)
         cmsInMenu = true;
         DISABLE_ARMING_FLAG(OK_TO_ARM);
         initfunc = cmsDeviceSelectCurrent(); 
-        cmsx_FeatureRead();
+        currentMenu = &menuMain;
-        currentMenu = &menuMain[0];
     } else {
         // Switch display
         displayClose(&currentDisplay);
@@ -512,6 +554,18 @@ void cmsMenuOpen(void)
     cmsMenuChange(&currentDisplay, currentMenu);
 }
 
+void cmsTraverseGlobalExit(CMS_Menu *pMenu)
+{
+    OSD_Entry *p;
+
+    for (p = pMenu->entries; p->type != OME_END ; p++)
+        if (p->type == OME_Submenu)
+            cmsTraverseGlobalExit(p->data);
+
+    if (pMenu->onGlobalExit)
+        pMenu->onGlobalExit();
+}
+
 long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
 {
     if (ptr) {
@@ -524,8 +578,10 @@ long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
         stopPwmAllMotors();
         delay(200);
 
-        // save local variables to configuration
+        cmsTraverseGlobalExit(&menuMain);
-        cmsx_FeatureWriteback();
+
+        if (currentMenu->onExit)
+            currentMenu->onExit();
     }
 
     cmsInMenu = false;
@@ -558,11 +614,11 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
         if (cursorRow < maxRow) {
             cursorRow++;
         } else {
-            if (nextPage) { // we have another page
+            if (pageTopAlt) { // we have another page
                 displayClear(pDisplay);
-                p = nextPage;
+                p = pageTopAlt;
-                nextPage = currentMenu;
+                pageTopAlt = pageTop;
-                currentMenu = (OSD_Entry *)p;
+                pageTop = (OSD_Entry *)p;
                 cmsUpdateMaxRow(pDisplay);
             }
             cursorRow = 0;    // Goto top in any case
@@ -572,15 +628,15 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
     if (key == KEY_UP) {
         cursorRow--;
 
-        if ((currentMenu + cursorRow)->type == OME_Label && cursorRow > 0)
+        if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0)
             cursorRow--;
 
-        if (cursorRow == -1 || (currentMenu + cursorRow)->type == OME_Label) {
+        if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) {
-            if (nextPage) {
+            if (pageTopAlt) {
                 displayClear(pDisplay);
-                p = nextPage;
+                p = pageTopAlt;
-                nextPage = currentMenu;
+                pageTopAlt = pageTop;
-                currentMenu = (OSD_Entry *)p;
+                pageTop = (OSD_Entry *)p;
                 cmsUpdateMaxRow(pDisplay);
             }
             cursorRow = maxRow;    // Goto bottom in any case
@@ -590,7 +646,7 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
     if (key == KEY_DOWN || key == KEY_UP)
         return res;
 
-    p = currentMenu + cursorRow;
+    p = pageTop + cursorRow;
 
     switch (p->type) {
         case OME_Submenu:
@@ -710,6 +766,9 @@ uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
         case OME_Label:
         case OME_END:
             break;
+        case OME_MENU:
+            // Shouldn't happen
+            break;
     }
     return res;
 }
@@ -753,7 +812,7 @@ void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
             key = KEY_RIGHT;
             rcDelay = BUTTON_TIME;
         }
-        else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW
+        else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != &cmsx_menuRc) // this menu is used to check transmitter signals so can't exit using YAW
         {
             key = KEY_ESC;
             rcDelay = BUTTON_TIME;
@@ -794,7 +853,7 @@ void cmsHandler(uint32_t currentTime)
 
 void cmsInit(void)
 {
-    cmsx_InfoInit();
+    //cmsx_InfoInit();
 }
 
 //
@@ -808,7 +867,7 @@ static char infoTargetName[] = __TARGET__;
 
 #include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere
 
-OSD_Entry menuInfo[] = {
+OSD_Entry menuInfoEntries[] = {
     { "--- INFO ---", OME_Label, NULL, NULL, 0 },
     { "FWID", OME_String, NULL, BETAFLIGHT_IDENTIFIER, 0 },
     { "FWVER", OME_String, NULL, FC_VERSION_STRING, 0 },
@@ -818,7 +877,16 @@ OSD_Entry menuInfo[] = {
     { NULL, OME_END, NULL, NULL, 0 }
 };
 
-void cmsx_InfoInit(void)
+CMS_Menu menuInfo = {
+    "MENUINFO",
+    OME_MENU,
+    cmsx_InfoInit,
+    NULL,
+    NULL,
+    menuInfoEntries,
+};
+
+long cmsx_InfoInit(void)
 {
     for (int i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
         if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
@@ -826,16 +894,18 @@ void cmsx_InfoInit(void)
         else
             infoGitRev[i] = shortGitRevision[i];
     }
+
+    return 0;
 }
 
 // Features
 
-OSD_Entry menuFeatures[] =
+OSD_Entry menuFeaturesEntries[] =
 {
     {"--- FEATURES ---", OME_Label, NULL, NULL, 0},
-    {"BLACKBOX", OME_Submenu, cmsMenuChange, cmsx_menuBlackbox, 0},
+    {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0},
 #if defined(VTX) || defined(USE_RTC6705)
-    {"VTX", OME_Submenu, cmsMenuChange, cmsx_menuVtx, 0},
+    {"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0},
 #endif // VTX || USE_RTC6705
 #ifdef LED_STRIP
     {"LED STRIP", OME_Submenu, cmsMenuChange, &cmsx_menuLedstrip, 0},
@@ -844,24 +914,45 @@ OSD_Entry menuFeatures[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
+CMS_Menu menuFeatures = {
+    "MENUFEATURES",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuFeaturesEntries,
+};
+
 // Main
 
-OSD_Entry menuMain[] =
+OSD_Entry menuMainEntries[] =
 {
     {"--- MAIN MENU ---", OME_Label, NULL, NULL, 0},
-    {"CFG&IMU", OME_Submenu, cmsMenuChange, cmsx_menuImu, 0},
+    {"CFG&IMU", OME_Submenu, cmsMenuChange, &cmsx_menuImu, 0},
-    {"FEATURES", OME_Submenu, cmsMenuChange, menuFeatures, 0},
+    {"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures, 0},
 #ifdef OSD
-    {"SCR LAYOUT", OME_Submenu, cmsMenuChange, cmsx_menuOsdLayout, 0},
+    {"SCR LAYOUT", OME_Submenu, cmsMenuChange, &cmsx_menuOsdLayout, 0},
-    {"ALARMS", OME_Submenu, cmsMenuChange, cmsx_menuAlarms, 0},
+    {"ALARMS", OME_Submenu, cmsMenuChange, &cmsx_menuAlarms, 0},
 #endif
-    {"FC&FW INFO", OME_Submenu, cmsMenuChange, menuInfo, 0},
+    {"FC&FW INFO", OME_Submenu, cmsMenuChange, &menuInfo, 0},
+    {"FC&FW INFO1", OME_Submenu, cmsMenuChange, &menuInfo, 0},
+    {"FC&FW INFO2", OME_Submenu, cmsMenuChange, &menuInfo, 0},
     {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},
     {"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0},
     {NULL,OME_END, NULL, NULL, 0}
 };
 
-void cmsx_FeatureRead(void)
+CMS_Menu menuMain = {
+    "MENUMAIN",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuMainEntries,
+};
+
+#if 0
+long cmsx_FeatureRead(void)
 {
     cmsx_Blackbox_FeatureRead();
 
@@ -874,9 +965,11 @@ void cmsx_FeatureRead(void)
     cmsx_Vtx_FeatureRead();
     cmsx_Vtx_ConfigRead();
 #endif // VTX || USE_RTC6705
+
+    return 0;
 }
 
-void cmsx_FeatureWriteback(void)
+long cmsx_FeatureWriteback(void)
 {
     cmsx_Blackbox_FeatureWriteback();
 
@@ -890,6 +983,9 @@ void cmsx_FeatureWriteback(void)
 #endif // VTX || USE_RTC6705
 
     saveConfigAndNotify();
+
+    return 0;
 }
+#endif
 
 #endif // CMS

src/main/io/cms_blackbox.c

@@ -1,7 +1,7 @@
 //
 // CMS things for blackbox and flashfs.
-// Should be part of blackbox.c (or new blackbox/blackbox_cms.c) and io/flashfs.c
 // 
+
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
@@ -48,9 +48,26 @@ long cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr)
 
 uint8_t cmsx_FeatureBlackbox;
 
+long cmsx_Blackbox_FeatureRead(void)
+{
+    cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
+
+    return 0;
+}
+
+long cmsx_Blackbox_FeatureWriteback(void)
+{
+    if (cmsx_FeatureBlackbox)
+        featureSet(FEATURE_BLACKBOX);
+    else
+        featureClear(FEATURE_BLACKBOX);
+
+    return 0;
+}
+
 OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1};
 
-OSD_Entry cmsx_menuBlackbox[] =
+OSD_Entry cmsx_menuBlackboxEntries[] =
 {
     {"--- BLACKBOX ---", OME_Label, NULL, NULL, 0},
     {"ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0},
@@ -62,16 +79,12 @@ OSD_Entry cmsx_menuBlackbox[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
-void cmsx_Blackbox_FeatureRead(void)
+CMS_Menu cmsx_menuBlackbox = {
-{
+    "MENUBB",
-    cmsx_FeatureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
+    OME_MENU,
-}
+    cmsx_Blackbox_FeatureRead,
-
+    NULL,
-void cmsx_Blackbox_FeatureWriteback(void)
+    cmsx_Blackbox_FeatureWriteback,
-{
+    cmsx_menuBlackboxEntries,
-    if (cmsx_FeatureBlackbox)
+};
-        featureSet(FEATURE_BLACKBOX);
-    else
-        featureClear(FEATURE_BLACKBOX);
-}
 #endif

src/main/io/cms_blackbox.h

@@ -1,4 +1 @@
-extern OSD_Entry cmsx_menuBlackbox[];
+extern CMS_Menu cmsx_menuBlackbox;
-
-void cmsx_Blackbox_FeatureRead(void);
-void cmsx_Blackbox_FeatureWriteback(void);

src/main/io/cms_imu.c

@@ -47,7 +47,7 @@ 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)
+long cmsx_PidRead(void)
 {
     uint8_t i;
 
@@ -59,9 +59,11 @@ void cmsx_PidRead(void)
     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];
+
+    return 0;
 }
 
-void cmsx_PidWriteback(void)
+long cmsx_PidWriteback(void)
 {
     uint8_t i;
 
@@ -74,9 +76,11 @@ void cmsx_PidWriteback(void)
     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];
+
+    return 0;
 }
 
-OSD_Entry cmsx_menuPid[] =
+OSD_Entry cmsx_menuPidEntries[] =
 {
     {"--- PID ---", OME_Label, NULL, NULL, 0},
     {"ROLL P", OME_UINT8, NULL, &entryRollP, 0},
@@ -95,6 +99,15 @@ OSD_Entry cmsx_menuPid[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
+CMS_Menu cmsx_menuPid = {
+    "MENUPID",
+    OME_MENU,
+    cmsx_PidRead,
+    cmsx_PidWriteback,
+    NULL,
+    cmsx_menuPidEntries,
+};
+
 //
 // Rate & Expo
 //
@@ -112,17 +125,21 @@ 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()
+long cmsx_RateExpoRead(void)
 {
     memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t));
+
+    return 0;
 }
 
-void cmsx_RateExpoWriteback()
+long cmsx_RateExpoWriteback(void)
 {
     memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t));
+
+    return 0;
 }
 
-OSD_Entry cmsx_menuRateExpo[] =
+OSD_Entry cmsx_menuRateExpoEntries[] =
 {
     {"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0},
     {"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0},
@@ -140,6 +157,16 @@ OSD_Entry cmsx_menuRateExpo[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
+CMS_Menu cmsx_menuRateExpo = {
+    "MENURTEX",
+    OME_MENU,
+    cmsx_RateExpoRead,
+    cmsx_RateExpoWriteback,
+    NULL,
+    cmsx_menuRateExpoEntries,
+};
+
+
 //
 // RC preview
 //
@@ -152,7 +179,7 @@ static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0};
 static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0};
 static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0};
 
-OSD_Entry cmsx_menuRc[] =
+OSD_Entry cmsx_menuRcEntries[] =
 {
     {"--- RC PREV ---", OME_Label, NULL, NULL, 0},
     {"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0},
@@ -167,6 +194,16 @@ OSD_Entry cmsx_menuRc[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
+CMS_Menu cmsx_menuRc = {
+    "MENURC",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    cmsx_menuRcEntries,
+};
+
+
 //
 // Misc
 //
@@ -178,7 +215,7 @@ OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit,
 OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1};
 OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1};
 
-OSD_Entry menuImuMisc[]=
+OSD_Entry menuImuMiscEntries[]=
 {
     {"--- MISC ---", OME_Label, NULL, NULL, 0},
     {"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0},
@@ -192,15 +229,33 @@ OSD_Entry menuImuMisc[]=
     {NULL, OME_END, NULL, NULL, 0}
 };
 
-OSD_Entry cmsx_menuImu[] =
+CMS_Menu menuImuMisc = {
+    "MENUIMU",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuImuMiscEntries,
+};
+
+OSD_Entry cmsx_menuImuEntries[] =
 {
     {"--- CFG.IMU ---", OME_Label, NULL, NULL, 0},
     {"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0},
-    {"PID", OME_Submenu, cmsMenuChange, cmsx_menuPid, 0},
+    {"PID", OME_Submenu, cmsMenuChange, &cmsx_menuPid, 0},
-    {"RATE&RXPO", OME_Submenu, cmsMenuChange, cmsx_menuRateExpo, 0},
+    {"RATE&RXPO", OME_Submenu, cmsMenuChange, &cmsx_menuRateExpo, 0},
-    {"RC PREV", OME_Submenu, cmsMenuChange, cmsx_menuRc, 0},
+    {"RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRc, 0},
-    {"MISC", OME_Submenu, cmsMenuChange, menuImuMisc, 0},
+    {"MISC", OME_Submenu, cmsMenuChange, &menuImuMisc, 0},
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
 };
+
+CMS_Menu cmsx_menuImu = {
+    "MENUIMU",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    cmsx_menuImuEntries,
+};
 #endif // CMS

src/main/io/cms_imu.h

@@ -1,13 +1,4 @@
-extern OSD_Entry cmsx_menuImu[];
+extern CMS_Menu cmsx_menuImu;
-
-// All of below should be gone.
-
-extern OSD_Entry cmsx_menuPid[];
-extern OSD_Entry cmsx_menuRc[];
-extern OSD_Entry cmsx_menuRateExpo[];
-
-void cmsx_PidRead(void);
-void cmsx_PidWriteback(void);
-void cmsx_RateExpoRead(void);
-void cmsx_RateExpoWriteback(void);
 
+// This should be gone
+extern CMS_Menu cmsx_menuRc;

src/main/io/cms_ledstrip.c

@@ -76,7 +76,39 @@ uint8_t cmsx_FeatureLedstrip;
 
 OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]};
 
-OSD_Entry cmsx_menuLedstrip[] =
+long cmsx_Ledstrip_FeatureRead(void)
+{
+    cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0;
+
+    return 0;
+}
+
+long cmsx_Ledstrip_FeatureWriteback(void)
+{
+    if (cmsx_FeatureLedstrip)
+        featureSet(FEATURE_LED_STRIP);
+    else
+        featureClear(FEATURE_LED_STRIP);
+
+    return 0;
+}
+
+long cmsx_Ledstrip_ConfigRead(void)
+{
+    cmsx_GetLedColor();
+
+    return 0;
+}
+
+long cmsx_Ledstrip_onEnter(void)
+{
+    cmsx_Ledstrip_FeatureRead();
+    cmsx_Ledstrip_ConfigRead();
+
+    return 0;
+}
+
+OSD_Entry cmsx_menuLedstripEntries[] =
 {
     {"--- LED STRIP ---", OME_Label, NULL, NULL, 0},
     {"ENABLED", OME_Bool, NULL, &cmsx_FeatureLedstrip, 0},
@@ -85,23 +117,13 @@ OSD_Entry cmsx_menuLedstrip[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
-void cmsx_Ledstrip_FeatureRead(void)
+CMS_Menu cmsx_menuLedstrip = {
-{
+    "MENULED",
-    cmsx_FeatureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0;
+    OME_MENU,
-}
+    cmsx_Ledstrip_onEnter,
-
+    NULL,
-void cmsx_Ledstrip_FeatureWriteback(void)
+    cmsx_Ledstrip_FeatureWriteback,
-{
+    cmsx_menuLedstripEntries,
-    if (cmsx_FeatureLedstrip)
+};
-        featureSet(FEATURE_LED_STRIP);
-    else
-        featureClear(FEATURE_LED_STRIP);
-}
-
-void cmsx_Ledstrip_ConfigRead(void)
-{
-    cmsx_GetLedColor();
-}
-
 #endif // LED_STRIP
 #endif // CMS

src/main/io/cms_ledstrip.h

@@ -1,6 +1 @@
-extern OSD_Entry cmsx_menuLedstrip[];
+extern CMS_Menu cmsx_menuLedstrip;
-
-void cmsx_Ledstrip_FeatureRead(void);
-void cmsx_Ledstrip_FeatureWriteback(void);
-
-void cmsx_Ledstrip_ConfigRead(void);

src/main/io/cms_osd.h

@@ -1,2 +1,2 @@
-extern OSD_Entry cmsx_menuAlarms[];
+extern CMS_Menu cmsx_menuAlarms;
-extern OSD_Entry cmsx_menuOsdLayout[];
+extern CMS_Menu cmsx_menuOsdLayout;

src/main/io/cms_types.h

@@ -5,7 +5,7 @@
 
 #pragma once
 
-typedef long (*OSDMenuFuncPtr)(displayPort_t *, void *);
+typedef long (*CMSEntryFuncPtr)(displayPort_t *, void *);
 
 //type of elements
 
@@ -29,13 +29,16 @@ typedef enum
 #endif
     OME_TAB,
     OME_END,
+
+    // Debug aid
+    OME_MENU
 } OSD_MenuElement;
 
 typedef struct
 {
     char *text;
     OSD_MenuElement type;
-    OSDMenuFuncPtr func;
+    CMSEntryFuncPtr func;
     void *data;
     uint8_t flags;
 } OSD_Entry;
@@ -52,6 +55,20 @@ typedef struct
 #define SET_PRINTLABEL(p) { (p)->flags |= PRINT_LABEL; }
 #define CLR_PRINTLABEL(p) { (p)->flags &= ~PRINT_LABEL; }
 
+typedef long (*CMSMenuFuncPtr)(void);
+
+typedef struct
+{
+    // These two are debug aids for menu content creators.
+    char *GUARD_text;
+    OSD_MenuElement GUARD_type;
+
+    CMSMenuFuncPtr onEnter;
+    CMSMenuFuncPtr onExit;
+    CMSMenuFuncPtr onGlobalExit;
+    OSD_Entry *entries;
+} CMS_Menu;
+
 typedef struct
 {
     uint8_t *val;

src/main/io/cms_vtx.c

@@ -20,39 +20,6 @@
 
 uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;
 
-static const char * const vtxBandNames[] = {
-    "BOSCAM A",
-    "BOSCAM B",
-    "BOSCAM E",
-    "FATSHARK",
-    "RACEBAND",
-};
-
-OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]};
-OSD_UINT8_t entryVtxChannel =  {&cmsx_vtxChannel, 1, 8, 1};
-
-#ifdef VTX
-OSD_UINT8_t entryVtxMode =  {&masterConfig.vtx_mode, 0, 2, 1};
-OSD_UINT16_t entryVtxMhz =  {&masterConfig.vtx_mhz, 5600, 5950, 1};
-#endif // VTX
-
-OSD_Entry cmsx_menuVtx[] =
-{
-    {"--- VTX ---", OME_Label, NULL, NULL, 0},
-    {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0},
-#ifdef VTX
-    {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0},
-    {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0},
-#endif // VTX
-    {"BAND", OME_TAB, NULL, &entryVtxBand, 0},
-    {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0},
-#ifdef USE_RTC6705
-    {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0},
-#endif // USE_RTC6705
-    {"BACK", OME_Back, NULL, NULL, 0},
-    {NULL, OME_END, NULL, NULL, 0}
-};
-
 void cmsx_Vtx_FeatureRead(void)
 {
     cmsx_featureVtx = feature(FEATURE_VTX) ? 1 : 0;
@@ -66,6 +33,17 @@ void cmsx_Vtx_FeatureWriteback(void)
         featureClear(FEATURE_VTX);
 }
 
+static const char * const vtxBandNames[] = {
+    "BOSCAM A",
+    "BOSCAM B",
+    "BOSCAM E",
+    "FATSHARK",
+    "RACEBAND",
+};
+
+OSD_TAB_t entryVtxBand = {&cmsx_vtxBand,4,&vtxBandNames[0]};
+OSD_UINT8_t entryVtxChannel =  {&cmsx_vtxChannel, 1, 8, 1};
+
 void cmsx_Vtx_ConfigRead(void)
 {
 #ifdef VTX
@@ -91,6 +69,36 @@ void cmsx_Vtx_ConfigWriteback(void)
 #endif // USE_RTC6705
 }
 
-#endif // VTX || USE_RTC6705
+#ifdef VTX
+OSD_UINT8_t entryVtxMode =  {&masterConfig.vtx_mode, 0, 2, 1};
+OSD_UINT16_t entryVtxMhz =  {&masterConfig.vtx_mhz, 5600, 5950, 1};
+#endif // VTX
 
+OSD_Entry cmsx_menuVtxEntries[] =
+{
+    {"--- VTX ---", OME_Label, NULL, NULL, 0},
+    {"ENABLED", OME_Bool, NULL, &cmsx_featureVtx, 0},
+#ifdef VTX
+    {"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0},
+    {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0},
+#endif // VTX
+    {"BAND", OME_TAB, NULL, &entryVtxBand, 0},
+    {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0},
+#ifdef USE_RTC6705
+    {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0},
+#endif // USE_RTC6705
+    {"BACK", OME_Back, NULL, NULL, 0},
+    {NULL, OME_END, NULL, NULL, 0}
+};
+
+CMS_Menu cmsx_menuVtx = {
+    "MENUVTX",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    cmsx_menuVtxEntries,
+};
+
+#endif // VTX || USE_RTC6705
 #endif // CMS

src/main/io/cms_vtx.h

@@ -1,7 +1 @@
-extern OSD_Entry cmsx_menuVtx[];
+extern CMS_Menu cmsx_menuVtx;
-
-void cmsx_Vtx_FeatureRead(void);
-void cmsx_Vtx_FeatureWriteback(void);
-
-void cmsx_Vtx_ConfigRead(void);
-void cmsx_Vtx_ConfigWriteback(void);

src/main/io/osd.c

@@ -663,7 +663,7 @@ OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000
 OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1};
 OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1};
 
-OSD_Entry cmsx_menuAlarms[] =
+OSD_Entry cmsx_menuAlarmsEntries[] =
 {
     {"--- ALARMS ---", OME_Label, NULL, NULL, 0},
     {"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0},
@@ -674,7 +674,16 @@ OSD_Entry cmsx_menuAlarms[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
-OSD_Entry menuOsdActiveElems[] =
+CMS_Menu cmsx_menuAlarms = {
+    "MENUALARMS",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    cmsx_menuAlarmsEntries,
+};
+
+OSD_Entry menuOsdActiveElemsEntries[] =
 {
     {"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0},
     {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0},
@@ -700,12 +709,30 @@ OSD_Entry menuOsdActiveElems[] =
     {NULL, OME_END, NULL, NULL, 0}
 };
 
-OSD_Entry cmsx_menuOsdLayout[] =
+CMS_Menu menuOsdActiveElems = {
+    "MENUOSDACT",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    menuOsdActiveElemsEntries,
+};
+
+OSD_Entry cmsx_menuOsdLayoutEntries[] =
 {
     {"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0},
-    {"ACTIVE ELEM.", OME_Submenu, cmsMenuChange, &menuOsdActiveElems[0], 0},
+    {"ACTIVE ELEM", OME_Submenu, cmsMenuChange, &menuOsdActiveElems, 0},
     {"BACK", OME_Back, NULL, NULL, 0},
     {NULL, OME_END, NULL, NULL, 0}
 };
 
+CMS_Menu cmsx_menuOsdLayout = {
+    "MENULAYOUT",
+    OME_MENU,
+    NULL,
+    NULL,
+    NULL,
+    cmsx_menuOsdLayoutEntries,
+};
+
 #endif // OSD