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