src/main/osd/osd.c

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