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