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