src/main/cms/cms.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  Original OSD code created by Marcin Baliniak
  23  OSD-CMS separation by jflyper
  24  CMS-displayPort separation by jflyper and martinbudden
  25  */
  26
  27 //#define CMS_PAGE_DEBUG // For multi-page/menu debugging
  28 //#define CMS_MENU_DEBUG // For external menu content creators
  29
  30 #include <stdbool.h>
  31 #include <stdint.h>
  32 #include <string.h>
  33 #include <ctype.h>
  34
  35 #include "platform.h"
  36
  37 #ifdef USE_CMS
  38
  39 #include "build/build_config.h"
  40 #include "build/debug.h"
  41 #include "build/version.h"
  42
  43 #include "cms/cms.h"
  44 #include "cms/cms_menu_main.h"
  45 #include "cms/cms_menu_saveexit.h"
  46 #include "cms/cms_types.h"
  47
  48 #include "common/maths.h"
  49 #include "common/typeconversion.h"
  50
  51 #include "config/config.h"
  52 #include "config/feature.h"
  53
  54 #include "drivers/motor.h"
  55 #include "drivers/osd_symbols.h"
  56 #include "drivers/system.h"
  57 #include "drivers/time.h"
  58
  59 #include "fc/rc_controls.h"
  60 #include "fc/runtime_config.h"
  61
  62 #include "flight/mixer.h"
  63
  64 #include "io/rcdevice_cam.h"
  65 #include "io/usb_cdc_hid.h"
  66
  67 #include "pg/pg.h"
  68 #include "pg/pg_ids.h"
  69 #include "pg/rx.h"
  70
  71 #include "osd/osd.h"
  72
  73 #include "rx/rx.h"
  74
  75 // DisplayPort management
  76
  77 #ifndef CMS_MAX_DEVICE
  78 #define CMS_MAX_DEVICE 4
  79 #endif
  80
  81 #define CMS_MENU_STACK_LIMIT 10
  82
  83 displayPort_t *pCurrentDisplay;
  84
  85 static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE];
  86 static unsigned cmsDeviceCount;
  87 static int cmsCurrentDevice = -1;
  88 #ifdef USE_OSD
  89 static unsigned int osdProfileCursor = 1;
  90 #endif
  91
  92 int menuChainBack;
  93
  94 bool cmsDisplayPortRegister(displayPort_t *pDisplay)
  95 {
  96     if (!pDisplay || cmsDeviceCount >= CMS_MAX_DEVICE) {
  97         return false;
  98     }
  99
 100     cmsDisplayPorts[cmsDeviceCount++] = pDisplay;
 101
 102     return true;
 103 }
 104
 105 static displayPort_t *cmsDisplayPortSelectCurrent(void)
 106 {
 107     if (cmsDeviceCount == 0) {
 108         return NULL;
 109     }
 110
 111     if (cmsCurrentDevice < 0) {
 112         cmsCurrentDevice = 0;
 113     }
 114
 115     return cmsDisplayPorts[cmsCurrentDevice];
 116 }
 117
 118 static displayPort_t *cmsDisplayPortSelectNext(void)
 119 {
 120     if (cmsDeviceCount == 0) {
 121         return NULL;
 122     }
 123
 124     cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay
 125
 126     return cmsDisplayPorts[cmsCurrentDevice];
 127 }
 128
 129 bool cmsDisplayPortSelect(displayPort_t *instance)
 130 {
 131     for (unsigned i = 0; i < cmsDeviceCount; i++) {
 132         if (cmsDisplayPortSelectNext() == instance) {
 133             return true;
 134         }
 135     }
 136     return false;
 137 }
 138
 139 #define CMS_POLL_INTERVAL_US   100000   // Interval of polling dynamic values (microsec)
 140
 141 // XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted
 142 // dynamically depending on size of the active output device,
 143 // or statically to accomodate sizes of all supported devices.
 144 //
 145 // Device characteristics
 146 // OLED
 147 //   21 cols x 8 rows
 148 //     128x64 with 5x7 (6x8) : 21 cols x 8 rows
 149 // MAX7456 (PAL)
 150 //   30 cols x 16 rows
 151 // MAX7456 (NTSC)
 152 //   30 cols x 13 rows
 153 // HoTT Telemetry Screen
 154 //   21 cols x 8 rows
 155 //
 156 // Spektrum SRXL Telemtry Textgenerator
 157 //   13 cols x 9 rows, top row printed as a Bold Heading
 158 //   Needs the "smallScreen" adaptions
 159
 160 #define CMS_MAX_ROWS 16
 161
 162 #define NORMAL_SCREEN_MIN_COLS 18      // Less is a small screen
 163 static bool    smallScreen;
 164 static uint8_t leftMenuColumn;
 165 static uint8_t rightMenuColumn;
 166 static uint8_t maxMenuItems;
 167 static uint8_t linesPerMenuItem;
 168 static cms_key_e externKey = CMS_KEY_NONE;
 169 static bool osdElementEditing = false;
 170
 171 bool cmsInMenu = false;
 172
 173 typedef struct cmsCtx_s {
 174     const CMS_Menu *menu;         // menu for this context
 175     uint8_t page;                 // page in the menu
 176     int8_t cursorRow;             // cursorRow in the page
 177 } cmsCtx_t;
 178
 179 static cmsCtx_t menuStack[CMS_MENU_STACK_LIMIT];
 180 static uint8_t menuStackIdx = 0;
 181
 182 static int8_t pageCount;         // Number of pages in the current menu
 183 static const OSD_Entry *pageTop;       // First entry for the current page
 184 static uint8_t pageMaxRow;       // Max row in the current page
 185
 186 static cmsCtx_t currentCtx;
 187
 188 static bool saveMenuInhibited = false;
 189
 190 #ifdef CMS_MENU_DEBUG // For external menu content creators
 191
 192 static char menuErrLabel[21 + 1] = "RANDOM DATA";
 193
 194 static OSD_Entry menuErrEntries[] = {
 195     { "BROKEN MENU", OME_Label, NULL, NULL, 0 },
 196     { menuErrLabel, OME_Label, NULL, NULL, 0 },
 197     { "BACK", OME_Back, NULL, NULL, 0 },
 198     { NULL, OME_END, NULL, NULL, 0 }
 199 };
 200
 201 static CMS_Menu menuErr = {
 202     "MENUERR",
 203     OME_MENU,
 204     NULL,
 205     NULL,
 206     NULL,
 207     menuErrEntries,
 208 };
 209 #endif
 210
 211 #ifdef CMS_PAGE_DEBUG
 212 #define cmsPageDebug() { \
 213     debug[0] = pageCount; \
 214     debug[1] = currentCtx.page; \
 215     debug[2] = pageMaxRow; \
 216     debug[3] = currentCtx.cursorRow; } struct _dummy
 217 #endif
 218
 219 static void cmsUpdateMaxRow(displayPort_t *instance)
 220 {
 221     UNUSED(instance);
 222     pageMaxRow = 0;
 223
 224     for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) {
 225         pageMaxRow++;
 226     }
 227
 228     if (pageMaxRow > maxMenuItems) {
 229         pageMaxRow = maxMenuItems;
 230     }
 231
 232     if (pageMaxRow > CMS_MAX_ROWS) {
 233         pageMaxRow = CMS_MAX_ROWS;
 234     }
 235
 236     pageMaxRow--;
 237 }
 238
 239 static uint8_t cmsCursorAbsolute(displayPort_t *instance)
 240 {
 241     UNUSED(instance);
 242     return currentCtx.cursorRow + currentCtx.page * maxMenuItems;
 243 }
 244
 245 uint8_t runtimeEntryFlags[CMS_MAX_ROWS] = { 0 };
 246
 247 #define LOOKUP_TABLE_TICKER_START_CYCLES 20   // Task loops for start/end of ticker (1 second delay)
 248 #define LOOKUP_TABLE_TICKER_SCROLL_CYCLES 3   // Task loops for each scrolling step of the ticker (150ms delay)
 249
 250 typedef struct cmsTableTicker_s {
 251     uint8_t loopCounter;
 252     uint8_t state;
 253 } cmsTableTicker_t;
 254
 255 cmsTableTicker_t runtimeTableTicker[CMS_MAX_ROWS];
 256
 257 static void cmsPageSelect(displayPort_t *instance, int8_t newpage)
 258 {
 259     currentCtx.page = (newpage + pageCount) % pageCount;
 260     pageTop = &currentCtx.menu->entries[currentCtx.page * maxMenuItems];
 261     cmsUpdateMaxRow(instance);
 262
 263     const OSD_Entry *p;
 264     int i;
 265     for (p = pageTop, i = 0; (p <= pageTop + pageMaxRow); p++, i++) {
 266         runtimeEntryFlags[i] = p->flags;
 267     }
 268     displayClearScreen(instance);
 269 }
 270
 271 static void cmsPageNext(displayPort_t *instance)
 272 {
 273     cmsPageSelect(instance, currentCtx.page + 1);
 274 }
 275
 276 static void cmsPagePrev(displayPort_t *instance)
 277 {
 278     cmsPageSelect(instance, currentCtx.page - 1);
 279 }
 280
 281 static void cmsFormatFloat(int32_t value, char *floatString)
 282 {
 283     uint8_t k;
 284     // np. 3450
 285
 286     itoa(100000 + value, floatString, 10); // Create string from abs of integer value
 287
 288     // 103450
 289
 290     floatString[0] = floatString[1];
 291     floatString[1] = floatString[2];
 292     floatString[2] = '.';
 293
 294     // 03.450
 295     // usuwam koncowe zera i kropke
 296     // Keep the first decimal place
 297     for (k = 5; k > 3; k--) {
 298         if (floatString[k] == '0' || floatString[k] == '.') {
 299             floatString[k] = 0;
 300         } else {
 301             break;
 302         }
 303     }
 304
 305     // oraz zero wiodonce
 306     if (floatString[0] == '0') {
 307         floatString[0] = ' ';
 308     }
 309 }
 310
 311 // CMS on OSD legacy was to use LEFT aligned values, not the RIGHT way ;-)
 312 #define CMS_OSD_RIGHT_ALIGNED_VALUES
 313
 314 #ifndef CMS_OSD_RIGHT_ALIGNED_VALUES
 315
 316 // Pad buffer to the left, i.e. align left
 317 static void cmsPadRightToSize(char *buf, int size)
 318 {
 319     int i;
 320
 321     for (i = 0 ; i < size ; i++) {
 322         if (buf[i] == 0) {
 323             break;
 324         }
 325     }
 326
 327     for ( ; i < size ; i++) {
 328         buf[i] = ' ';
 329     }
 330
 331     buf[size] = 0;
 332 }
 333 #endif
 334
 335 // Pad buffer to the left, i.e. align right
 336 static void cmsPadLeftToSize(char *buf, int size)
 337 {
 338     int i,j;
 339     int len = strlen(buf);
 340
 341     for (i = size - 1, j = size - len ; i - j >= 0 ; i--) {
 342       buf[i] = buf[i - j];
 343     }
 344
 345     for ( ; i >= 0 ; i--) {
 346       buf[i] = ' ';
 347     }
 348
 349     buf[size] = 0;
 350 }
 351
 352 static void cmsPadToSize(char *buf, int size)
 353 {
 354     // Make absolutely sure the string terminated.
 355     buf[size] = 0x00,
 356
 357 #ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
 358     cmsPadLeftToSize(buf, size);
 359 #else
 360     smallScreen ? cmsPadLeftToSize(buf, size) : cmsPadRightToSize(buf, size);
 361 #endif
 362 }
 363
 364 static int cmsDisplayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *s)
 365 {
 366     char buffer[strlen(s) + 1];
 367     char* b = buffer;
 368     while (*s) {
 369         char c = toupper(*s++);
 370         *b++ = (c < 0x20 || c > 0x5F) ? ' ' : c; // limit to alphanumeric and punctuation
 371     }
 372     *b++ = '\0';
 373
 374     return displayWrite(instance, x, y, attr, buffer);
 375 }
 376
 377 static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row, uint8_t maxSize)
 378 {
 379     int colpos;
 380     int cnt;
 381
 382     cmsPadToSize(buff, maxSize);
 383 #ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
 384     colpos = rightMenuColumn - maxSize;
 385 #else
 386     colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn;
 387 #endif
 388     cnt = cmsDisplayWrite(pDisplay, colpos, row, DISPLAYPORT_ATTR_NONE, buff);
 389     return cnt;
 390 }
 391
 392 static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t row, bool selectedRow, uint8_t *flags, cmsTableTicker_t *ticker)
 393 {
 394     #define CMS_DRAW_BUFFER_LEN 12
 395     #define CMS_TABLE_VALUE_MAX_LEN 30
 396     #define CMS_NUM_FIELD_LEN 5
 397     #define CMS_CURSOR_BLINK_DELAY_MS 500
 398
 399     char buff[CMS_DRAW_BUFFER_LEN +1]; // Make room for null terminator.
 400     char tableBuff[CMS_TABLE_VALUE_MAX_LEN +1];
 401     int cnt = 0;
 402
 403 #ifndef USE_OSD
 404     UNUSED(selectedRow);
 405 #endif
 406
 407     if (smallScreen) {
 408         row++;
 409     }
 410
 411     switch (p->type) {
 412     case OME_String:
 413         if (IS_PRINTVALUE(*flags) && p->data) {
 414             strncpy(buff, p->data, CMS_DRAW_BUFFER_LEN);
 415             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN);
 416             CLR_PRINTVALUE(*flags);
 417         }
 418         break;
 419
 420     case OME_Submenu:
 421     case OME_Funcall:
 422         if (IS_PRINTVALUE(*flags)) {
 423             buff[0]= 0x0;
 424
 425             if (p->type == OME_Submenu && p->func && *flags & OPTSTRING) {
 426
 427                 // Special case of sub menu entry with optional value display.
 428
 429                 const char *str = p->func(pDisplay, p->data);
 430                 strncpy(buff, str, CMS_DRAW_BUFFER_LEN);
 431             } else if (p->type == OME_Funcall && p->data) {
 432                 strncpy(buff, p->data, CMS_DRAW_BUFFER_LEN);
 433             }
 434             strncat(buff, ">", CMS_DRAW_BUFFER_LEN);
 435
 436             row = smallScreen ? row - 1 : row;
 437             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, strlen(buff));
 438             CLR_PRINTVALUE(*flags);
 439         }
 440         break;
 441
 442     case OME_Bool:
 443         if (IS_PRINTVALUE(*flags) && p->data) {
 444             if (*((uint8_t *)(p->data))) {
 445               strcpy(buff, "YES");
 446             } else {
 447               strcpy(buff, "NO ");
 448             }
 449
 450             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3);
 451             CLR_PRINTVALUE(*flags);
 452         }
 453         break;
 454
 455     case OME_TAB:
 456         if (IS_PRINTVALUE(*flags) || IS_SCROLLINGTICKER(*flags)) {
 457             bool drawText = false;
 458             OSD_TAB_t *ptr = p->data;
 459             const int labelLength = strlen(p->text) + 1; // account for the space between label and display data
 460             char *str = (char *)ptr->names[*ptr->val];   // lookup table display text
 461             const int displayLength = strlen(str);
 462
 463             // Calculate the available space to display the lookup table entry based on the
 464             // screen size and the length of the label. Always display at least CMS_DRAW_BUFFER_LEN
 465             // characters to prevent really long labels from overriding the data display.
 466             const int availableSpace = MAX(CMS_DRAW_BUFFER_LEN, rightMenuColumn - labelLength - leftMenuColumn - 1);
 467
 468             if (IS_PRINTVALUE(*flags)) {
 469                 drawText = true;
 470                 ticker->state = 0;
 471                 ticker->loopCounter = 0;
 472                 if (displayLength > availableSpace) {  // table entry text is longer than the available space so start the ticker
 473                     SET_SCROLLINGTICKER(*flags);
 474                 } else {
 475                     CLR_SCROLLINGTICKER(*flags);
 476                 }
 477             } else if (IS_SCROLLINGTICKER(*flags)) {
 478                 ticker->loopCounter++;
 479                 const uint8_t loopLimit = (ticker->state == 0 || ticker->state == (displayLength - availableSpace)) ? LOOKUP_TABLE_TICKER_START_CYCLES : LOOKUP_TABLE_TICKER_SCROLL_CYCLES;
 480                 if (ticker->loopCounter >= loopLimit) {
 481                     ticker->loopCounter = 0;
 482                     drawText = true;
 483                     ticker->state++;
 484                     if (ticker->state > (displayLength - availableSpace)) {
 485                         ticker->state = 0;
 486                     }
 487                 }
 488             }
 489             if (drawText) {
 490                 strncpy(tableBuff, (char *)(str + ticker->state), CMS_TABLE_VALUE_MAX_LEN);
 491                 cnt = cmsDrawMenuItemValue(pDisplay, tableBuff, row, availableSpace);
 492             }
 493             CLR_PRINTVALUE(*flags);
 494         }
 495         break;
 496
 497 #ifdef USE_OSD
 498     case OME_VISIBLE:
 499         if (IS_PRINTVALUE(*flags) && p->data) {
 500             uint16_t *val = (uint16_t *)p->data;
 501             bool cursorBlink = millis() % (2 * CMS_CURSOR_BLINK_DELAY_MS) < CMS_CURSOR_BLINK_DELAY_MS;
 502             for (unsigned x = 1; x < OSD_PROFILE_COUNT + 1; x++) {
 503                 if (VISIBLE_IN_OSD_PROFILE(*val, x)) {
 504                     if (osdElementEditing && cursorBlink && selectedRow && (x == osdProfileCursor)) {
 505                         strcpy(buff + x - 1, " ");
 506                     } else {
 507                         strcpy(buff + x - 1, "X");
 508                     }
 509                 } else {
 510                     if (osdElementEditing && cursorBlink && selectedRow && (x == osdProfileCursor)) {
 511                         strcpy(buff + x - 1, " ");
 512                     } else {
 513                         strcpy(buff + x - 1, "-");
 514                     }
 515                 }
 516             }
 517             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3);
 518             CLR_PRINTVALUE(*flags);
 519         }
 520         break;
 521 #endif
 522
 523     case OME_UINT8:
 524         if (IS_PRINTVALUE(*flags) && p->data) {
 525             OSD_UINT8_t *ptr = p->data;
 526             itoa(*ptr->val, buff, 10);
 527             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 528             CLR_PRINTVALUE(*flags);
 529         }
 530         break;
 531
 532     case OME_INT8:
 533         if (IS_PRINTVALUE(*flags) && p->data) {
 534             OSD_INT8_t *ptr = p->data;
 535             itoa(*ptr->val, buff, 10);
 536             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 537             CLR_PRINTVALUE(*flags);
 538         }
 539         break;
 540
 541     case OME_UINT16:
 542         if (IS_PRINTVALUE(*flags) && p->data) {
 543             OSD_UINT16_t *ptr = p->data;
 544             itoa(*ptr->val, buff, 10);
 545             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 546             CLR_PRINTVALUE(*flags);
 547         }
 548         break;
 549
 550     case OME_INT16:
 551         if (IS_PRINTVALUE(*flags) && p->data) {
 552             OSD_INT16_t *ptr = p->data;
 553             itoa(*ptr->val, buff, 10);
 554             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 555             CLR_PRINTVALUE(*flags);
 556         }
 557         break;
 558
 559     case OME_UINT32:
 560         if (IS_PRINTVALUE(*flags) && p->data) {
 561             OSD_UINT32_t *ptr = p->data;
 562             itoa(*ptr->val, buff, 10);
 563             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 564             CLR_PRINTVALUE(*flags);
 565         }
 566         break;
 567
 568     case OME_INT32:
 569         if (IS_PRINTVALUE(*flags) && p->data) {
 570             OSD_INT32_t *ptr = p->data;
 571             itoa(*ptr->val, buff, 10);
 572             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 573             CLR_PRINTVALUE(*flags);
 574         }
 575         break;
 576
 577     case OME_FLOAT:
 578         if (IS_PRINTVALUE(*flags) && p->data) {
 579             OSD_FLOAT_t *ptr = p->data;
 580             cmsFormatFloat(*ptr->val * ptr->multipler, buff);
 581             cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN);
 582             CLR_PRINTVALUE(*flags);
 583         }
 584         break;
 585
 586     case OME_Label:
 587         if (IS_PRINTVALUE(*flags) && p->data) {
 588             // A label with optional string, immediately following text
 589             cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_ATTR_NONE, p->data);
 590             CLR_PRINTVALUE(*flags);
 591         }
 592         break;
 593
 594     case OME_OSD_Exit:
 595     case OME_END:
 596     case OME_Back:
 597         break;
 598
 599     case OME_MENU:
 600         // Fall through
 601     default:
 602 #ifdef CMS_MENU_DEBUG
 603         // Shouldn't happen. Notify creator of this menu content
 604 #ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
 605         cnt = cmsDisplayWrite(pDisplay, rightMenuColumn - 6, row, DISPLAYPORT_ATTR_NONE, "BADENT");
 606 #else
 607         cnt = cmsDisplayWrite(pDisplay, rightMenuColumn, row, DISPLAYPORT_ATTR_NONE, "BADENT");
 608 #endif
 609 #endif
 610         break;
 611     }
 612
 613     return cnt;
 614 }
 615
 616 static void cmsMenuCountPage(displayPort_t *pDisplay)
 617 {
 618     UNUSED(pDisplay);
 619     const OSD_Entry *p;
 620     for (p = currentCtx.menu->entries; p->type != OME_END; p++);
 621     pageCount = (p - currentCtx.menu->entries - 1) / maxMenuItems + 1;
 622 }
 623
 624 STATIC_UNIT_TESTED const void *cmsMenuBack(displayPort_t *pDisplay)
 625 {
 626     // Let onExit function decide whether to allow exit or not.
 627     if (currentCtx.menu->onExit) {
 628         const void *result = currentCtx.menu->onExit(pDisplay, pageTop + currentCtx.cursorRow);
 629         if (result == MENU_CHAIN_BACK) {
 630             return result;
 631         }
 632     }
 633
 634     saveMenuInhibited = false;
 635
 636     if (!menuStackIdx) {
 637         return NULL;
 638     }
 639
 640     currentCtx = menuStack[--menuStackIdx];
 641
 642     cmsMenuCountPage(pDisplay);
 643     cmsPageSelect(pDisplay, currentCtx.page);
 644
 645 #if defined(CMS_PAGE_DEBUG)
 646     cmsPageDebug();
 647 #endif
 648
 649     return NULL;
 650 }
 651
 652 // Skip read-only entries
 653 static bool rowIsSkippable(const OSD_Entry *row)
 654 {
 655     if (row->type == OME_Label) {
 656         return true;
 657     }
 658
 659     if (row->type == OME_String) {
 660         return true;
 661     }
 662
 663     if ((row->type == OME_UINT16 || row->type == OME_INT16) && row->flags == DYNAMIC) {
 664         return true;
 665     }
 666     return false;
 667 }
 668
 669 static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 670 {
 671     if (!pageTop || !cmsInMenu) {
 672         return;
 673     }
 674
 675     const bool displayWasCleared = pDisplay->cleared;
 676     uint8_t i;
 677     const OSD_Entry *p;
 678     uint8_t top = smallScreen ? 1 : (pDisplay->rows - pageMaxRow)/2;
 679
 680     pDisplay->cleared = false;
 681
 682     // Polled (dynamic) value display denominator.
 683
 684     bool drawPolled = false;
 685     static uint32_t lastPolledUs = 0;
 686
 687     if (currentTimeUs > lastPolledUs + CMS_POLL_INTERVAL_US) {
 688         drawPolled = true;
 689         lastPolledUs = currentTimeUs;
 690     }
 691
 692     uint32_t room = displayTxBytesFree(pDisplay);
 693
 694     if (displayWasCleared) {
 695         for (p = pageTop, i= 0; (p <= pageTop + pageMaxRow); p++, i++) {
 696             SET_PRINTLABEL(runtimeEntryFlags[i]);
 697             SET_PRINTVALUE(runtimeEntryFlags[i]);
 698         }
 699     } else if (drawPolled) {
 700         for (p = pageTop, i = 0; (p <= pageTop + pageMaxRow); p++, i++) {
 701             if (IS_DYNAMIC(p))
 702                 SET_PRINTVALUE(runtimeEntryFlags[i]);
 703         }
 704     }
 705
 706     // Cursor manipulation
 707
 708     while (rowIsSkippable(pageTop + currentCtx.cursorRow)) { // skip labels, strings and dynamic read-only entries
 709         currentCtx.cursorRow++;
 710     }
 711
 712 #if defined(CMS_PAGE_DEBUG)
 713     cmsPageDebug();
 714 #endif
 715
 716     if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
 717         room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, " ");
 718     }
 719
 720     if (room < 30) {
 721         return;
 722     }
 723
 724     if (pDisplay->cursorRow != currentCtx.cursorRow) {
 725         room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, ">");
 726         pDisplay->cursorRow = currentCtx.cursorRow;
 727     }
 728
 729     if (room < 30) {
 730         return;
 731     }
 732
 733     if (currentCtx.menu->onDisplayUpdate) {
 734         const void *result = currentCtx.menu->onDisplayUpdate(pDisplay, pageTop + currentCtx.cursorRow);
 735         if (result == MENU_CHAIN_BACK) {
 736             cmsMenuBack(pDisplay);
 737
 738             return;
 739         }
 740     }
 741
 742     // Print text labels
 743     for (i = 0, p = pageTop; (p <= pageTop + pageMaxRow); i++, p++) {
 744         if (IS_PRINTLABEL(runtimeEntryFlags[i])) {
 745             uint8_t coloff = leftMenuColumn;
 746             coloff += (p->type == OME_Label) ? 0 : 1;
 747             room -= cmsDisplayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, p->text);
 748             CLR_PRINTLABEL(runtimeEntryFlags[i]);
 749             if (room < 30) {
 750                 return;
 751             }
 752         }
 753
 754     // Print values
 755
 756     // XXX Polled values at latter positions in the list may not be
 757     // XXX printed if not enough room in the middle of the list.
 758
 759         if (IS_PRINTVALUE(runtimeEntryFlags[i]) || IS_SCROLLINGTICKER(runtimeEntryFlags[i])) {
 760             bool selectedRow = i == currentCtx.cursorRow;
 761             room -= cmsDrawMenuEntry(pDisplay, p, top + i * linesPerMenuItem, selectedRow, &runtimeEntryFlags[i], &runtimeTableTicker[i]);
 762             if (room < 30) {
 763                 return;
 764             }
 765         }
 766     }
 767
 768     // Draw the up/down page indicators if the display has space.
 769     // Only draw the symbols when necessary after the screen has been cleared. Otherwise they're static.
 770     // If the device supports OSD symbols then use the up/down arrows. Otherwise assume it's a
 771     // simple text device and use the '^' (carat) and 'V' for arrow approximations.
 772     if (displayWasCleared && leftMenuColumn > 0) {      // make sure there's room to draw the symbol
 773         if (currentCtx.page > 0) {
 774             const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_NORTH : '^';
 775             displayWriteChar(pDisplay, leftMenuColumn - 1, top, DISPLAYPORT_ATTR_NONE, symbol);
 776         }
 777          if (currentCtx.page < pageCount - 1) {
 778             const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SOUTH : 'V';
 779             displayWriteChar(pDisplay, leftMenuColumn - 1, top + pageMaxRow, DISPLAYPORT_ATTR_NONE, symbol);
 780         }
 781     }
 782
 783 }
 784
 785 const void *cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
 786 {
 787     const CMS_Menu *pMenu = (const CMS_Menu *)ptr;
 788
 789     if (!pMenu) {
 790         return NULL;
 791     }
 792
 793 #ifdef CMS_MENU_DEBUG
 794     if (pMenu->GUARD_type != OME_MENU) {
 795         // ptr isn't pointing to a CMS_Menu.
 796         if (pMenu->GUARD_type <= OME_MAX) {
 797             strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1);
 798         } else {
 799             strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1);
 800         }
 801         pMenu = &menuErr;
 802     }
 803 #endif
 804
 805     if (pMenu != currentCtx.menu) {
 806         saveMenuInhibited = false;
 807
 808         if (currentCtx.menu) {
 809             // If we are opening the initial top-level menu, then currentCtx.menu will be NULL and nothing to do.
 810             // Otherwise stack the current menu before moving to the selected menu.
 811             if (menuStackIdx >= CMS_MENU_STACK_LIMIT - 1) {
 812                 // menu stack limit reached - prevent array overflow
 813                 return NULL;
 814             }
 815             menuStack[menuStackIdx++] = currentCtx;
 816         }
 817
 818         currentCtx.menu = pMenu;
 819         currentCtx.cursorRow = 0;
 820
 821         if (pMenu->onEnter) {
 822             const void *result = pMenu->onEnter(pDisplay);
 823             if (result == MENU_CHAIN_BACK) {
 824                 return cmsMenuBack(pDisplay);
 825             }
 826         }
 827
 828         cmsMenuCountPage(pDisplay);
 829         cmsPageSelect(pDisplay, 0);
 830     } else {
 831         // The (pMenu == curretMenu) case occurs when reopening for display cycling
 832         // currentCtx.cursorRow has been saved as absolute; convert it back to page + relative
 833
 834         int8_t cursorAbs = currentCtx.cursorRow;
 835         currentCtx.cursorRow = cursorAbs % maxMenuItems;
 836         cmsMenuCountPage(pDisplay);
 837         cmsPageSelect(pDisplay, cursorAbs / maxMenuItems);
 838     }
 839
 840 #if defined(CMS_PAGE_DEBUG)
 841     cmsPageDebug();
 842 #endif
 843
 844     return NULL;
 845 }
 846
 847 void cmsMenuOpen(void)
 848 {
 849     const CMS_Menu *startMenu;
 850     if (!cmsInMenu) {
 851         // New open
 852         pCurrentDisplay = cmsDisplayPortSelectCurrent();
 853         if (!pCurrentDisplay) {
 854             return;
 855         }
 856         cmsInMenu = true;
 857         currentCtx = (cmsCtx_t){ NULL, 0, 0 };
 858         startMenu = &cmsx_menuMain;
 859         menuStackIdx = 0;
 860         setArmingDisabled(ARMING_DISABLED_CMS_MENU);
 861         displayLayerSelect(pCurrentDisplay, DISPLAYPORT_LAYER_FOREGROUND); // make sure the foreground layer is active
 862         if (osdConfig()->cms_background_type != DISPLAY_BACKGROUND_TRANSPARENT) {
 863             displaySetBackgroundType(pCurrentDisplay, (displayPortBackground_e)osdConfig()->cms_background_type); // set the background type if not transparent
 864         }
 865     } else {
 866         // Switch display
 867         displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
 868         startMenu = currentCtx.menu;
 869         if (pNextDisplay != pCurrentDisplay) {
 870             // DisplayPort has been changed.
 871             // Convert cursorRow to absolute value
 872             currentCtx.cursorRow = cmsCursorAbsolute(pCurrentDisplay);
 873             displaySetBackgroundType(pCurrentDisplay, DISPLAY_BACKGROUND_TRANSPARENT); // reset previous displayPort to transparent
 874             displayRelease(pCurrentDisplay);
 875             pCurrentDisplay = pNextDisplay;
 876             displaySetBackgroundType(pCurrentDisplay, (displayPortBackground_e)osdConfig()->cms_background_type); // set the background type if not transparent
 877         } else {
 878             return;
 879         }
 880     }
 881     displayGrab(pCurrentDisplay); // grab the display for use by the CMS
 882     // FIXME this should probably not have a dependency on the OSD or OSD slave code
 883 #ifdef USE_OSD
 884     resumeRefreshAt = 0;
 885 #endif
 886
 887     if ( pCurrentDisplay->cols < NORMAL_SCREEN_MIN_COLS) {
 888       smallScreen       = true;
 889       linesPerMenuItem  = 2;
 890       leftMenuColumn    = 0;
 891       rightMenuColumn   = pCurrentDisplay->cols;
 892       maxMenuItems      = (pCurrentDisplay->rows) / linesPerMenuItem;
 893     } else {
 894       smallScreen       = false;
 895       linesPerMenuItem  = 1;
 896       leftMenuColumn    = 2;
 897 #ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
 898       rightMenuColumn   = pCurrentDisplay->cols - 2;
 899 #else
 900       rightMenuColumn   = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
 901 #endif
 902       maxMenuItems      = pCurrentDisplay->rows - 2;
 903     }
 904
 905     if (pCurrentDisplay->useFullscreen) {
 906         leftMenuColumn = 0;
 907         rightMenuColumn   = pCurrentDisplay->cols;
 908         maxMenuItems      = pCurrentDisplay->rows;
 909     }
 910
 911     cmsMenuChange(pCurrentDisplay, startMenu);
 912 }
 913
 914 static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
 915 {
 916     for (const OSD_Entry *p = pMenu->entries; p->type != OME_END ; p++) {
 917         if (p->type == OME_Submenu) {
 918             cmsTraverseGlobalExit(p->data);
 919         }
 920     }
 921
 922 }
 923
 924 const void *cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
 925 {
 926     int exitType = (int)ptr;
 927     switch (exitType) {
 928     case CMS_EXIT_SAVE:
 929     case CMS_EXIT_SAVEREBOOT:
 930     case CMS_POPUP_SAVE:
 931     case CMS_POPUP_SAVEREBOOT:
 932
 933         cmsTraverseGlobalExit(&cmsx_menuMain);
 934
 935         if (currentCtx.menu->onExit) {
 936             currentCtx.menu->onExit(pDisplay, (OSD_Entry *)NULL); // Forced exit
 937         }
 938
 939         if ((exitType == CMS_POPUP_SAVE) || (exitType == CMS_POPUP_SAVEREBOOT)) {
 940             // traverse through the menu stack and call their onExit functions
 941             for (int i = menuStackIdx - 1; i >= 0; i--) {
 942                 if (menuStack[i].menu->onExit) {
 943                     menuStack[i].menu->onExit(pDisplay, (OSD_Entry *)NULL);
 944                 }
 945             }
 946         }
 947
 948         saveConfigAndNotify();
 949         break;
 950
 951     case CMS_EXIT:
 952         break;
 953     }
 954
 955     cmsInMenu = false;
 956
 957     displaySetBackgroundType(pCurrentDisplay, DISPLAY_BACKGROUND_TRANSPARENT); // reset the background to transparent
 958
 959     displayRelease(pDisplay);
 960     currentCtx.menu = NULL;
 961
 962     if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT) || (exitType == CMS_POPUP_EXITREBOOT)) {
 963         displayClearScreen(pDisplay);
 964         cmsDisplayWrite(pDisplay, 5, 3, DISPLAYPORT_ATTR_NONE, "REBOOTING...");
 965
 966         // Flush display
 967         displayRedraw(pDisplay);
 968
 969         stopMotors();
 970         motorShutdown();
 971         delay(200);
 972
 973         systemReset();
 974     }
 975
 976     unsetArmingDisabled(ARMING_DISABLED_CMS_MENU);
 977
 978     return NULL;
 979 }
 980
 981 // Stick/key detection and key codes
 982
 983 #define IS_HI(X)  (rcData[X] > 1750)
 984 #define IS_LO(X)  (rcData[X] < 1250)
 985 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
 986
 987 #define BUTTON_TIME   250 // msec
 988 #define BUTTON_PAUSE  500 // msec
 989
 990 STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, cms_key_e key)
 991 {
 992     uint16_t res = BUTTON_TIME;
 993     const OSD_Entry *p;
 994
 995     if (!currentCtx.menu) {
 996         return res;
 997     }
 998
 999     if (key == CMS_KEY_MENU) {
1000         cmsMenuOpen();
1001         return BUTTON_PAUSE;
1002     }
1003
1004     if (key == CMS_KEY_ESC) {
1005         if (osdElementEditing) {
1006             osdElementEditing = false;
1007         } else {
1008             cmsMenuBack(pDisplay);
1009         }
1010         return BUTTON_PAUSE;
1011     }
1012
1013     if (key == CMS_KEY_SAVEMENU && !saveMenuInhibited) {
1014         osdElementEditing = false;
1015         cmsMenuChange(pDisplay, getSaveExitMenu());
1016
1017         return BUTTON_PAUSE;
1018     }
1019
1020     if ((key == CMS_KEY_DOWN) && (!osdElementEditing)) {
1021         if (currentCtx.cursorRow < pageMaxRow) {
1022             currentCtx.cursorRow++;
1023         } else {
1024             cmsPageNext(pDisplay);
1025             currentCtx.cursorRow = 0;    // Goto top in any case
1026         }
1027     }
1028
1029     if ((key == CMS_KEY_UP) && (!osdElementEditing)) {
1030         currentCtx.cursorRow--;
1031
1032         // Skip non-title labels, strings and dynamic read-only entries
1033         while ((rowIsSkippable(pageTop + currentCtx.cursorRow)) && currentCtx.cursorRow > 0) {
1034             currentCtx.cursorRow--;
1035         }
1036         if (currentCtx.cursorRow == -1 || (pageTop + currentCtx.cursorRow)->type == OME_Label) {
1037             // Goto previous page
1038             cmsPagePrev(pDisplay);
1039             currentCtx.cursorRow = pageMaxRow;
1040         }
1041     }
1042
1043     if ((key == CMS_KEY_DOWN || key == CMS_KEY_UP) && (!osdElementEditing)) {
1044         return res;
1045     }
1046
1047     p = pageTop + currentCtx.cursorRow;
1048
1049     switch (p->type) {
1050         case OME_Submenu:
1051             if (key == CMS_KEY_RIGHT) {
1052                 cmsMenuChange(pDisplay, p->data);
1053                 res = BUTTON_PAUSE;
1054             }
1055             break;
1056
1057         case OME_Funcall:;
1058             const void *retval;
1059             if (p->func && key == CMS_KEY_RIGHT) {
1060                 retval = p->func(pDisplay, p->data);
1061                 if (retval == MENU_CHAIN_BACK) {
1062                     cmsMenuBack(pDisplay);
1063                 }
1064                 if ((p->flags & REBOOT_REQUIRED)) {
1065                     setRebootRequired();
1066                 }
1067                 res = BUTTON_PAUSE;
1068             }
1069             break;
1070
1071         case OME_OSD_Exit:
1072             if (p->func && key == CMS_KEY_RIGHT) {
1073                 p->func(pDisplay, p->data);
1074                 res = BUTTON_PAUSE;
1075             }
1076             break;
1077
1078         case OME_Back:
1079             cmsMenuBack(pDisplay);
1080             res = BUTTON_PAUSE;
1081             osdElementEditing = false;
1082             break;
1083
1084         case OME_Bool:
1085             if (p->data) {
1086                 uint8_t *val = p->data;
1087                 const uint8_t previousValue = *val;
1088                 *val = (key == CMS_KEY_RIGHT) ? 1 : 0;
1089                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1090                 if ((p->flags & REBOOT_REQUIRED) && (*val != previousValue)) {
1091                     setRebootRequired();
1092                 }
1093                 if (p->func) {
1094                     p->func(pDisplay, p->data);
1095                 }
1096             }
1097             break;
1098
1099 #ifdef USE_OSD
1100         case OME_VISIBLE:
1101             if (p->data) {
1102                 uint16_t *val = (uint16_t *)p->data;
1103                 const uint16_t previousValue = *val;
1104                 if ((key == CMS_KEY_RIGHT) && (!osdElementEditing)) {
1105                     osdElementEditing = true;
1106                     osdProfileCursor = 1;
1107                 } else if (osdElementEditing) {
1108 #ifdef USE_OSD_PROFILES
1109                     if (key == CMS_KEY_RIGHT) {
1110                         if (osdProfileCursor < OSD_PROFILE_COUNT) {
1111                             osdProfileCursor++;
1112                         }
1113                     }
1114                     if (key == CMS_KEY_LEFT) {
1115                         if (osdProfileCursor > 1) {
1116                             osdProfileCursor--;
1117                         }
1118                     }
1119 #endif
1120                     if (key == CMS_KEY_UP) {
1121                         *val |= OSD_PROFILE_FLAG(osdProfileCursor);
1122                     }
1123                     if (key == CMS_KEY_DOWN) {
1124                         *val &= ~OSD_PROFILE_FLAG(osdProfileCursor);
1125                     }
1126                 }
1127                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1128                 if ((p->flags & REBOOT_REQUIRED) && (*val != previousValue)) {
1129                     setRebootRequired();
1130                 }
1131             }
1132             break;
1133 #endif
1134
1135         case OME_UINT8:
1136         case OME_FLOAT:
1137             if (p->data) {
1138                 OSD_UINT8_t *ptr = p->data;
1139                 const uint16_t previousValue = *ptr->val;
1140                 if (key == CMS_KEY_RIGHT) {
1141                     if (*ptr->val < ptr->max) {
1142                         *ptr->val += ptr->step;
1143                     }
1144                 } else {
1145                     if (*ptr->val > ptr->min) {
1146                         *ptr->val -= ptr->step;
1147                     }
1148                 }
1149                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1150                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1151                     setRebootRequired();
1152                 }
1153                 if (p->func) {
1154                     p->func(pDisplay, p);
1155                 }
1156             }
1157             break;
1158
1159         case OME_TAB:
1160             if (p->type == OME_TAB) {
1161                 OSD_TAB_t *ptr = p->data;
1162                 const uint8_t previousValue = *ptr->val;
1163
1164                 if (key == CMS_KEY_RIGHT) {
1165                     if (*ptr->val < ptr->max) {
1166                         *ptr->val += 1;
1167                     }
1168                 } else {
1169                     if (*ptr->val > 0) {
1170                         *ptr->val -= 1;
1171                     }
1172                 }
1173                 if (p->func) {
1174                     p->func(pDisplay, p->data);
1175                 }
1176                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1177                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1178                     setRebootRequired();
1179                 }
1180             }
1181             break;
1182
1183         case OME_INT8:
1184             if (p->data) {
1185                 OSD_INT8_t *ptr = p->data;
1186                 const int8_t previousValue = *ptr->val;
1187                 if (key == CMS_KEY_RIGHT) {
1188                     if (*ptr->val < ptr->max) {
1189                         *ptr->val += ptr->step;
1190                     }
1191                 } else {
1192                     if (*ptr->val > ptr->min) {
1193                         *ptr->val -= ptr->step;
1194                     }
1195                 }
1196                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1197                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1198                     setRebootRequired();
1199                 }
1200                 if (p->func) {
1201                     p->func(pDisplay, p);
1202                 }
1203             }
1204             break;
1205
1206         case OME_UINT16:
1207             if (p->data) {
1208                 OSD_UINT16_t *ptr = p->data;
1209                 const uint16_t previousValue = *ptr->val;
1210                 if (key == CMS_KEY_RIGHT) {
1211                     if (*ptr->val < ptr->max) {
1212                         *ptr->val += ptr->step;
1213                     }
1214                 } else {
1215                     if (*ptr->val > ptr->min) {
1216                         *ptr->val -= ptr->step;
1217                     }
1218                 }
1219                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1220                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1221                     setRebootRequired();
1222                 }
1223                 if (p->func) {
1224                     p->func(pDisplay, p);
1225                 }
1226             }
1227             break;
1228
1229         case OME_INT16:
1230             if (p->data) {
1231                 OSD_INT16_t *ptr = p->data;
1232                 const int16_t previousValue = *ptr->val;
1233                 if (key == CMS_KEY_RIGHT) {
1234                     if (*ptr->val < ptr->max) {
1235                         *ptr->val += ptr->step;
1236                     }
1237                 } else {
1238                     if (*ptr->val > ptr->min) {
1239                         *ptr->val -= ptr->step;
1240                     }
1241                 }
1242                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1243                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1244                     setRebootRequired();
1245                 }
1246                 if (p->func) {
1247                     p->func(pDisplay, p);
1248                 }
1249             }
1250             break;
1251
1252         case OME_UINT32:
1253             if (p->data) {
1254                 OSD_UINT32_t *ptr = p->data;
1255                 const uint32_t previousValue = *ptr->val;
1256                 if (key == CMS_KEY_RIGHT) {
1257                     if (*ptr->val < ptr->max) {
1258                         *ptr->val += ptr->step;
1259                     }
1260                 } else {
1261                     if (*ptr->val > ptr->min) {
1262                         *ptr->val -= ptr->step;
1263                     }
1264                 }
1265                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1266                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1267                     setRebootRequired();
1268                 }
1269                 if (p->func) {
1270                     p->func(pDisplay, p);
1271                 }
1272             }
1273             break;
1274
1275         case OME_INT32:
1276             if (p->data) {
1277                 OSD_INT32_t *ptr = p->data;
1278                 const int32_t previousValue = *ptr->val;
1279                 if (key == CMS_KEY_RIGHT) {
1280                     if (*ptr->val < ptr->max) {
1281                         *ptr->val += ptr->step;
1282                     }
1283                 } else {
1284                     if (*ptr->val > ptr->min) {
1285                         *ptr->val -= ptr->step;
1286                     }
1287                 }
1288                 SET_PRINTVALUE(runtimeEntryFlags[currentCtx.cursorRow]);
1289                 if ((p->flags & REBOOT_REQUIRED) && (*ptr->val != previousValue)) {
1290                     setRebootRequired();
1291                 }
1292                 if (p->func) {
1293                     p->func(pDisplay, p);
1294                 }
1295             }
1296             break;
1297
1298         case OME_String:
1299             break;
1300
1301         case OME_Label:
1302         case OME_END:
1303             break;
1304
1305         case OME_MENU:
1306             // Shouldn't happen
1307             break;
1308     }
1309     return res;
1310 }
1311
1312 void cmsSetExternKey(cms_key_e extKey)
1313 {
1314     if (externKey == CMS_KEY_NONE)
1315         externKey = extKey;
1316 }
1317
1318 uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, cms_key_e key, int repeatCount)
1319 {
1320     uint16_t ret = 0;
1321
1322     for (int i = 0 ; i < repeatCount ; i++) {
1323         ret = cmsHandleKey(pDisplay, key);
1324     }
1325
1326     return ret;
1327 }
1328
1329 static void cmsUpdate(uint32_t currentTimeUs)
1330 {
1331     if (IS_RC_MODE_ACTIVE(BOXPARALYZE)
1332 #ifdef USE_RCDEVICE
1333         || rcdeviceInMenu
1334 #endif
1335 #ifdef USE_USB_CDC_HID
1336         || cdcDeviceIsMayBeActive() // If this target is used as a joystick, we should leave here.
1337 #endif
1338        ) {
1339         return;
1340     }
1341
1342     static int16_t rcDelayMs = BUTTON_TIME;
1343     static int holdCount = 1;
1344     static int repeatCount = 1;
1345     static int repeatBase = 0;
1346
1347     static uint32_t lastCalledMs = 0;
1348     static uint32_t lastCmsHeartBeatMs = 0;
1349
1350     const uint32_t currentTimeMs = currentTimeUs / 1000;
1351
1352     if (!cmsInMenu) {
1353         // Detect menu invocation
1354         if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXSTICKCOMMANDDISABLE)) {
1355             cmsMenuOpen();
1356             rcDelayMs = BUTTON_PAUSE;    // Tends to overshoot if BUTTON_TIME
1357         }
1358     } else {
1359         //
1360         // Scan 'key' first
1361         //
1362
1363         cms_key_e key = CMS_KEY_NONE;
1364
1365         if (externKey != CMS_KEY_NONE) {
1366             rcDelayMs = cmsHandleKey(pCurrentDisplay, externKey);
1367             externKey = CMS_KEY_NONE;
1368         } else {
1369             if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
1370                 key = CMS_KEY_MENU;
1371             } else if (IS_HI(PITCH)) {
1372                 key = CMS_KEY_UP;
1373             } else if (IS_LO(PITCH)) {
1374                 key = CMS_KEY_DOWN;
1375             } else if (IS_LO(ROLL)) {
1376                 key = CMS_KEY_LEFT;
1377             } else if (IS_HI(ROLL)) {
1378                 key = CMS_KEY_RIGHT;
1379             } else if (IS_LO(YAW)) {
1380                 key = CMS_KEY_ESC;
1381             } else if (IS_HI(YAW)) {
1382                 key = CMS_KEY_SAVEMENU;
1383             }
1384
1385             if (key == CMS_KEY_NONE) {
1386                 // No 'key' pressed, reset repeat control
1387                 holdCount = 1;
1388                 repeatCount = 1;
1389                 repeatBase = 0;
1390             } else {
1391                 // The 'key' is being pressed; keep counting
1392                 ++holdCount;
1393             }
1394
1395             if (rcDelayMs > 0) {
1396                 rcDelayMs -= (currentTimeMs - lastCalledMs);
1397             } else if (key) {
1398                 rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount);
1399
1400                 // Key repeat effect is implemented in two phases.
1401                 // First phldase is to decrease rcDelayMs reciprocal to hold time.
1402                 // When rcDelayMs reached a certain limit (scheduling interval),
1403                 // repeat rate will not raise anymore, so we call key handler
1404                 // multiple times (repeatCount).
1405                 //
1406                 // XXX Caveat: Most constants are adjusted pragmatically.
1407                 // XXX Rewrite this someday, so it uses actual hold time instead
1408                 // of holdCount, which depends on the scheduling interval.
1409
1410                 if (((key == CMS_KEY_LEFT) || (key == CMS_KEY_RIGHT)) && (holdCount > 20)) {
1411
1412                     // Decrease rcDelayMs reciprocally
1413
1414                     rcDelayMs /= (holdCount - 20);
1415
1416                     // When we reach the scheduling limit,
1417
1418                     if (rcDelayMs <= 50) {
1419
1420                         // start calling handler multiple times.
1421
1422                         if (repeatBase == 0) {
1423                             repeatBase = holdCount;
1424                         }
1425
1426                         repeatCount = repeatCount + (holdCount - repeatBase) / 5;
1427
1428                         if (repeatCount > 5) {
1429                             repeatCount= 5;
1430                         }
1431                     }
1432                 }
1433             }
1434         }
1435
1436         cmsDrawMenu(pCurrentDisplay, currentTimeUs);
1437
1438         if (currentTimeMs > lastCmsHeartBeatMs + 500) {
1439             // Heart beat for external CMS display device @ 500msec
1440             // (Timeout @ 1000msec)
1441             displayHeartbeat(pCurrentDisplay);
1442             lastCmsHeartBeatMs = currentTimeMs;
1443         }
1444     }
1445
1446     // Some key (command), notably flash erase, takes too long to use the
1447     // currentTimeMs to be used as lastCalledMs (freezes CMS for a minute or so
1448     // if used).
1449     lastCalledMs = millis();
1450 }
1451
1452 void cmsHandler(timeUs_t currentTimeUs)
1453 {
1454     if (cmsDeviceCount > 0) {
1455         cmsUpdate(currentTimeUs);
1456     }
1457 }
1458
1459 void cmsInit(void)
1460 {
1461     cmsDeviceCount = 0;
1462     cmsCurrentDevice = -1;
1463 }
1464
1465 void inhibitSaveMenu(void)
1466 {
1467     saveMenuInhibited = true;
1468 }
1469
1470 void cmsAddMenuEntry(OSD_Entry *menuEntry, char *text, OSD_MenuElement type, CMSEntryFuncPtr func, void *data, uint8_t flags)
1471 {
1472         menuEntry->text = text;
1473         menuEntry->type = type;
1474         menuEntry->func = func;
1475         menuEntry->data = data;
1476         menuEntry->flags = flags;
1477 }
1478
1479 #endif // CMS