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