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