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