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