src/main/osd/osd.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  Created by Marcin Baliniak
  23  some functions based on MinimOSD
  24
  25  OSD-CMS separation by jflyper
  26  */
  27
  28 #include <stdbool.h>
  29 #include <stdint.h>
  30 #include <stdlib.h>
  31 #include <string.h>
  32 #include <ctype.h>
  33 #include <math.h>
  34
  35 #include "platform.h"
  36
  37 #ifdef USE_OSD
  38
  39 #include "blackbox/blackbox.h"
  40 #include "blackbox/blackbox_io.h"
  41
  42 #include "build/build_config.h"
  43 #include "build/version.h"
  44
  45 #include "cms/cms.h"
  46
  47 #include "common/axis.h"
  48 #include "common/maths.h"
  49 #include "common/printf.h"
  50 #include "common/typeconversion.h"
  51 #include "common/utils.h"
  52 #include "common/unit.h"
  53
  54 #include "config/feature.h"
  55
  56 #include "drivers/display.h"
  57 #include "drivers/dshot.h"
  58 #include "drivers/flash/flash.h"
  59 #include "drivers/osd_symbols.h"
  60 #include "drivers/sdcard.h"
  61 #include "drivers/time.h"
  62
  63 #include "drivers/pinio.h"
  64
  65 #include "fc/core.h"
  66 #include "fc/gps_lap_timer.h"
  67 #include "fc/rc_controls.h"
  68 #include "fc/rc_modes.h"
  69 #include "fc/runtime_config.h"
  70
  71 #if defined(USE_DYN_NOTCH_FILTER)
  72 #include "flight/dyn_notch_filter.h"
  73 #endif
  74 #include "flight/failsafe.h"
  75 #include "flight/imu.h"
  76 #include "flight/mixer.h"
  77 #include "flight/position.h"
  78
  79 #include "io/asyncfatfs/asyncfatfs.h"
  80 #include "io/beeper.h"
  81 #include "io/flashfs.h"
  82 #include "io/gps.h"
  83
  84 #include "osd/osd.h"
  85 #include "osd/osd_elements.h"
  86 #include "osd/osd_warnings.h"
  87
  88 #include "pg/motor.h"
  89 #include "pg/pg.h"
  90 #include "pg/pg_ids.h"
  91 #include "pg/stats.h"
  92
  93 #include "rx/crsf.h"
  94 #include "rx/rc_stats.h"
  95 #include "rx/rx.h"
  96
  97 #include "scheduler/scheduler.h"
  98
  99 #include "sensors/acceleration.h"
 100 #include "sensors/battery.h"
 101 #include "sensors/sensors.h"
 102
 103 #ifdef USE_HARDWARE_REVISION_DETECTION
 104 #include "hardware_revision.h"
 105 #endif
 106
 107 typedef enum {
 108     OSD_LOGO_ARMING_OFF,
 109     OSD_LOGO_ARMING_ON,
 110     OSD_LOGO_ARMING_FIRST
 111 } osd_logo_on_arming_e;
 112
 113 const char * const osdTimerSourceNames[] = {
 114     "ON TIME  ",
 115     "TOTAL ARM",
 116     "LAST ARM ",
 117     "ON/ARM   "
 118 };
 119
 120 #define OSD_LOGO_ROWS 4
 121 #define OSD_LOGO_COLS 24
 122
 123 // Things in both OSD and CMS
 124
 125 #define IS_HI(X)  (rcData[X] > 1750)
 126 #define IS_LO(X)  (rcData[X] < 1250)
 127 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
 128
 129 timeUs_t osdFlyTime = 0;
 130 #if defined(USE_ACC)
 131 float osdGForce = 0;
 132 #endif
 133 uint16_t osdAuxValue = 0;
 134
 135 static bool showVisualBeeper = false;
 136
 137 static statistic_t stats;
 138 timeUs_t resumeRefreshAt = 0;
 139 #define REFRESH_1S    1000 * 1000
 140
 141 static uint8_t armState;
 142 #ifdef USE_OSD_PROFILES
 143 static uint8_t osdProfile = 1;
 144 #endif
 145 static displayPort_t *osdDisplayPort;
 146 static osdDisplayPortDevice_e osdDisplayPortDeviceType;
 147 static bool osdIsReady;
 148
 149 static bool suppressStatsDisplay = false;
 150
 151 static bool backgroundLayerSupported = false;
 152
 153 #ifdef USE_ESC_SENSOR
 154 escSensorData_t *osdEscDataCombined;
 155 #endif
 156
 157 STATIC_ASSERT(OSD_POS_MAX == OSD_POS(63,31), OSD_POS_MAX_incorrect);
 158
 159 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
 160
 161 PG_REGISTER_WITH_RESET_FN(osdElementConfig_t, osdElementConfig, PG_OSD_ELEMENT_CONFIG, 1);
 162
 163 // Controls the display order of the OSD post-flight statistics.
 164 // Adjust the ordering here to control how the post-flight stats are presented.
 165 // Every entry in osd_stats_e should be represented. Any that are missing will not
 166 // be shown on the the post-flight statistics page.
 167 // If you reorder the stats it's likely that you'll need to make likewise updates
 168 // to the unit tests.
 169
 170 // If adding new stats, please add to the osdStatsNeedAccelerometer() function
 171 // if the statistic utilizes the accelerometer.
 172 //
 173 const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
 174     OSD_STAT_RTC_DATE_TIME,
 175     OSD_STAT_TIMER_1,
 176     OSD_STAT_TIMER_2,
 177     OSD_STAT_MAX_ALTITUDE,
 178     OSD_STAT_MAX_SPEED,
 179     OSD_STAT_MAX_DISTANCE,
 180     OSD_STAT_FLIGHT_DISTANCE,
 181     OSD_STAT_MIN_BATTERY,
 182     OSD_STAT_END_BATTERY,
 183     OSD_STAT_BATTERY,
 184     OSD_STAT_MIN_RSSI,
 185     OSD_STAT_MAX_CURRENT,
 186     OSD_STAT_USED_MAH,
 187     OSD_STAT_BLACKBOX,
 188     OSD_STAT_BLACKBOX_NUMBER,
 189     OSD_STAT_MAX_G_FORCE,
 190     OSD_STAT_MAX_ESC_TEMP,
 191     OSD_STAT_MAX_ESC_RPM,
 192     OSD_STAT_MIN_LINK_QUALITY,
 193     OSD_STAT_MAX_FFT,
 194     OSD_STAT_MIN_RSSI_DBM,
 195     OSD_STAT_MIN_RSNR,
 196     OSD_STAT_TOTAL_FLIGHTS,
 197     OSD_STAT_TOTAL_TIME,
 198     OSD_STAT_TOTAL_DIST,
 199     OSD_STAT_WATT_HOURS_DRAWN,
 200     OSD_STAT_BEST_3_CONSEC_LAPS,
 201     OSD_STAT_BEST_LAP,
 202     OSD_STAT_FULL_THROTTLE_TIME,
 203     OSD_STAT_FULL_THROTTLE_COUNTER,
 204     OSD_STAT_AVG_THROTTLE,
 205 };
 206
 207 #define OSD_TASK_MARGIN                 1
 208 #define OSD_ELEMENT_MARGIN              4
 209
 210 // Decay the estimated max task duration by 1/(1 << OSD_EXEC_TIME_SHIFT) on every invocation
 211 #define OSD_EXEC_TIME_SHIFT             5
 212
 213 // Format a float to the specified number of decimal places with optional rounding.
 214 // OSD symbols can optionally be placed before and after the formatted number (use SYM_NONE for no symbol).
 215 // The formatString can be used for customized formatting of the integer part. Follow the printf style.
 216 // Pass an empty formatString for default.
 217 int osdPrintFloat(char *buffer, char leadingSymbol, float value, char *formatString, unsigned decimalPlaces, bool round, char trailingSymbol)
 218 {
 219     char mask[7];
 220     int pos = 0;
 221     int multiplier = 1;
 222     for (unsigned i = 0; i < decimalPlaces; i++) {
 223         multiplier *= 10;
 224     }
 225
 226     value *= multiplier;
 227     const int scaledValueAbs = abs(round ? (int)lrintf(value) : (int)value);
 228     const int integerPart = scaledValueAbs / multiplier;
 229     const int fractionalPart = scaledValueAbs % multiplier;
 230
 231     if (leadingSymbol != SYM_NONE) {
 232         buffer[pos++] = leadingSymbol;
 233     }
 234     if (value < 0 && (integerPart || fractionalPart)) {
 235         buffer[pos++] = '-';
 236     }
 237
 238     pos += tfp_sprintf(buffer + pos, (strlen(formatString) ? formatString : "%01u"), integerPart);
 239     if (decimalPlaces) {
 240         tfp_sprintf((char *)&mask, ".%%0%uu", decimalPlaces); // builds up the format string to be like ".%03u" for decimalPlaces == 3 as an example
 241         pos += tfp_sprintf(buffer + pos, mask, fractionalPart);
 242     }
 243
 244     if (trailingSymbol != SYM_NONE) {
 245         buffer[pos++] = trailingSymbol;
 246     }
 247     buffer[pos] = '\0';
 248
 249     return pos;
 250 }
 251
 252 void osdStatSetState(uint8_t statIndex, bool enabled)
 253 {
 254     if (enabled) {
 255         osdConfigMutable()->enabled_stats |= (1 << statIndex);
 256     } else {
 257         osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
 258     }
 259 }
 260
 261 bool osdStatGetState(uint8_t statIndex)
 262 {
 263     return osdConfig()->enabled_stats & (1 << statIndex);
 264 }
 265
 266 void osdWarnSetState(uint8_t warningIndex, bool enabled)
 267 {
 268     if (enabled) {
 269         osdConfigMutable()->enabledWarnings |= (1 << warningIndex);
 270     } else {
 271         osdConfigMutable()->enabledWarnings &= ~(1 << warningIndex);
 272     }
 273 }
 274
 275 bool osdWarnGetState(uint8_t warningIndex)
 276 {
 277     return osdConfig()->enabledWarnings & (1 << warningIndex);
 278 }
 279
 280 #ifdef USE_OSD_PROFILES
 281 static void setOsdProfile(uint8_t value)
 282 {
 283     // 1 ->> 001
 284     // 2 ->> 010
 285     // 3 ->> 100
 286     if (value <= OSD_PROFILE_COUNT) {
 287         if (value == 0) {
 288             osdProfile = 1;
 289         } else {
 290             osdProfile = 1 << (value - 1);
 291         }
 292     }
 293  }
 294
 295 uint8_t getCurrentOsdProfileIndex(void)
 296 {
 297     return osdConfig()->osdProfileIndex;
 298 }
 299
 300 void changeOsdProfileIndex(uint8_t profileIndex)
 301 {
 302     if (profileIndex <= OSD_PROFILE_COUNT) {
 303         osdConfigMutable()->osdProfileIndex = profileIndex;
 304         setOsdProfile(profileIndex);
 305         osdAnalyzeActiveElements();
 306     }
 307 }
 308 #endif
 309
 310 void osdAnalyzeActiveElements(void)
 311 {
 312     /* This code results in a total RX task RX_STATE_MODES state time of ~68us on an F411 overclocked to 108MHz
 313      * This upsets the scheduler task duration estimation and will break SPI RX communication. This can
 314      * occur in flight, e.g. when the OSD profile is changed by switch so can be ignored, or GPS sensor comms
 315      * is lost - only causing one late task instance.
 316      */
 317     schedulerIgnoreTaskExecTime();
 318
 319     osdAddActiveElements();
 320     osdDrawActiveElementsBackground(osdDisplayPort);
 321 }
 322
 323 const uint16_t osdTimerDefault[OSD_TIMER_COUNT] = {
 324         OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10),
 325         OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10)
 326 };
 327
 328 #ifdef USE_RACE_PRO
 329 #define RACE_PRO true
 330 #else
 331 #define RACE_PRO false
 332 #endif
 333
 334 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 335 {
 336     // Enable the default stats
 337     osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
 338     osdStatSetState(OSD_STAT_MAX_SPEED, !RACE_PRO);
 339     osdStatSetState(OSD_STAT_MIN_BATTERY, true);
 340     osdStatSetState(OSD_STAT_MIN_RSSI, !RACE_PRO);
 341     osdStatSetState(OSD_STAT_MAX_CURRENT, !RACE_PRO);
 342     osdStatSetState(OSD_STAT_USED_MAH, !RACE_PRO);
 343     osdStatSetState(OSD_STAT_BLACKBOX, !RACE_PRO);
 344     osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, !RACE_PRO);
 345     osdStatSetState(OSD_STAT_TIMER_2, true);
 346
 347     osdConfig->units = UNIT_METRIC;
 348
 349     // Enable all warnings by default
 350     for (int i=0; i < OSD_WARNING_COUNT; i++) {
 351         osdWarnSetState(i, true);
 352     }
 353     // turn off RSSI & Link Quality warnings by default
 354     osdWarnSetState(OSD_WARNING_RSSI, false);
 355     osdWarnSetState(OSD_WARNING_LINK_QUALITY, false);
 356     osdWarnSetState(OSD_WARNING_RSSI_DBM, false);
 357     osdWarnSetState(OSD_WARNING_RSNR, false);
 358     // turn off the over mah capacity warning
 359     osdWarnSetState(OSD_WARNING_OVER_CAP, false);
 360
 361 #ifdef USE_RC_STATS
 362     osdStatSetState(OSD_STAT_FULL_THROTTLE_TIME, true);
 363     osdStatSetState(OSD_STAT_FULL_THROTTLE_COUNTER, true);
 364     osdStatSetState(OSD_STAT_AVG_THROTTLE, true);
 365 #endif
 366
 367     osdConfig->timers[OSD_TIMER_1] = osdTimerDefault[OSD_TIMER_1];
 368     osdConfig->timers[OSD_TIMER_2] = osdTimerDefault[OSD_TIMER_2];
 369
 370     osdConfig->overlay_radio_mode = 2;
 371
 372     osdConfig->rssi_alarm = 20;
 373     osdConfig->link_quality_alarm = 80;
 374     osdConfig->cap_alarm  = 2200;
 375     osdConfig->alt_alarm  = 100; // meters or feet depend on configuration
 376     osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
 377     osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
 378     osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
 379     osdConfig->core_temp_alarm = 70; // a temperature above 70C should produce a warning, lockups have been reported above 80C
 380
 381     osdConfig->ahMaxPitch = 20; // 20 degrees
 382     osdConfig->ahMaxRoll = 40; // 40 degrees
 383
 384     osdConfig->osdProfileIndex = 1;
 385     osdConfig->ahInvert = false;
 386     for (int i=0; i < OSD_PROFILE_COUNT; i++) {
 387         osdConfig->profile[i][0] = '\0';
 388     }
 389     osdConfig->rssi_dbm_alarm = -60;
 390     osdConfig->rsnr_alarm = 4;
 391     osdConfig->gps_sats_show_pdop = false;
 392
 393     for (int i = 0; i < OSD_RCCHANNELS_COUNT; i++) {
 394         osdConfig->rcChannels[i] = -1;
 395     }
 396
 397     osdConfig->distance_alarm = 0;
 398     osdConfig->logo_on_arming = OSD_LOGO_ARMING_OFF;
 399     osdConfig->logo_on_arming_duration = 5;  // 0.5 seconds
 400
 401     osdConfig->camera_frame_width = 24;
 402     osdConfig->camera_frame_height = 11;
 403
 404     osdConfig->stat_show_cell_value = false;
 405     osdConfig->framerate_hz = OSD_FRAMERATE_DEFAULT_HZ;
 406     osdConfig->cms_background_type = DISPLAY_BACKGROUND_TRANSPARENT;
 407     #ifdef USE_CRAFTNAME_MSGS
 408     osdConfig->osd_craftname_msgs = false;   // Insert LQ/RSSI-dBm and warnings into CraftName
 409     #endif //USE_CRAFTNAME_MSGS
 410
 411     osdConfig->aux_channel = 1;
 412     osdConfig->aux_scale = 200;
 413     osdConfig->aux_symbol = 'A';
 414
 415     // Make it obvious on the configurator that the FC doesn't support HD
 416 #ifdef USE_OSD_HD
 417     osdConfig->displayPortDevice = OSD_DISPLAYPORT_DEVICE_MSP;
 418     osdConfig->canvas_cols = OSD_HD_COLS;
 419     osdConfig->canvas_rows = OSD_HD_ROWS;
 420 #else
 421     osdConfig->displayPortDevice = OSD_DISPLAYPORT_DEVICE_AUTO;
 422     osdConfig->canvas_cols = OSD_SD_COLS;
 423     osdConfig->canvas_rows = OSD_SD_ROWS;
 424 #endif
 425
 426 #ifdef USE_OSD_QUICK_MENU
 427     osdConfig->osd_use_quick_menu = true;
 428 #endif // USE_OSD_QUICK_MENU
 429
 430 #ifdef USE_RACE_PRO
 431     osdConfig->osd_show_spec_prearm = true;
 432 #endif // USE_RACE_PRO
 433 }
 434
 435 void pgResetFn_osdElementConfig(osdElementConfig_t *osdElementConfig)
 436 {
 437 // If user includes OSD_HD in the build assume they want to use it as default
 438 #ifdef USE_OSD_HD
 439     uint8_t midRow = 10;
 440     uint8_t midCol = 26;
 441 #else
 442     uint8_t midRow = 7;
 443     uint8_t midCol = 15;
 444 #endif
 445
 446     // Position elements near centre of screen and disabled by default
 447     for (int i = 0; i < OSD_ITEM_COUNT; i++) {
 448         osdElementConfig->item_pos[i] = OSD_POS((midCol - 5), midRow);
 449     }
 450
 451     // Always enable warnings elements by default
 452     uint16_t profileFlags = 0;
 453     for (unsigned i = 1; i <= OSD_PROFILE_COUNT; i++) {
 454         profileFlags |= OSD_PROFILE_FLAG(i);
 455     }
 456     osdElementConfig->item_pos[OSD_WARNINGS] = OSD_POS((midCol - OSD_WARNINGS_PREFFERED_SIZE / 2), (midRow + 3)) | profileFlags;
 457
 458     // Default to old fixed positions for these elements
 459     osdElementConfig->item_pos[OSD_CROSSHAIRS]         = OSD_POS((midCol - 2), (midRow - 1));
 460     osdElementConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS((midCol - 1), (midRow - 5));
 461     osdElementConfig->item_pos[OSD_HORIZON_SIDEBARS]   = OSD_POS((midCol - 1), (midRow - 1));
 462     osdElementConfig->item_pos[OSD_CAMERA_FRAME]       = OSD_POS((midCol - 12), (midRow - 6));
 463     osdElementConfig->item_pos[OSD_UP_DOWN_REFERENCE]  = OSD_POS((midCol - 2), (midRow - 1));
 464 }
 465
 466 static void osdDrawLogo(int x, int y, displayPortSeverity_e fontSel)
 467 {
 468     // display logo and help
 469     int fontOffset = 160;
 470     for (int row = 0; row < OSD_LOGO_ROWS; row++) {
 471         for (int column = 0; column < OSD_LOGO_COLS; column++) {
 472             if (fontOffset <= SYM_END_OF_FONT)
 473                 displayWriteChar(osdDisplayPort, x + column, y + row, fontSel, fontOffset++);
 474         }
 475     }
 476 }
 477
 478 static void osdCompleteInitialization(void)
 479 {
 480     uint8_t midRow = osdDisplayPort->rows / 2;
 481     uint8_t midCol = osdDisplayPort->cols / 2;
 482
 483     armState = ARMING_FLAG(ARMED);
 484
 485     osdResetAlarms();
 486
 487     backgroundLayerSupported = displayLayerSupported(osdDisplayPort, DISPLAYPORT_LAYER_BACKGROUND);
 488     displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND);
 489
 490     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
 491     displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_WAIT);
 492
 493     // Display betaflight logo
 494     osdDrawLogo(midCol - (OSD_LOGO_COLS) / 2, midRow - 5, DISPLAYPORT_SEVERITY_NORMAL);
 495
 496     char string_buffer[30];
 497     tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
 498     displayWrite(osdDisplayPort, midCol + 5, midRow, DISPLAYPORT_SEVERITY_NORMAL, string_buffer);
 499 #ifdef USE_CMS
 500     displayWrite(osdDisplayPort, midCol - 8, midRow + 2,  DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT1);
 501     displayWrite(osdDisplayPort, midCol - 4, midRow + 3, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT2);
 502     displayWrite(osdDisplayPort, midCol - 4, midRow + 4, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT3);
 503 #endif
 504
 505 #ifdef USE_RTC_TIME
 506     char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
 507     if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
 508         displayWrite(osdDisplayPort, midCol - 10, midRow + 6, DISPLAYPORT_SEVERITY_NORMAL, dateTimeBuffer);
 509     }
 510 #endif
 511
 512     resumeRefreshAt = micros() + (4 * REFRESH_1S);
 513 #ifdef USE_OSD_PROFILES
 514     setOsdProfile(osdConfig()->osdProfileIndex);
 515 #endif
 516
 517     osdElementsInit(backgroundLayerSupported);
 518     osdAnalyzeActiveElements();
 519
 520     osdIsReady = true;
 521 }
 522
 523 void osdInit(displayPort_t *osdDisplayPortToUse, osdDisplayPortDevice_e displayPortDeviceType)
 524 {
 525     osdDisplayPortDeviceType = displayPortDeviceType;
 526
 527     if (!osdDisplayPortToUse) {
 528         return;
 529     }
 530
 531     osdDisplayPort = osdDisplayPortToUse;
 532 #ifdef USE_CMS
 533     cmsDisplayPortRegister(osdDisplayPort);
 534 #endif
 535
 536     if (osdDisplayPort->cols && osdDisplayPort->rows) {
 537         // Ensure that osd_canvas_width/height are correct
 538         if (osdConfig()->canvas_cols != osdDisplayPort->cols) {
 539             osdConfigMutable()->canvas_cols = osdDisplayPort->cols;
 540         }
 541         if (osdConfig()->canvas_rows != osdDisplayPort->rows) {
 542             osdConfigMutable()->canvas_rows = osdDisplayPort->rows;
 543         }
 544
 545         // Ensure that all OSD elements are on the canvas once number of row/columns is known
 546         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
 547             uint16_t itemPos = osdElementConfig()->item_pos[i];
 548             uint8_t elemPosX = OSD_X(itemPos);
 549             uint8_t elemPosY = OSD_Y(itemPos);
 550             uint16_t elemProfileType = itemPos & (OSD_PROFILE_MASK | OSD_TYPE_MASK);
 551             bool pos_reset = false;
 552
 553             if (elemPosX >= osdDisplayPort->cols) {
 554                 elemPosX = osdDisplayPort->cols - 1;
 555                 pos_reset  = true;
 556             }
 557
 558             if (elemPosY >= osdDisplayPort->rows) {
 559                 elemPosY = osdDisplayPort->rows - 1;
 560                 pos_reset  = true;
 561             }
 562
 563             if (pos_reset) {
 564                 osdElementConfigMutable()->item_pos[i] = elemProfileType | OSD_POS(elemPosX, elemPosY);
 565             }
 566         }
 567     }
 568 }
 569
 570 #ifdef USE_GPS_LAP_TIMER
 571 void printLapTime(char *buffer, const uint32_t timeMs) {
 572     if (timeMs != 0) {
 573         const uint32_t timeRoundMs = timeMs + 5; // round value in division by 10
 574         const int timeSeconds = timeRoundMs / 1000;
 575         const int timeDecimals = (timeRoundMs % 1000) / 10;
 576         tfp_sprintf(buffer, "%3u.%02u", timeSeconds, timeDecimals);
 577     } else {
 578         tfp_sprintf(buffer, "  -.--");
 579     }
 580 }
 581 #endif // USE_GPS_LAP_TIMER
 582
 583 static void osdResetStats(void)
 584 {
 585     stats.max_current     = 0;
 586     stats.max_speed       = 0;
 587     stats.min_voltage     = 5000;
 588     stats.end_voltage     = 0;
 589     stats.min_rssi        = 99; // percent
 590     stats.max_altitude    = 0;
 591     stats.max_distance    = 0;
 592     stats.armed_time      = 0;
 593     stats.max_g_force     = 0;
 594     stats.max_esc_temp_ix = 0;
 595     stats.max_esc_temp    = 0;
 596     stats.max_esc_rpm     = 0;
 597     stats.min_link_quality = (linkQualitySource == LQ_SOURCE_NONE) ? 99 : 100; // percent
 598     stats.min_rssi_dbm = CRSF_RSSI_MAX;
 599     stats.min_rsnr = CRSF_SNR_MAX;
 600 }
 601
 602 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 603 static int32_t getAverageEscRpm(void)
 604 {
 605 #ifdef USE_ESC_SENSOR
 606     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 607         return lrintf(erpmToRpm(osdEscDataCombined->rpm));
 608     }
 609 #endif
 610 #ifdef USE_DSHOT_TELEMETRY
 611     if (useDshotTelemetry) {
 612         return lrintf(getDshotRpmAverage());
 613     }
 614 #endif
 615     return 0;
 616 }
 617 #endif
 618
 619 static uint16_t getStatsVoltage(void)
 620 {
 621     return osdConfig()->stat_show_cell_value ? getBatteryAverageCellVoltage() : getBatteryVoltage();
 622 }
 623
 624 static void osdUpdateStats(void)
 625 {
 626     int16_t value = 0;
 627
 628 #ifdef USE_GPS
 629     if (gpsConfig()->gps_use_3d_speed) {
 630         value = gpsSol.speed3d;
 631     } else {
 632         value = gpsSol.groundSpeed;
 633     }
 634     if (stats.max_speed < value) {
 635         stats.max_speed = value;
 636     }
 637 #endif
 638
 639     value = getStatsVoltage();
 640     if (stats.min_voltage > value) {
 641         stats.min_voltage = value;
 642     }
 643
 644     value = getAmperage() / 100;
 645     if (stats.max_current < value) {
 646         stats.max_current = value;
 647     }
 648
 649     value = getRssiPercent();
 650     if (stats.min_rssi > value) {
 651         stats.min_rssi = value;
 652     }
 653
 654     int32_t altitudeCm = getEstimatedAltitudeCm();
 655     if (stats.max_altitude < altitudeCm) {
 656         stats.max_altitude = altitudeCm;
 657     }
 658
 659 #if defined(USE_ACC)
 660     if (stats.max_g_force < osdGForce) {
 661         stats.max_g_force = osdGForce;
 662     }
 663 #endif
 664
 665 #ifdef USE_RX_LINK_QUALITY_INFO
 666     value = rxGetLinkQualityPercent();
 667     if (stats.min_link_quality > value) {
 668         stats.min_link_quality = value;
 669     }
 670 #endif
 671
 672 #ifdef USE_RX_RSSI_DBM
 673     value = getRssiDbm();
 674     if (stats.min_rssi_dbm > value) {
 675         stats.min_rssi_dbm = value;
 676     }
 677 #endif
 678
 679 #ifdef USE_RX_RSNR
 680     value = getRsnr();
 681     if (stats.min_rsnr > value) {
 682         stats.min_rsnr = value;
 683     }
 684 #endif
 685
 686 #ifdef USE_GPS
 687     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 688         if (stats.max_distance < GPS_distanceToHome) {
 689             stats.max_distance = GPS_distanceToHome;
 690         }
 691     }
 692 #endif
 693
 694 #if defined(USE_ESC_SENSOR)
 695     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 696         value = osdEscDataCombined->temperature;
 697         if (stats.max_esc_temp < value) {
 698             stats.max_esc_temp = value;
 699         }
 700     } else
 701 #endif
 702 #if defined(USE_DSHOT_TELEMETRY)
 703     {
 704         // Take max temp from dshot telemetry
 705         for (uint8_t k = 0; k < getMotorCount(); k++) {
 706             if (dshotTelemetryState.motorState[k].maxTemp > stats.max_esc_temp) {
 707                 stats.max_esc_temp_ix = k + 1;
 708                 stats.max_esc_temp = dshotTelemetryState.motorState[k].maxTemp;
 709             }
 710         }
 711     }
 712 #else
 713     {}
 714 #endif
 715
 716 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 717     int32_t rpm = getAverageEscRpm();
 718     if (stats.max_esc_rpm < rpm) {
 719         stats.max_esc_rpm = rpm;
 720     }
 721 #endif
 722 }
 723
 724 #ifdef USE_BLACKBOX
 725
 726 static void osdGetBlackboxStatusString(char * buff)
 727 {
 728     bool storageDeviceIsWorking = isBlackboxDeviceWorking();
 729     uint32_t storageUsed = 0;
 730     uint32_t storageTotal = 0;
 731
 732     switch (blackboxConfig()->device) {
 733 #ifdef USE_SDCARD
 734     case BLACKBOX_DEVICE_SDCARD:
 735         if (storageDeviceIsWorking) {
 736             storageTotal = sdcard_getMetadata()->numBlocks / 2000;
 737             storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
 738         }
 739         break;
 740 #endif
 741
 742 #ifdef USE_FLASHFS
 743     case BLACKBOX_DEVICE_FLASH:
 744         if (storageDeviceIsWorking) {
 745
 746             const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
 747             const flashGeometry_t *flashGeometry = flashGetGeometry();
 748
 749             storageTotal = ((FLASH_PARTITION_SECTOR_COUNT(flashPartition) * flashGeometry->sectorSize) / 1024);
 750             storageUsed = flashfsGetOffset() / 1024;
 751         }
 752         break;
 753 #endif
 754
 755     default:
 756         break;
 757     }
 758
 759     if (storageDeviceIsWorking) {
 760         const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
 761         tfp_sprintf(buff, "%d%%", storageUsedPercent);
 762     } else {
 763         tfp_sprintf(buff, "FAULT");
 764     }
 765 }
 766 #endif
 767
 768 static void osdDisplayStatisticLabel(uint8_t x, uint8_t y, const char * text, const char * value)
 769 {
 770     displayWrite(osdDisplayPort, x - 13, y, DISPLAYPORT_SEVERITY_NORMAL, text);
 771     displayWrite(osdDisplayPort, x + 5, y, DISPLAYPORT_SEVERITY_NORMAL, ":");
 772     displayWrite(osdDisplayPort, x + 7, y, DISPLAYPORT_SEVERITY_NORMAL, value);
 773 }
 774
 775 /*
 776  * Test if there's some stat enabled
 777  */
 778 static bool isSomeStatEnabled(void)
 779 {
 780     return (osdConfig()->enabled_stats != 0);
 781 }
 782
 783 // *** IMPORTANT ***
 784 // The stats display order was previously required to match the enumeration definition so it matched
 785 // the order shown in the configurator. However, to allow reordering this screen without breaking the
 786 // compatibility, this requirement has been relaxed to a best effort approach. Reordering the elements
 787 // on the stats screen will have to be more beneficial than the hassle of not matching exactly to the
 788 // configurator list.
 789
 790 static bool osdDisplayStat(int statistic, uint8_t displayRow)
 791 {
 792     uint8_t midCol = osdDisplayPort->cols / 2;
 793     char buff[OSD_ELEMENT_BUFFER_LENGTH];
 794
 795     switch (statistic) {
 796     case OSD_STAT_RTC_DATE_TIME: {
 797         bool success = false;
 798 #ifdef USE_RTC_TIME
 799         success = osdFormatRtcDateTime(&buff[0]);
 800 #endif
 801         if (!success) {
 802             tfp_sprintf(buff, "NO RTC");
 803         }
 804
 805         displayWrite(osdDisplayPort, midCol - 13, displayRow, DISPLAYPORT_SEVERITY_NORMAL, buff);
 806         return true;
 807     }
 808
 809     case OSD_STAT_TIMER_1:
 810         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
 811         osdDisplayStatisticLabel(midCol, displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
 812         return true;
 813
 814     case OSD_STAT_TIMER_2:
 815         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
 816         osdDisplayStatisticLabel(midCol, displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
 817         return true;
 818
 819     case OSD_STAT_MAX_ALTITUDE: {
 820         osdPrintFloat(buff, SYM_NONE, osdGetMetersToSelectedUnit(stats.max_altitude) / 100.0f, "", 1, true, osdGetMetersToSelectedUnitSymbol());
 821         osdDisplayStatisticLabel(midCol, displayRow, "MAX ALTITUDE", buff);
 822         return true;
 823     }
 824
 825 #ifdef USE_GPS
 826     case OSD_STAT_MAX_SPEED:
 827         if (featureIsEnabled(FEATURE_GPS)) {
 828             tfp_sprintf(buff, "%d%c", osdGetSpeedToSelectedUnit(stats.max_speed), osdGetSpeedToSelectedUnitSymbol());
 829             osdDisplayStatisticLabel(midCol, displayRow, "MAX SPEED", buff);
 830             return true;
 831         }
 832         break;
 833
 834     case OSD_STAT_MAX_DISTANCE:
 835         if (featureIsEnabled(FEATURE_GPS)) {
 836             osdFormatDistanceString(buff, stats.max_distance, SYM_NONE);
 837             osdDisplayStatisticLabel(midCol, displayRow, "MAX DISTANCE", buff);
 838             return true;
 839         }
 840         break;
 841
 842     case OSD_STAT_FLIGHT_DISTANCE:
 843         if (featureIsEnabled(FEATURE_GPS)) {
 844             const int distanceFlown = GPS_distanceFlownInCm / 100;
 845             osdFormatDistanceString(buff, distanceFlown, SYM_NONE);
 846             osdDisplayStatisticLabel(midCol, displayRow, "FLIGHT DISTANCE", buff);
 847             return true;
 848         }
 849         break;
 850 #endif
 851
 852     case OSD_STAT_MIN_BATTERY:
 853         osdPrintFloat(buff, SYM_NONE, stats.min_voltage / 100.0f, "", 2, true, SYM_VOLT);
 854         osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value? "MIN AVG CELL" : "MIN BATTERY", buff);
 855         return true;
 856
 857     case OSD_STAT_END_BATTERY:
 858         osdPrintFloat(buff, SYM_NONE, stats.end_voltage / 100.0f, "", 2, true, SYM_VOLT);
 859         osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value ? "END AVG CELL" : "END BATTERY", buff);
 860         return true;
 861
 862     case OSD_STAT_BATTERY:
 863         {
 864             const uint16_t statsVoltage = getStatsVoltage();
 865             osdPrintFloat(buff, SYM_NONE, statsVoltage / 100.0f, "", 2, true, SYM_VOLT);
 866             osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value ? "AVG BATT CELL" : "BATTERY", buff);
 867             return true;
 868         }
 869         break;
 870
 871     case OSD_STAT_MIN_RSSI:
 872         itoa(stats.min_rssi, buff, 10);
 873         strcat(buff, "%");
 874         osdDisplayStatisticLabel(midCol, displayRow, "MIN RSSI", buff);
 875         return true;
 876
 877     case OSD_STAT_MAX_CURRENT:
 878         if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
 879             tfp_sprintf(buff, "%d%c", stats.max_current, SYM_AMP);
 880             osdDisplayStatisticLabel(midCol, displayRow, "MAX CURRENT", buff);
 881             return true;
 882         }
 883         break;
 884
 885     case OSD_STAT_USED_MAH:
 886         if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
 887             tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
 888             osdDisplayStatisticLabel(midCol, displayRow, "USED MAH", buff);
 889             return true;
 890         }
 891         break;
 892
 893     case OSD_STAT_WATT_HOURS_DRAWN:
 894         if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
 895             osdPrintFloat(buff, SYM_NONE, getWhDrawn(), "", 2, true, SYM_NONE);
 896             osdDisplayStatisticLabel(midCol, displayRow, "USED WATT HOURS", buff);
 897             return true;
 898         }
 899         break;
 900
 901 #ifdef USE_BLACKBOX
 902     case OSD_STAT_BLACKBOX:
 903         if (blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
 904             osdGetBlackboxStatusString(buff);
 905             osdDisplayStatisticLabel(midCol, displayRow, "BLACKBOX", buff);
 906             return true;
 907         }
 908         break;
 909
 910     case OSD_STAT_BLACKBOX_NUMBER:
 911         {
 912             int32_t logNumber = blackboxGetLogNumber();
 913             if (logNumber >= 0) {
 914                 itoa(logNumber, buff, 10);
 915                 osdDisplayStatisticLabel(midCol, displayRow, "BB LOG NUM", buff);
 916                 return true;
 917             }
 918         }
 919         break;
 920 #endif
 921
 922 #if defined(USE_ACC)
 923     case OSD_STAT_MAX_G_FORCE:
 924         if (sensors(SENSOR_ACC)) {
 925             osdPrintFloat(buff, SYM_NONE, stats.max_g_force, "", 1, true, 'G');
 926             osdDisplayStatisticLabel(midCol, displayRow, "MAX G-FORCE", buff);
 927             return true;
 928         }
 929         break;
 930 #endif
 931
 932 #ifdef USE_ESC_SENSOR
 933     case OSD_STAT_MAX_ESC_TEMP:
 934     {
 935         uint16_t ix = 0;
 936         if (stats.max_esc_temp_ix > 0) {
 937             ix = tfp_sprintf(buff, "%d ", stats.max_esc_temp_ix);
 938         }
 939         tfp_sprintf(buff + ix, "%d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
 940         osdDisplayStatisticLabel(midCol, displayRow, "MAX ESC TEMP", buff);
 941         return true;
 942     }
 943 #endif
 944
 945 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 946     case OSD_STAT_MAX_ESC_RPM:
 947         itoa(stats.max_esc_rpm, buff, 10);
 948         osdDisplayStatisticLabel(midCol, displayRow, "MAX ESC RPM", buff);
 949         return true;
 950 #endif
 951
 952 #ifdef USE_RX_LINK_QUALITY_INFO
 953     case OSD_STAT_MIN_LINK_QUALITY:
 954         tfp_sprintf(buff, "%d", stats.min_link_quality);
 955         strcat(buff, "%");
 956         osdDisplayStatisticLabel(midCol, displayRow, "MIN LINK", buff);
 957         return true;
 958 #endif
 959
 960 #if defined(USE_DYN_NOTCH_FILTER)
 961     case OSD_STAT_MAX_FFT:
 962         if (isDynNotchActive()) {
 963             int value = getMaxFFT();
 964             if (value > 0) {
 965                 tfp_sprintf(buff, "%dHZ", value);
 966                 osdDisplayStatisticLabel(midCol, displayRow, "PEAK FFT", buff);
 967             } else {
 968                 osdDisplayStatisticLabel(midCol, displayRow, "PEAK FFT", "THRT<20%");
 969             }
 970             return true;
 971         }
 972         break;
 973 #endif
 974
 975 #ifdef USE_RX_RSSI_DBM
 976     case OSD_STAT_MIN_RSSI_DBM:
 977         tfp_sprintf(buff, "%3d", stats.min_rssi_dbm);
 978         osdDisplayStatisticLabel(midCol, displayRow, "MIN RSSI DBM", buff);
 979         return true;
 980 #endif
 981
 982 #ifdef USE_RX_RSNR
 983     case OSD_STAT_MIN_RSNR:
 984         tfp_sprintf(buff, "%3d", stats.min_rsnr);
 985         osdDisplayStatisticLabel(midCol, displayRow, "MIN RSNR", buff);
 986         return true;
 987 #endif
 988
 989 #ifdef USE_GPS_LAP_TIMER
 990     case OSD_STAT_BEST_3_CONSEC_LAPS: {
 991         printLapTime(buff, gpsLapTimerData.best3Consec);
 992         osdDisplayStatisticLabel(midCol, displayRow, "BEST 3 CON", buff);
 993         return true;
 994     }
 995
 996     case OSD_STAT_BEST_LAP: {
 997         printLapTime(buff, gpsLapTimerData.bestLapTime);
 998         osdDisplayStatisticLabel(midCol, displayRow, "BEST LAP", buff);
 999         return true;
1000     }
1001 #endif // USE_GPS_LAP_TIMER
1002
1003 #ifdef USE_PERSISTENT_STATS
1004     case OSD_STAT_TOTAL_FLIGHTS:
1005         itoa(statsConfig()->stats_total_flights, buff, 10);
1006         osdDisplayStatisticLabel(midCol, displayRow, "TOTAL FLIGHTS", buff);
1007         return true;
1008
1009     case OSD_STAT_TOTAL_TIME: {
1010         int minutes = statsConfig()->stats_total_time_s / 60;
1011         tfp_sprintf(buff, "%d:%02dH", minutes / 60, minutes % 60);
1012         osdDisplayStatisticLabel(midCol, displayRow, "TOTAL FLIGHT TIME", buff);
1013         return true;
1014     }
1015
1016     case OSD_STAT_TOTAL_DIST:
1017         #define METERS_PER_KILOMETER 1000
1018         #define METERS_PER_MILE      1609
1019         if (osdConfig()->units == UNIT_IMPERIAL) {
1020             tfp_sprintf(buff, "%d%c", statsConfig()->stats_total_dist_m / METERS_PER_MILE, SYM_MILES);
1021         } else {
1022             tfp_sprintf(buff, "%d%c", statsConfig()->stats_total_dist_m / METERS_PER_KILOMETER, SYM_KM);
1023         }
1024         osdDisplayStatisticLabel(midCol, displayRow, "TOTAL DISTANCE", buff);
1025         return true;
1026 #endif
1027 #ifdef USE_RC_STATS
1028     case OSD_STAT_FULL_THROTTLE_TIME: {
1029         int seconds = RcStatsGetFullThrottleTimeUs() / 1000000;
1030         const int minutes = seconds / 60;
1031         seconds = seconds % 60;
1032         tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
1033         osdDisplayStatisticLabel(midCol, displayRow, "100% THRT TIME", buff);
1034         return true;
1035     }
1036
1037     case OSD_STAT_FULL_THROTTLE_COUNTER: {
1038         itoa(RcStatsGetFullThrottleCounter(), buff, 10);
1039         osdDisplayStatisticLabel(midCol, displayRow, "100% THRT COUNT", buff);
1040         return true;
1041     }
1042
1043     case OSD_STAT_AVG_THROTTLE: {
1044         itoa(RcStatsGetAverageThrottle(), buff, 10);
1045         osdDisplayStatisticLabel(midCol, displayRow, "AVG THROTTLE", buff);
1046         return true;
1047     }
1048 #endif // USE_RC_STATS
1049     }
1050     return false;
1051 }
1052
1053 typedef struct osdStatsRenderingState_s {
1054     uint8_t row;
1055     uint8_t index;
1056     uint8_t rowCount;
1057 } osdStatsRenderingState_t;
1058
1059 static osdStatsRenderingState_t osdStatsRenderingState;
1060
1061 static void osdRenderStatsReset(void)
1062 {
1063     // reset to 0 so it will be recalculated on the next stats refresh
1064     osdStatsRenderingState.rowCount = 0;
1065 }
1066
1067 static void osdRenderStatsBegin(void)
1068 {
1069     osdStatsRenderingState.row = 0;
1070     osdStatsRenderingState.index = 0;
1071 }
1072
1073 // call repeatedly until it returns true which indicates that all stats have been rendered.
1074 static bool osdRenderStatsContinue(void)
1075 {
1076     uint8_t midCol = osdDisplayPort->cols / 2;
1077
1078     if (osdStatsRenderingState.row == 0) {
1079
1080         bool displayLabel = false;
1081
1082         // if rowCount is 0 then we're running an initial analysis of the active stats items
1083         if (osdStatsRenderingState.rowCount > 0) {
1084             const int availableRows = osdDisplayPort->rows;
1085             int displayRows = MIN(osdStatsRenderingState.rowCount, availableRows);
1086             if (osdStatsRenderingState.rowCount < availableRows) {
1087                 displayLabel = true;
1088                 displayRows++;
1089             }
1090             osdStatsRenderingState.row = (availableRows - displayRows) / 2;  // center the stats vertically
1091         }
1092
1093         if (displayLabel) {
1094             displayWrite(osdDisplayPort, midCol - (strlen("--- STATS ---") / 2), osdStatsRenderingState.row++, DISPLAYPORT_SEVERITY_NORMAL, "--- STATS ---");
1095             return false;
1096         }
1097     }
1098
1099     bool renderedStat = false;
1100
1101     while (osdStatsRenderingState.index < OSD_STAT_COUNT) {
1102         int index = osdStatsRenderingState.index;
1103
1104         // prepare for the next call to the method
1105         osdStatsRenderingState.index++;
1106
1107         // look for something to render
1108         if (osdStatGetState(osdStatsDisplayOrder[index])) {
1109             if (osdDisplayStat(osdStatsDisplayOrder[index], osdStatsRenderingState.row)) {
1110                 osdStatsRenderingState.row++;
1111                 renderedStat = true;
1112                 break;
1113             }
1114         }
1115     }
1116
1117     bool moreSpaceAvailable = osdStatsRenderingState.row < osdDisplayPort->rows;
1118
1119     if (renderedStat && moreSpaceAvailable) {
1120         return false;
1121     }
1122
1123     if (osdStatsRenderingState.rowCount == 0) {
1124         osdStatsRenderingState.rowCount = osdStatsRenderingState.row;
1125     }
1126
1127     return true;
1128 }
1129
1130 // returns true when all phases are complete
1131 static bool osdRefreshStats(void)
1132 {
1133     bool completed = false;
1134
1135     typedef enum {
1136         INITIAL_CLEAR_SCREEN = 0,
1137         COUNT_STATS,
1138         CLEAR_SCREEN,
1139         RENDER_STATS,
1140     } osdRefreshStatsPhase_e;
1141
1142     static osdRefreshStatsPhase_e phase = INITIAL_CLEAR_SCREEN;
1143
1144     switch (phase) {
1145     default:
1146     case INITIAL_CLEAR_SCREEN:
1147         osdRenderStatsBegin();
1148         if (osdStatsRenderingState.rowCount > 0) {
1149             phase = RENDER_STATS;
1150         } else {
1151             phase = COUNT_STATS;
1152         }
1153         displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_NONE);
1154         break;
1155     case COUNT_STATS:
1156         {
1157             // No stats row count has been set yet.
1158             // Go through the logic one time to determine how many stats are actually displayed.
1159             bool count_phase_complete = osdRenderStatsContinue();
1160             if (count_phase_complete) {
1161                 phase = CLEAR_SCREEN;
1162             }
1163             break;
1164         }
1165     case CLEAR_SCREEN:
1166         osdRenderStatsBegin();
1167         // Then clear the screen and commence with normal stats display which will
1168         // determine if the heading should be displayed and also center the content vertically.
1169         displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_NONE);
1170         phase = RENDER_STATS;
1171         break;
1172     case RENDER_STATS:
1173         completed = osdRenderStatsContinue();
1174         break;
1175     };
1176
1177     if (completed) {
1178         phase = INITIAL_CLEAR_SCREEN;
1179     }
1180
1181     return completed;
1182 }
1183
1184 static timeDelta_t osdShowArmed(void)
1185 {
1186     uint8_t midRow = osdDisplayPort->rows / 2;
1187     uint8_t midCol = osdDisplayPort->cols / 2;
1188     timeDelta_t ret;
1189
1190     displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_WAIT);
1191
1192     if ((osdConfig()->logo_on_arming == OSD_LOGO_ARMING_ON) || ((osdConfig()->logo_on_arming == OSD_LOGO_ARMING_FIRST) && !ARMING_FLAG(WAS_EVER_ARMED))) {
1193         uint8_t midRow = osdDisplayPort->rows / 2;
1194         uint8_t midCol = osdDisplayPort->cols / 2;
1195         osdDrawLogo(midCol - (OSD_LOGO_COLS) / 2, midRow - 5, osdConfig()->arming_logo);
1196         ret = osdConfig()->logo_on_arming_duration * 1e5;
1197     } else {
1198         ret = (REFRESH_1S / 2);
1199     }
1200     displayWrite(osdDisplayPort, midCol - (strlen("ARMED") / 2), midRow, DISPLAYPORT_SEVERITY_NORMAL, "ARMED");
1201
1202     if (isCrashFlipModeActive()) {
1203         displayWrite(osdDisplayPort, midCol - (strlen(CRASHFLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASHFLIP_WARNING);
1204     }
1205
1206     return ret;
1207 }
1208
1209 static bool osdStatsVisible = false;
1210 static bool osdStatsEnabled = false;
1211
1212 STATIC_UNIT_TESTED bool osdProcessStats1(timeUs_t currentTimeUs)
1213 {
1214     static timeUs_t lastTimeUs = 0;
1215     static timeUs_t osdStatsRefreshTimeUs;
1216     static timeUs_t osdAuxRefreshTimeUs = 0;
1217
1218     bool refreshStatsRequired = false;
1219
1220     // detect arm/disarm
1221     if (armState != ARMING_FLAG(ARMED)) {
1222         if (ARMING_FLAG(ARMED)) {
1223             osdStatsEnabled = false;
1224             osdStatsVisible = false;
1225             osdResetStats();
1226             resumeRefreshAt = osdShowArmed() + currentTimeUs;
1227         } else if (isSomeStatEnabled()
1228                    && !suppressStatsDisplay
1229                    && !failsafeIsActive()
1230                    && (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))
1231                        || !VISIBLE(osdElementConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
1232             osdStatsEnabled = true;
1233             resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
1234             stats.end_voltage = getStatsVoltage();
1235             osdRenderStatsReset();
1236         }
1237
1238         armState = ARMING_FLAG(ARMED);
1239     }
1240
1241     if (ARMING_FLAG(ARMED)) {
1242         osdUpdateStats();
1243         timeUs_t deltaT = currentTimeUs - lastTimeUs;
1244         osdFlyTime += deltaT;
1245         stats.armed_time += deltaT;
1246     } else if (osdStatsEnabled) {  // handle showing/hiding stats based on OSD disable switch position
1247         if (displayIsGrabbed(osdDisplayPort)) {
1248             osdStatsEnabled = false;
1249             resumeRefreshAt = 0;
1250             stats.armed_time = 0;
1251         } else {
1252             if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
1253                 osdStatsVisible = false;
1254                 displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_NONE);
1255             } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
1256                 if (!osdStatsVisible) {
1257                     osdStatsVisible = true;
1258                     osdStatsRefreshTimeUs = 0;
1259                 }
1260                 if (currentTimeUs >= osdStatsRefreshTimeUs) {
1261                     osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
1262                     refreshStatsRequired = true;
1263                 }
1264             }
1265         }
1266     }
1267
1268     if (VISIBLE(osdElementConfig()->item_pos[OSD_AUX_VALUE])) {
1269         const uint8_t auxChannel = osdConfig()->aux_channel + NON_AUX_CHANNEL_COUNT - 1;
1270         if (currentTimeUs > osdAuxRefreshTimeUs) {
1271             // aux channel start after main channels
1272             osdAuxValue = (constrain(rcData[auxChannel], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * osdConfig()->aux_scale / PWM_RANGE;
1273             osdAuxRefreshTimeUs = currentTimeUs + REFRESH_1S;
1274         }
1275     }
1276
1277     lastTimeUs = currentTimeUs;
1278
1279     return refreshStatsRequired;
1280 }
1281
1282 static void osdProcessStats2(timeUs_t currentTimeUs)
1283 {
1284     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
1285
1286     if (resumeRefreshAt) {
1287         if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
1288             // in timeout period, check sticks for activity or CRASHFLIP switch to resume display.
1289             if (!ARMING_FLAG(ARMED) &&
1290                 (IS_HI(THROTTLE) || IS_HI(PITCH) || IS_RC_MODE_ACTIVE(BOXCRASHFLIP))) {
1291                 resumeRefreshAt = currentTimeUs;
1292             }
1293             return;
1294         } else {
1295             displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_NONE);
1296             resumeRefreshAt = 0;
1297             osdStatsEnabled = false;
1298             stats.armed_time = 0;
1299         }
1300
1301         schedulerIgnoreTaskExecTime();
1302     }
1303 #ifdef USE_ESC_SENSOR
1304     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1305         osdEscDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
1306     }
1307 #endif
1308 }
1309
1310 static void osdProcessStats3(void)
1311 {
1312 #if defined(USE_ACC)
1313     osdGForce = 0.0f;
1314     if (sensors(SENSOR_ACC)
1315        && (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
1316             // only calculate the G force if the element is visible or the stat is enabled
1317         osdGForce = acc.accMagnitude;
1318     }
1319 #endif
1320 }
1321
1322 typedef enum {
1323     OSD_STATE_INIT,
1324     OSD_STATE_IDLE,
1325     OSD_STATE_CHECK,
1326     OSD_STATE_PROCESS_STATS1,
1327     OSD_STATE_REFRESH_STATS,
1328     OSD_STATE_PROCESS_STATS2,
1329     OSD_STATE_PROCESS_STATS3,
1330     OSD_STATE_UPDATE_ALARMS,
1331     OSD_STATE_REFRESH_PREARM,
1332     OSD_STATE_UPDATE_CANVAS,
1333     // Elements are handled in two steps, drawing into a buffer, and then sending to the display
1334     OSD_STATE_DRAW_ELEMENT,
1335     OSD_STATE_DISPLAY_ELEMENT,
1336     OSD_STATE_UPDATE_HEARTBEAT,
1337     OSD_STATE_COMMIT,
1338     OSD_STATE_TRANSFER,
1339     OSD_STATE_COUNT
1340 } osdState_e;
1341
1342 osdState_e osdState = OSD_STATE_INIT;
1343
1344 #define OSD_UPDATE_INTERVAL_US (1000000 / osdConfig()->framerate_hz)
1345
1346 // Called periodically by the scheduler
1347 bool osdUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
1348 {
1349     UNUSED(currentDeltaTimeUs);
1350     static timeUs_t osdUpdateDueUs = 0;
1351
1352     if (osdState == OSD_STATE_IDLE) {
1353         // If the OSD is due a refresh, mark that as being the case
1354         if (cmpTimeUs(currentTimeUs, osdUpdateDueUs) > 0) {
1355             osdState = OSD_STATE_CHECK;
1356
1357             // Determine time of next update
1358             if (osdUpdateDueUs) {
1359                 // Ensure there's not a flurry of updates to catch up
1360                 while (cmpTimeUs(osdUpdateDueUs, currentTimeUs) < 0) {
1361                     osdUpdateDueUs += OSD_UPDATE_INTERVAL_US;
1362                 }
1363             } else {
1364                 osdUpdateDueUs = currentTimeUs + OSD_UPDATE_INTERVAL_US;
1365             }
1366         }
1367     }
1368
1369     return (osdState != OSD_STATE_IDLE);
1370 }
1371
1372 // Called when there is OSD update work to be done
1373 void osdUpdate(timeUs_t currentTimeUs)
1374 {
1375     static uint16_t osdStateDurationFractionUs[OSD_STATE_COUNT] = { 0 };
1376     static uint32_t osdElementDurationFractionUs[OSD_ITEM_COUNT] = { 0 };
1377     static bool moreElementsToDraw;
1378
1379     timeUs_t executeTimeUs;
1380     osdState_e osdCurrentState = osdState;
1381
1382     if (osdState != OSD_STATE_UPDATE_CANVAS) {
1383         schedulerIgnoreTaskExecRate();
1384     }
1385
1386     switch (osdState) {
1387     case OSD_STATE_INIT:
1388         if (!displayCheckReady(osdDisplayPort, false)) {
1389             // Frsky osd need a display redraw after search for MAX7456 devices
1390             if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_FRSKYOSD) {
1391                 displayRedraw(osdDisplayPort);
1392             } else {
1393                 schedulerIgnoreTaskExecTime();
1394             }
1395             return;
1396         }
1397
1398         osdCompleteInitialization();
1399         displayRedraw(osdDisplayPort);
1400         osdState = OSD_STATE_COMMIT;
1401
1402         break;
1403
1404     case OSD_STATE_CHECK:
1405         // don't touch buffers if DMA transaction is in progress
1406         if (displayIsTransferInProgress(osdDisplayPort)) {
1407             break;
1408         }
1409
1410         osdState = OSD_STATE_UPDATE_HEARTBEAT;
1411         break;
1412
1413     case OSD_STATE_UPDATE_HEARTBEAT:
1414         if (displayHeartbeat(osdDisplayPort)) {
1415             // Extraordinary action was taken, so return without allowing osdStateDurationFractionUs table to be updated
1416             return;
1417         }
1418
1419         osdState = OSD_STATE_PROCESS_STATS1;
1420         break;
1421
1422     case OSD_STATE_PROCESS_STATS1:
1423         {
1424             bool refreshStatsRequired = osdProcessStats1(currentTimeUs);
1425
1426             if (refreshStatsRequired) {
1427                 osdState = OSD_STATE_REFRESH_STATS;
1428             } else {
1429                 osdState = OSD_STATE_PROCESS_STATS2;
1430             }
1431             break;
1432         }
1433     case OSD_STATE_REFRESH_STATS:
1434         {
1435             bool completed = osdRefreshStats();
1436             if (completed) {
1437                 osdState = OSD_STATE_PROCESS_STATS2;
1438             }
1439             break;
1440         }
1441     case OSD_STATE_PROCESS_STATS2:
1442         osdProcessStats2(currentTimeUs);
1443
1444         osdState = OSD_STATE_PROCESS_STATS3;
1445         break;
1446     case OSD_STATE_PROCESS_STATS3:
1447         osdProcessStats3();
1448
1449 #ifdef USE_CMS
1450         if (!displayIsGrabbed(osdDisplayPort))
1451 #endif
1452         {
1453             osdState = OSD_STATE_UPDATE_ALARMS;
1454             break;
1455         }
1456
1457         osdState = OSD_STATE_COMMIT;
1458         break;
1459
1460     case OSD_STATE_UPDATE_ALARMS:
1461         osdUpdateAlarms();
1462
1463         if (resumeRefreshAt) {
1464             osdState = OSD_STATE_TRANSFER;
1465         } else {
1466             osdState = OSD_STATE_UPDATE_CANVAS;
1467         }
1468         break;
1469
1470     case OSD_STATE_UPDATE_CANVAS:
1471         // Hide OSD when OSDSW mode is active
1472         if (IS_RC_MODE_ACTIVE(BOXOSD)) {
1473             displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_NONE);
1474             osdState = OSD_STATE_COMMIT;
1475             break;
1476         }
1477
1478         if (backgroundLayerSupported) {
1479             // Background layer is supported, overlay it onto the foreground
1480             // so that we only need to draw the active parts of the elements.
1481             displayLayerCopy(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND, DISPLAYPORT_LAYER_BACKGROUND);
1482         } else {
1483             // Background layer not supported, just clear the foreground in preparation
1484             // for drawing the elements including their backgrounds.
1485             displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_NONE);
1486         }
1487
1488 #ifdef USE_GPS
1489         static bool lastGpsSensorState;
1490         // Handle the case that the GPS_SENSOR may be delayed in activation
1491         // or deactivate if communication is lost with the module.
1492         const bool currentGpsSensorState = sensors(SENSOR_GPS);
1493         if (lastGpsSensorState != currentGpsSensorState) {
1494             lastGpsSensorState = currentGpsSensorState;
1495             osdAnalyzeActiveElements();
1496         }
1497 #endif // USE_GPS
1498
1499         osdSyncBlink(currentTimeUs);
1500
1501         osdState = OSD_STATE_DRAW_ELEMENT;
1502
1503         break;
1504
1505     case OSD_STATE_DRAW_ELEMENT:
1506         {
1507             uint8_t osdElement = osdGetActiveElement();
1508
1509             timeUs_t startElementTime = micros();
1510
1511             moreElementsToDraw = osdDrawNextActiveElement(osdDisplayPort);
1512
1513             executeTimeUs = micros() - startElementTime;
1514
1515             if (executeTimeUs > (osdElementDurationFractionUs[osdElement] >> OSD_EXEC_TIME_SHIFT)) {
1516                 osdElementDurationFractionUs[osdElement] = executeTimeUs << OSD_EXEC_TIME_SHIFT;
1517             } else if (osdElementDurationFractionUs[osdElement] > 0) {
1518                 // Slowly decay the max time
1519                 osdElementDurationFractionUs[osdElement]--;
1520             }
1521
1522             if (osdIsRenderPending()) {
1523                 osdState = OSD_STATE_DISPLAY_ELEMENT;
1524
1525                 // Render the element just drawn
1526                 break;
1527             }
1528
1529             if (moreElementsToDraw) {
1530                 // There are more elements to draw
1531                 break;
1532             }
1533
1534 #ifdef USE_SPEC_PREARM_SCREEN
1535             if (!ARMING_FLAG(ARMED) && osdConfig()->osd_show_spec_prearm) {
1536                 osdState = OSD_STATE_REFRESH_PREARM;
1537             } else
1538 #endif // USE_SPEC_PREARM_SCREEN
1539             {
1540                 osdState = OSD_STATE_COMMIT;
1541             }
1542         }
1543         break;
1544
1545     case OSD_STATE_DISPLAY_ELEMENT:
1546         {
1547             const bool pendingDisplay = osdDisplayActiveElement();
1548
1549             if (!pendingDisplay) {
1550                 if (moreElementsToDraw) {
1551                     // There is no more to draw so advance to the next element
1552                     osdState = OSD_STATE_DRAW_ELEMENT;
1553                 } else {
1554                     // Displaying the current element is complete and there are no futher elements to draw so advance
1555 #ifdef USE_SPEC_PREARM_SCREEN
1556                     if (!ARMING_FLAG(ARMED) && osdConfig()->osd_show_spec_prearm) {
1557                         osdState = OSD_STATE_REFRESH_PREARM;
1558                     } else
1559 #endif // USE_SPEC_PREARM_SCREEN
1560                     {
1561                         osdState = OSD_STATE_COMMIT;
1562                     }
1563                 }
1564             }
1565         }
1566         break;
1567
1568 #ifdef USE_SPEC_PREARM_SCREEN
1569     case OSD_STATE_REFRESH_PREARM:
1570         if (osdDrawSpec(osdDisplayPort)) {
1571             // Rendering is complete
1572             osdState = OSD_STATE_COMMIT;
1573         }
1574
1575         break;
1576 #endif // USE_SPEC_PREARM_SCREEN
1577
1578     case OSD_STATE_COMMIT:
1579         displayCommitTransaction(osdDisplayPort);
1580
1581         if (resumeRefreshAt) {
1582             osdState = OSD_STATE_IDLE;
1583         } else {
1584             osdState = OSD_STATE_TRANSFER;
1585         }
1586         break;
1587
1588     case OSD_STATE_TRANSFER:
1589         // Wait for any current transfer to complete
1590         if (displayIsTransferInProgress(osdDisplayPort)) {
1591             break;
1592         }
1593
1594         // Transfer may be broken into many parts
1595         if (displayDrawScreen(osdDisplayPort)) {
1596             break;
1597         }
1598
1599         osdState = OSD_STATE_IDLE;
1600
1601         break;
1602
1603     case OSD_STATE_IDLE:
1604     default:
1605         osdState = OSD_STATE_IDLE;
1606         break;
1607     }
1608
1609     if (!schedulerGetIgnoreTaskExecTime()) {
1610         executeTimeUs = micros() - currentTimeUs;
1611
1612         if (executeTimeUs > (osdStateDurationFractionUs[osdCurrentState] >> OSD_EXEC_TIME_SHIFT)) {
1613             osdStateDurationFractionUs[osdCurrentState] = executeTimeUs << OSD_EXEC_TIME_SHIFT;
1614         } else if (osdStateDurationFractionUs[osdCurrentState] > 0) {
1615             // Slowly decay the max time
1616             osdStateDurationFractionUs[osdCurrentState]--;
1617         }
1618     }
1619
1620     if (osdState == OSD_STATE_IDLE) {
1621         schedulerSetNextStateTime((osdStateDurationFractionUs[OSD_STATE_CHECK] >> OSD_EXEC_TIME_SHIFT));
1622     } else if (osdState == OSD_STATE_DRAW_ELEMENT) {
1623         schedulerSetNextStateTime((osdElementDurationFractionUs[osdGetActiveElement()] >> OSD_EXEC_TIME_SHIFT) + OSD_ELEMENT_MARGIN);
1624     } else {
1625         schedulerSetNextStateTime((osdStateDurationFractionUs[osdState] >> OSD_EXEC_TIME_SHIFT) + OSD_TASK_MARGIN);
1626     }
1627 }
1628
1629 void osdSuppressStats(bool flag)
1630 {
1631     suppressStatsDisplay = flag;
1632 }
1633
1634 #ifdef USE_OSD_PROFILES
1635 bool osdElementVisible(uint16_t value)
1636 {
1637     return (bool)((((value & OSD_PROFILE_MASK) >> OSD_PROFILE_BITS_POS) & osdProfile) != 0);
1638 }
1639 #endif
1640
1641 bool osdGetVisualBeeperState(void)
1642 {
1643     return showVisualBeeper;
1644 }
1645
1646 void osdSetVisualBeeperState(bool state)
1647 {
1648     showVisualBeeper = state;
1649 }
1650
1651 statistic_t *osdGetStats(void)
1652 {
1653     return &stats;
1654 }
1655
1656 #ifdef USE_ACC
1657 // Determine if there are any enabled stats that need
1658 // the ACC (currently only MAX_G_FORCE).
1659 static bool osdStatsNeedAccelerometer(void)
1660 {
1661     return osdStatGetState(OSD_STAT_MAX_G_FORCE);
1662 }
1663
1664 // Check if any enabled elements or stats need the ACC
1665 bool osdNeedsAccelerometer(void)
1666 {
1667     return osdStatsNeedAccelerometer() || osdElementsNeedAccelerometer();
1668 }
1669 #endif // USE_ACC
1670
1671 displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *displayPortDeviceType)
1672 {
1673     if (displayPortDeviceType) {
1674         *displayPortDeviceType = osdDisplayPortDeviceType;
1675     }
1676     return osdDisplayPort;
1677 }
1678
1679 #endif // USE_OSD