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