src/main/cms/cms.c

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