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