src/main/io/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/debug.h"
  44 #include "build/version.h"
  45
  46 #include "cms/cms.h"
  47 #include "cms/cms_types.h"
  48
  49 #include "common/axis.h"
  50 #include "common/maths.h"
  51 #include "common/printf.h"
  52 #include "common/typeconversion.h"
  53 #include "common/utils.h"
  54
  55 #include "config/feature.h"
  56
  57 #include "drivers/display.h"
  58 #include "drivers/flash.h"
  59 #include "drivers/max7456_symbols.h"
  60 #include "drivers/sdcard.h"
  61 #include "drivers/time.h"
  62
  63 #include "fc/config.h"
  64 #include "fc/core.h"
  65 #include "fc/rc_adjustments.h"
  66 #include "fc/rc_controls.h"
  67 #include "fc/rc_modes.h"
  68 #include "fc/rc.h"
  69 #include "fc/runtime_config.h"
  70
  71 #include "flight/gps_rescue.h"
  72 #include "flight/failsafe.h"
  73 #include "flight/position.h"
  74 #include "flight/imu.h"
  75 #include "flight/mixer.h"
  76 #include "flight/pid.h"
  77
  78 #include "io/asyncfatfs/asyncfatfs.h"
  79 #include "io/beeper.h"
  80 #include "io/flashfs.h"
  81 #include "io/gps.h"
  82 #include "io/osd.h"
  83 #include "io/vtx_string.h"
  84 #include "io/vtx.h"
  85
  86 #include "pg/pg.h"
  87 #include "pg/pg_ids.h"
  88 #include "pg/rx.h"
  89
  90 #include "rx/rx.h"
  91
  92 #include "sensors/acceleration.h"
  93 #include "sensors/adcinternal.h"
  94 #include "sensors/barometer.h"
  95 #include "sensors/battery.h"
  96 #include "sensors/esc_sensor.h"
  97 #include "sensors/sensors.h"
  98
  99 #ifdef USE_HARDWARE_REVISION_DETECTION
 100 #include "hardware_revision.h"
 101 #endif
 102
 103 #define VIDEO_BUFFER_CHARS_PAL    480
 104 #define FULL_CIRCLE 360
 105
 106 #define STICK_OVERLAY_HORIZONTAL_CHAR   '-'
 107 #define STICK_OVERLAY_VERTICAL_CHAR     '|'
 108 #define STICK_OVERLAY_CROSS_CHAR        '+'
 109 #define STICK_OVERLAY_CURSOR_CHAR       '0'
 110
 111 const char * const osdTimerSourceNames[] = {
 112     "ON TIME  ",
 113     "TOTAL ARM",
 114     "LAST ARM "
 115 };
 116
 117 // Blink control
 118
 119 static bool blinkState = true;
 120 static bool showVisualBeeper = false;
 121
 122 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
 123 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
 124 #define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
 125 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 126 #define BLINK(item) (IS_BLINK(item) && blinkState)
 127
 128 // Things in both OSD and CMS
 129
 130 #define IS_HI(X)  (rcData[X] > 1750)
 131 #define IS_LO(X)  (rcData[X] < 1250)
 132 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
 133
 134 static timeUs_t flyTime = 0;
 135 static float osdGForce = 0;
 136
 137 typedef struct statistic_s {
 138     timeUs_t armed_time;
 139     int16_t max_speed;
 140     int16_t min_voltage; // /10
 141     int16_t max_current; // /10
 142     uint8_t min_rssi;
 143     int32_t max_altitude;
 144     int16_t max_distance;
 145     float max_g_force;
 146     int16_t max_esc_temp;
 147     int32_t max_esc_rpm;
 148     uint8_t min_link_quality;
 149 } statistic_t;
 150
 151 typedef struct radioControls_s {
 152     uint8_t left_vertical;
 153     uint8_t left_horizontal;
 154     uint8_t right_vertical;
 155     uint8_t right_horizontal;
 156 } radioControls_t;
 157
 158 typedef enum radioModes_e {
 159     MODE1,
 160     MODE2,
 161     MODE3,
 162     MODE4
 163 } radioModes_t;
 164
 165 static statistic_t stats;
 166 #ifdef USE_OSD_STICK_OVERLAY
 167 static const radioControls_t radioModes[4] = {
 168     { PITCH,    YAW,    THROTTLE,   ROLL }, // Mode 1
 169     { THROTTLE, YAW,    PITCH,      ROLL }, // Mode 2
 170     { PITCH,    ROLL,   THROTTLE,   YAW  }, // Mode 3
 171     { THROTTLE, ROLL,   PITCH,      YAW  }, // Mode 4
 172 };
 173 #endif
 174 timeUs_t resumeRefreshAt = 0;
 175 #define REFRESH_1S    1000 * 1000
 176
 177 static uint8_t armState;
 178 static bool lastArmState;
 179 #ifdef USE_OSD_PROFILES
 180 static uint8_t osdProfile = 1;
 181 #endif
 182 static displayPort_t *osdDisplayPort;
 183
 184 static bool suppressStatsDisplay = false;
 185
 186 #ifdef USE_ESC_SENSOR
 187 static escSensorData_t *escDataCombined;
 188 #endif
 189
 190 #define AH_SYMBOL_COUNT 9
 191 #define AH_SIDEBAR_WIDTH_POS 7
 192 #define AH_SIDEBAR_HEIGHT_POS 3
 193
 194 static const char compassBar[] = {
 195   SYM_HEADING_W,
 196   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 197   SYM_HEADING_N,
 198   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 199   SYM_HEADING_E,
 200   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 201   SYM_HEADING_S,
 202   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 203   SYM_HEADING_W,
 204   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 205   SYM_HEADING_N,
 206   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE
 207 };
 208
 209 static const uint8_t osdElementDisplayOrder[] = {
 210     OSD_MAIN_BATT_VOLTAGE,
 211     OSD_RSSI_VALUE,
 212     OSD_CROSSHAIRS,
 213     OSD_HORIZON_SIDEBARS,
 214     OSD_ITEM_TIMER_1,
 215     OSD_ITEM_TIMER_2,
 216     OSD_REMAINING_TIME_ESTIMATE,
 217     OSD_FLYMODE,
 218     OSD_THROTTLE_POS,
 219     OSD_VTX_CHANNEL,
 220     OSD_CURRENT_DRAW,
 221     OSD_MAH_DRAWN,
 222     OSD_CRAFT_NAME,
 223     OSD_ALTITUDE,
 224     OSD_ROLL_PIDS,
 225     OSD_PITCH_PIDS,
 226     OSD_YAW_PIDS,
 227     OSD_POWER,
 228     OSD_PIDRATE_PROFILE,
 229     OSD_WARNINGS,
 230     OSD_AVG_CELL_VOLTAGE,
 231     OSD_DEBUG,
 232     OSD_PITCH_ANGLE,
 233     OSD_ROLL_ANGLE,
 234     OSD_MAIN_BATT_USAGE,
 235     OSD_DISARMED,
 236     OSD_NUMERICAL_HEADING,
 237     OSD_NUMERICAL_VARIO,
 238     OSD_COMPASS_BAR,
 239     OSD_ANTI_GRAVITY,
 240     OSD_MOTOR_DIAG,
 241     OSD_FLIP_ARROW,
 242 #ifdef USE_RTC_TIME
 243     OSD_RTC_DATETIME,
 244 #endif
 245 #ifdef USE_OSD_ADJUSTMENTS
 246     OSD_ADJUSTMENT_RANGE,
 247 #endif
 248 #ifdef USE_ADC_INTERNAL
 249     OSD_CORE_TEMPERATURE,
 250 #endif
 251 #ifdef USE_RX_LINK_QUALITY_INFO
 252     OSD_LINK_QUALITY,
 253 #endif
 254 };
 255
 256 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 4);
 257
 258 /**
 259  * Gets the correct altitude symbol for the current unit system
 260  */
 261 static char osdGetMetersToSelectedUnitSymbol(void)
 262 {
 263     switch (osdConfig()->units) {
 264     case OSD_UNIT_IMPERIAL:
 265         return SYM_FT;
 266     default:
 267         return SYM_M;
 268     }
 269 }
 270
 271 /**
 272  * Gets average battery cell voltage in 0.01V units.
 273  */
 274 static int osdGetBatteryAverageCellVoltage(void)
 275 {
 276     return (getBatteryVoltage() * 10) / getBatteryCellCount();
 277 }
 278
 279 static char osdGetBatterySymbol(int cellVoltage)
 280 {
 281     if (getBatteryState() == BATTERY_CRITICAL) {
 282         return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
 283     } else {
 284         // Calculate a symbol offset using cell voltage over full cell voltage range
 285         const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7);
 286         return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
 287     }
 288 }
 289
 290 /**
 291  * Converts altitude based on the current unit system.
 292  * @param meters Value in meters to convert
 293  */
 294 static int32_t osdGetMetersToSelectedUnit(int32_t meters)
 295 {
 296     switch (osdConfig()->units) {
 297     case OSD_UNIT_IMPERIAL:
 298         return (meters * 328) / 100; // Convert to feet / 100
 299     default:
 300         return meters;               // Already in metre / 100
 301     }
 302 }
 303
 304 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 305 STATIC_UNIT_TESTED int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
 306 {
 307     switch (osdConfig()->units) {
 308     case OSD_UNIT_IMPERIAL:
 309         return lrintf(((tempInDegreesCelcius * 9.0f) / 5) + 32);
 310     default:
 311         return tempInDegreesCelcius;
 312     }
 313 }
 314
 315 static char osdGetTemperatureSymbolForSelectedUnit(void)
 316 {
 317     switch (osdConfig()->units) {
 318     case OSD_UNIT_IMPERIAL:
 319         return 'F';
 320     default:
 321         return 'C';
 322     }
 323 }
 324 #endif
 325
 326 static void osdFormatAltitudeString(char * buff, int32_t altitudeCm)
 327 {
 328     const int alt = osdGetMetersToSelectedUnit(altitudeCm) / 10;
 329
 330     tfp_sprintf(buff, "%5d %c", alt, osdGetMetersToSelectedUnitSymbol());
 331     buff[5] = buff[4];
 332     buff[4] = '.';
 333 }
 334
 335 static void osdFormatPID(char * buff, const char * label, const pidf_t * pid)
 336 {
 337     tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
 338 }
 339
 340 static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
 341 {
 342     heading += FULL_CIRCLE;  // Ensure positive value
 343
 344     // Split input heading 0..359 into sectors 0..(directions-1), but offset
 345     // by half a sector so that sector 0 gets centered around heading 0.
 346     // We multiply heading by directions to not loose precision in divisions
 347     // In this way each segment will be a FULL_CIRCLE length
 348     int direction = (heading * directions + FULL_CIRCLE / 2) /  FULL_CIRCLE; // scale with rounding
 349     direction %= directions; // normalize
 350
 351     return direction; // return segment number
 352 }
 353
 354 static uint8_t osdGetDirectionSymbolFromHeading(int heading)
 355 {
 356     heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
 357
 358     // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
 359     // Our symbols are Down=0, Right=4, Up=8 and Left=12
 360     // There're 16 arrow symbols. Transform it.
 361     heading = 16 - heading;
 362     heading = (heading + 8) % 16;
 363
 364     return SYM_ARROW_SOUTH + heading;
 365 }
 366
 367 static char osdGetTimerSymbol(osd_timer_source_e src)
 368 {
 369     switch (src) {
 370     case OSD_TIMER_SRC_ON:
 371         return SYM_ON_M;
 372     case OSD_TIMER_SRC_TOTAL_ARMED:
 373     case OSD_TIMER_SRC_LAST_ARMED:
 374         return SYM_FLY_M;
 375     default:
 376         return ' ';
 377     }
 378 }
 379
 380 static timeUs_t osdGetTimerValue(osd_timer_source_e src)
 381 {
 382     switch (src) {
 383     case OSD_TIMER_SRC_ON:
 384         return micros();
 385     case OSD_TIMER_SRC_TOTAL_ARMED:
 386         return flyTime;
 387     case OSD_TIMER_SRC_LAST_ARMED:
 388         return stats.armed_time;
 389     default:
 390         return 0;
 391     }
 392 }
 393
 394 STATIC_UNIT_TESTED void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
 395 {
 396     int seconds = time / 1000000;
 397     const int minutes = seconds / 60;
 398     seconds = seconds % 60;
 399
 400     switch (precision) {
 401     case OSD_TIMER_PREC_SECOND:
 402     default:
 403         tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
 404         break;
 405     case OSD_TIMER_PREC_HUNDREDTHS:
 406         {
 407             const int hundredths = (time / 10000) % 100;
 408             tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
 409             break;
 410         }
 411     }
 412 }
 413
 414 STATIC_UNIT_TESTED void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
 415 {
 416     const uint16_t timer = osdConfig()->timers[timerIndex];
 417     const uint8_t src = OSD_TIMER_SRC(timer);
 418
 419     if (showSymbol) {
 420         *(buff++) = osdGetTimerSymbol(src);
 421     }
 422
 423     osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
 424 }
 425
 426 #ifdef USE_GPS
 427 static void osdFormatCoordinate(char *buff, char sym, int32_t val)
 428 {
 429     // latitude maximum integer width is 3 (-90).
 430     // longitude maximum integer width is 4 (-180).
 431     // We show 7 decimals, so we need to use 12 characters:
 432     // eg: s-180.1234567z   s=symbol, z=zero terminator, decimal separator  between 0 and 1
 433
 434     static const int coordinateMaxLength = 13;//12 for the number (4 + dot + 7) + 1 for the symbol
 435
 436     buff[0] = sym;
 437     const int32_t integerPart = val / GPS_DEGREES_DIVIDER;
 438     const int32_t decimalPart = labs(val % GPS_DEGREES_DIVIDER);
 439     const int written = tfp_sprintf(buff + 1, "%d.%07d", integerPart, decimalPart);
 440     // pad with blanks to coordinateMaxLength
 441     for (int pos = 1 + written; pos < coordinateMaxLength; ++pos) {
 442         buff[pos] = SYM_BLANK;
 443     }
 444     buff[coordinateMaxLength] = '\0';
 445 }
 446 #endif // USE_GPS
 447
 448 #ifdef USE_RTC_TIME
 449 static bool osdFormatRtcDateTime(char *buffer)
 450 {
 451     dateTime_t dateTime;
 452     if (!rtcGetDateTime(&dateTime)) {
 453         buffer[0] = '\0';
 454
 455         return false;
 456     }
 457
 458     dateTimeFormatLocalShort(buffer, &dateTime);
 459
 460     return true;
 461 }
 462 #endif
 463
 464 static void osdFormatMessage(char *buff, size_t size, const char *message)
 465 {
 466     memset(buff, SYM_BLANK, size);
 467     if (message) {
 468         memcpy(buff, message, strlen(message));
 469     }
 470     // Ensure buff is zero terminated
 471     buff[size - 1] = '\0';
 472 }
 473
 474 void osdStatSetState(uint8_t statIndex, bool enabled)
 475 {
 476     if (enabled) {
 477         osdConfigMutable()->enabled_stats |= (1 << statIndex);
 478     } else {
 479         osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
 480     }
 481 }
 482
 483 bool osdStatGetState(uint8_t statIndex)
 484 {
 485     return osdConfig()->enabled_stats & (1 << statIndex);
 486 }
 487
 488 void osdWarnSetState(uint8_t warningIndex, bool enabled)
 489 {
 490     if (enabled) {
 491         osdConfigMutable()->enabledWarnings |= (1 << warningIndex);
 492     } else {
 493         osdConfigMutable()->enabledWarnings &= ~(1 << warningIndex);
 494     }
 495 }
 496
 497 bool osdWarnGetState(uint8_t warningIndex)
 498 {
 499     return osdConfig()->enabledWarnings & (1 << warningIndex);
 500 }
 501
 502 #ifdef USE_OSD_PROFILES
 503 void setOsdProfile(uint8_t value)
 504 {
 505     // 1 ->> 001
 506     // 2 ->> 010
 507     // 3 ->> 100
 508     if (value <= OSD_PROFILE_COUNT) {
 509         if (value == 0) {
 510             osdProfile = 1;
 511         } else {
 512             osdProfile = 1 << (value - 1);
 513         }
 514     }
 515  }
 516
 517 uint8_t getCurrentOsdProfileIndex(void)
 518 {
 519     return osdConfig()->osdProfileIndex;
 520 }
 521
 522 void changeOsdProfileIndex(uint8_t profileIndex)
 523 {
 524     if (profileIndex <= OSD_PROFILE_COUNT) {
 525         osdConfigMutable()->osdProfileIndex = profileIndex;
 526         setOsdProfile(profileIndex);
 527     }
 528 }
 529 #endif
 530
 531 static bool osdDrawSingleElement(uint8_t item)
 532 {
 533     if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
 534         return false;
 535     }
 536
 537     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
 538     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
 539     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
 540
 541     switch (item) {
 542     case OSD_FLIP_ARROW:
 543         {
 544             int rollAngle = attitude.values.roll / 10;
 545             const int pitchAngle = attitude.values.pitch / 10;
 546             if (abs(rollAngle) > 90) {
 547                 rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
 548             }
 549
 550             if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) {
 551                 if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
 552                     if (pitchAngle > 0) {
 553                         if (rollAngle > 0) {
 554                             buff[0] = SYM_ARROW_WEST + 2;
 555                         } else {
 556                             buff[0] = SYM_ARROW_EAST - 2;
 557                         }
 558                     } else {
 559                         if (rollAngle > 0) {
 560                             buff[0] = SYM_ARROW_WEST - 2;
 561                         } else {
 562                             buff[0] = SYM_ARROW_EAST + 2;
 563                         }
 564                     }
 565                 } else {
 566                     if (abs(pitchAngle) > abs(rollAngle)) {
 567                         if (pitchAngle > 0) {
 568                             buff[0] = SYM_ARROW_SOUTH;
 569                         } else {
 570                             buff[0] = SYM_ARROW_NORTH;
 571                         }
 572                     } else {
 573                         if (rollAngle > 0) {
 574                             buff[0] = SYM_ARROW_WEST;
 575                         } else {
 576                             buff[0] = SYM_ARROW_EAST;
 577                         }
 578                     }
 579                 }
 580             } else {
 581                 buff[0] = ' ';
 582             }
 583
 584             buff[1] = '\0';
 585             break;
 586         }
 587     case OSD_RSSI_VALUE:
 588         {
 589             uint16_t osdRssi = getRssi() * 100 / 1024; // change range
 590             if (osdRssi >= 100) {
 591                 osdRssi = 99;
 592             }
 593
 594             tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi);
 595             break;
 596         }
 597
 598 #ifdef USE_RX_LINK_QUALITY_INFO
 599     case OSD_LINK_QUALITY:
 600         {
 601             // change range to 0-9 (two sig. fig. adds little extra value, also reduces screen estate)
 602             uint8_t osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
 603             if (osdLinkQuality >= 10) {
 604                 osdLinkQuality = 9;
 605             }
 606
 607             tfp_sprintf(buff, "%1d", osdLinkQuality);
 608             break;
 609         }
 610 #endif
 611
 612     case OSD_MAIN_BATT_VOLTAGE:
 613         buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage());
 614         tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
 615         break;
 616
 617     case OSD_CURRENT_DRAW:
 618         {
 619             const int32_t amperage = getAmperage();
 620             tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
 621             break;
 622         }
 623
 624     case OSD_MAH_DRAWN:
 625         tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH);
 626         break;
 627
 628 #ifdef USE_GPS
 629     case OSD_GPS_SATS:
 630         tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
 631         break;
 632
 633     case OSD_GPS_SPEED:
 634         // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K (M for MPH)
 635         switch (osdConfig()->units) {
 636         case OSD_UNIT_IMPERIAL:
 637             tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed));
 638             break;
 639         default:
 640             tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
 641             break;
 642         }
 643         break;
 644
 645     case OSD_GPS_LAT:
 646         // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH
 647         osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat);
 648         break;
 649
 650     case OSD_GPS_LON:
 651         // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST
 652         osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon);
 653         break;
 654
 655     case OSD_HOME_DIR:
 656         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 657             if (GPS_distanceToHome > 0) {
 658                 const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 659                 buff[0] = osdGetDirectionSymbolFromHeading(h);
 660             } else {
 661                 // We don't have a HOME symbol in the font, by now we use this
 662                 buff[0] = SYM_THR1;
 663             }
 664
 665         } else {
 666             // We use this symbol when we don't have a FIX
 667             buff[0] = SYM_COLON;
 668         }
 669
 670         buff[1] = 0;
 671
 672         break;
 673
 674     case OSD_HOME_DIST:
 675         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 676             const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome);
 677             tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
 678         } else {
 679             // We use this symbol when we don't have a FIX
 680             buff[0] = SYM_COLON;
 681             // overwrite any previous distance with blanks
 682             memset(buff + 1, SYM_BLANK, 6);
 683             buff[7] = '\0';
 684         }
 685         break;
 686
 687     case OSD_FLIGHT_DIST:
 688         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 689             const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceFlownInCm / 100);
 690             tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
 691         } else {
 692             // We use this symbol when we don't have a FIX
 693             buff[0] = SYM_COLON;
 694             // overwrite any previous distance with blanks
 695             memset(buff + 1, SYM_BLANK, 6);
 696             buff[7] = '\0';
 697         }
 698         break;
 699
 700 #endif // GPS
 701
 702     case OSD_COMPASS_BAR:
 703         memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
 704         buff[9] = 0;
 705         break;
 706
 707     case OSD_ALTITUDE:
 708         osdFormatAltitudeString(buff, getEstimatedAltitudeCm());
 709         break;
 710
 711     case OSD_ITEM_TIMER_1:
 712     case OSD_ITEM_TIMER_2:
 713         osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1);
 714         break;
 715
 716     case OSD_REMAINING_TIME_ESTIMATE:
 717         {
 718             const int mAhDrawn = getMAhDrawn();
 719
 720             if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) {  // also handles the mAhDrawn == 0 condition
 721                 tfp_sprintf(buff, "--:--");
 722             } else if (mAhDrawn > osdConfig()->cap_alarm) {
 723                 tfp_sprintf(buff, "00:00");
 724             } else {
 725                 const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn);
 726                 osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time);
 727             }
 728             break;
 729         }
 730
 731     case OSD_FLYMODE:
 732         {
 733             // Note that flight mode display has precedence in what to display.
 734             //  1. FS
 735             //  2. GPS RESCUE
 736             //  3. ANGLE, HORIZON, ACRO TRAINER
 737             //  4. AIR
 738             //  5. ACRO
 739
 740             if (FLIGHT_MODE(FAILSAFE_MODE)) {
 741                 strcpy(buff, "!FS!");
 742             } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 743                 strcpy(buff, "RESC");
 744             } else if (FLIGHT_MODE(HEADFREE_MODE)) {
 745                 strcpy(buff, "HEAD");
 746             } else if (FLIGHT_MODE(ANGLE_MODE)) {
 747                 strcpy(buff, "STAB");
 748             } else if (FLIGHT_MODE(HORIZON_MODE)) {
 749                 strcpy(buff, "HOR ");
 750             } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
 751                 strcpy(buff, "ATRN");
 752             } else if (airmodeIsEnabled()) {
 753                 strcpy(buff, "AIR ");
 754             } else {
 755                 strcpy(buff, "ACRO");
 756             }
 757
 758             break;
 759         }
 760
 761     case OSD_ANTI_GRAVITY:
 762         {
 763             if (pidOsdAntiGravityActive()) {
 764                 strcpy(buff, "AG");
 765             }
 766
 767             break;
 768         }
 769
 770     case OSD_MOTOR_DIAG:
 771         {
 772             int i = 0;
 773             const bool motorsRunning = areMotorsRunning();
 774             for (; i < getMotorCount(); i++) {
 775                 if (motorsRunning) {
 776                     buff[i] =  0x88 - scaleRange(motor[i], motorOutputLow, motorOutputHigh, 0, 8);
 777                 } else {
 778                     buff[i] =  0x88;
 779                 }
 780             }
 781             buff[i] = '\0';
 782             break;
 783         }
 784
 785     case OSD_CRAFT_NAME:
 786         // This does not strictly support iterative updating if the craft name changes at run time. But since the craft name is not supposed to be changing this should not matter, and blanking the entire length of the craft name string on update will make it impossible to configure elements to be displayed on the right hand side of the craft name.
 787         //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered.
 788
 789         if (strlen(pilotConfig()->name) == 0) {
 790             strcpy(buff, "CRAFT_NAME");
 791         } else {
 792             unsigned i;
 793             for (i = 0; i < MAX_NAME_LENGTH; i++) {
 794                 if (pilotConfig()->name[i]) {
 795                     buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
 796                 } else {
 797                     break;
 798                 }
 799             }
 800             buff[i] = '\0';
 801         }
 802
 803         break;
 804
 805     case OSD_THROTTLE_POS:
 806         buff[0] = SYM_THR;
 807         buff[1] = SYM_THR1;
 808         tfp_sprintf(buff + 2, "%3d", calculateThrottlePercent());
 809         break;
 810
 811 #if defined(USE_VTX_COMMON)
 812     case OSD_VTX_CHANNEL:
 813         {
 814             const vtxDevice_t *vtxDevice = vtxCommonDevice();
 815             const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, vtxSettingsConfig()->band);
 816             const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, vtxSettingsConfig()->channel);
 817             uint8_t vtxPower = vtxSettingsConfig()->power;
 818             if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
 819                 vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
 820             }
 821             tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
 822             break;
 823         }
 824 #endif
 825
 826     case OSD_CROSSHAIRS:
 827         buff[0] = SYM_AH_CENTER_LINE;
 828         buff[1] = SYM_AH_CENTER;
 829         buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 830         buff[3] = 0;
 831         break;
 832
 833     case OSD_ARTIFICIAL_HORIZON:
 834         {
 835             // Get pitch and roll limits in tenths of degrees
 836             const int maxPitch = osdConfig()->ahMaxPitch * 10;
 837             const int maxRoll = osdConfig()->ahMaxRoll * 10;
 838             const int ahSign = osdConfig()->ahInvert ? -1 : 1;
 839             const int rollAngle = constrain(attitude.values.roll * ahSign, -maxRoll, maxRoll);
 840             int pitchAngle = constrain(attitude.values.pitch * ahSign, -maxPitch, maxPitch);
 841             // Convert pitchAngle to y compensation value
 842             // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
 843             if (maxPitch > 0) {
 844                 pitchAngle = ((pitchAngle * 25) / maxPitch);
 845             }
 846             pitchAngle -= 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
 847
 848             for (int x = -4; x <= 4; x++) {
 849                 const int y = ((-rollAngle * x) / 64) - pitchAngle;
 850                 if (y >= 0 && y <= 81) {
 851                     displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
 852                 }
 853             }
 854
 855             return true;
 856         }
 857
 858     case OSD_HORIZON_SIDEBARS:
 859         {
 860             // Draw AH sides
 861             const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
 862             const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
 863             for (int y = -hudheight; y <= hudheight; y++) {
 864                 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION);
 865                 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION);
 866             }
 867
 868             // AH level indicators
 869             displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
 870             displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
 871
 872             return true;
 873         }
 874
 875     case OSD_G_FORCE:
 876         {
 877             const int gForce = lrintf(osdGForce * 10);
 878             tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10);
 879             break;
 880         }
 881
 882     case OSD_ROLL_PIDS:
 883         osdFormatPID(buff, "ROL", &currentPidProfile->pid[PID_ROLL]);
 884         break;
 885
 886     case OSD_PITCH_PIDS:
 887         osdFormatPID(buff, "PIT", &currentPidProfile->pid[PID_PITCH]);
 888         break;
 889
 890     case OSD_YAW_PIDS:
 891         osdFormatPID(buff, "YAW", &currentPidProfile->pid[PID_YAW]);
 892         break;
 893
 894     case OSD_POWER:
 895         tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000);
 896         break;
 897
 898     case OSD_PIDRATE_PROFILE:
 899         tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
 900         break;
 901
 902     case OSD_WARNINGS:
 903         {
 904
 905 #define OSD_WARNINGS_MAX_SIZE 11
 906 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
 907
 908             STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
 909
 910             const batteryState_e batteryState = getBatteryState();
 911             const timeUs_t currentTimeUs = micros();
 912
 913             static timeUs_t armingDisabledUpdateTimeUs;
 914             static unsigned armingDisabledDisplayIndex;
 915
 916             CLR_BLINK(OSD_WARNINGS);
 917
 918             // Cycle through the arming disabled reasons
 919             if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE)) {
 920                 if (IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
 921                     const armingDisableFlags_e armSwitchOnlyFlag = 1 << (ARMING_DISABLE_FLAGS_COUNT - 1);
 922                     armingDisableFlags_e flags = getArmingDisableFlags();
 923
 924                     // Remove the ARMSWITCH flag unless it's the only one
 925                     if ((flags & armSwitchOnlyFlag) && (flags != armSwitchOnlyFlag)) {
 926                         flags -= armSwitchOnlyFlag;
 927                     }
 928
 929                     // Rotate to the next arming disabled reason after a 0.5 second time delay
 930                     // or if the current flag is no longer set
 931                     if ((currentTimeUs - armingDisabledUpdateTimeUs > 5e5) || !(flags & (1 << armingDisabledDisplayIndex))) {
 932                         if (armingDisabledUpdateTimeUs == 0) {
 933                             armingDisabledDisplayIndex = ARMING_DISABLE_FLAGS_COUNT - 1;
 934                         }
 935                         armingDisabledUpdateTimeUs = currentTimeUs;
 936
 937                         do {
 938                             if (++armingDisabledDisplayIndex >= ARMING_DISABLE_FLAGS_COUNT) {
 939                                 armingDisabledDisplayIndex = 0;
 940                             }
 941                         } while (!(flags & (1 << armingDisabledDisplayIndex)));
 942                     }
 943
 944                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[armingDisabledDisplayIndex]);
 945                     break;
 946                 } else {
 947                     armingDisabledUpdateTimeUs = 0;
 948                 }
 949             }
 950
 951 #ifdef USE_DSHOT
 952             if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
 953                 int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - currentTimeUs) / 1e5;
 954                 if (armingDelayTime < 0) {
 955                     armingDelayTime = 0;
 956                 }
 957                 if (armingDelayTime >= (DSHOT_BEACON_GUARD_DELAY_US / 1e5 - 5)) {
 958                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " BEACON ON"); // Display this message for the first 0.5 seconds
 959                 } else {
 960                     char armingDelayMessage[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
 961                     tfp_sprintf(armingDelayMessage, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
 962                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDelayMessage);
 963                 }
 964                 break;
 965             }
 966 #endif
 967             if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
 968                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "FAIL SAFE");
 969                 SET_BLINK(OSD_WARNINGS);
 970                 break;
 971             }
 972
 973             // Warn when in flip over after crash mode
 974             if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashActive()) {
 975                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP");
 976                 break;
 977             }
 978
 979 #ifdef USE_LAUNCH_CONTROL
 980             // Warn when in launch control mode
 981             if (osdWarnGetState(OSD_WARNING_LAUNCH_CONTROL) && isLaunchControlActive()) {
 982                 if (sensors(SENSOR_ACC)) {
 983                     char launchControlMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
 984                     const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
 985                     tfp_sprintf(launchControlMsg, "LAUNCH %d", pitchAngle);
 986                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, launchControlMsg);
 987                 } else {
 988                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LAUNCH");
 989                 }
 990                 break;
 991             }
 992 #endif
 993
 994             if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
 995                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW");
 996                 SET_BLINK(OSD_WARNINGS);
 997                 break;
 998             }
 999
1000 #ifdef USE_GPS_RESCUE
1001             if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
1002                ARMING_FLAG(ARMED) &&
1003                gpsRescueIsConfigured() &&
1004                !isGPSRescueAvailable()) {
1005                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC");
1006                 SET_BLINK(OSD_WARNINGS);
1007                 break;
1008             }
1009 #endif
1010
1011             // Show warning if in HEADFREE flight mode
1012             if (FLIGHT_MODE(HEADFREE_MODE)) {
1013                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "HEADFREE");
1014                 SET_BLINK(OSD_WARNINGS);
1015                 break;
1016             }
1017
1018 #ifdef USE_ADC_INTERNAL
1019             const int16_t coreTemperature = getCoreTemperatureCelsius();
1020             if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
1021                 char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
1022                 tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
1023
1024                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg);
1025                 SET_BLINK(OSD_WARNINGS);
1026                 break;
1027             }
1028 #endif
1029
1030 #ifdef USE_ESC_SENSOR
1031             // Show warning if we lose motor output, the ESC is overheating or excessive current draw
1032             if (featureIsEnabled(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
1033                 char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
1034                 unsigned pos = 0;
1035
1036                 const char *title = "ESC";
1037
1038                 // center justify message
1039                 while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
1040                     escWarningMsg[pos++] = ' ';
1041                 }
1042
1043                 strcpy(escWarningMsg + pos, title);
1044                 pos += strlen(title);
1045
1046                 unsigned i = 0;
1047                 unsigned escWarningCount = 0;
1048                 while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
1049                     escSensorData_t *escData = getEscSensorData(i);
1050                     const char motorNumber = '1' + i;
1051                     // if everything is OK just display motor number else R, T or C
1052                     char warnFlag = motorNumber;
1053                     if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
1054                         warnFlag = 'R';
1055                     }
1056                     if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
1057                         warnFlag = 'T';
1058                     }
1059                     if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
1060                         warnFlag = 'C';
1061                     }
1062
1063                     escWarningMsg[pos++] = warnFlag;
1064
1065                     if (warnFlag != motorNumber) {
1066                         escWarningCount++;
1067                     }
1068
1069                     i++;
1070                 }
1071
1072                 escWarningMsg[pos] = '\0';
1073
1074                 if (escWarningCount > 0) {
1075                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg);
1076                     SET_BLINK(OSD_WARNINGS);
1077                     break;
1078                 }
1079             }
1080 #endif
1081
1082             if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
1083                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
1084                 SET_BLINK(OSD_WARNINGS);
1085                 break;
1086             }
1087
1088 #ifdef USE_RC_SMOOTHING_FILTER
1089             // Show warning if rc smoothing hasn't initialized the filters
1090             if (osdWarnGetState(OSD_WARNING_RC_SMOOTHING) && ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
1091                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RCSMOOTHING");
1092                 SET_BLINK(OSD_WARNINGS);
1093                 break;
1094             }
1095 #endif
1096
1097             // Show warning if battery is not fresh
1098             if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK)
1099                   && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
1100                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL");
1101                 break;
1102             }
1103
1104             // Visual beeper
1105             if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) {
1106                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "  * * * *");
1107                 break;
1108             }
1109
1110             osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL);
1111             break;
1112         }
1113
1114     case OSD_AVG_CELL_VOLTAGE:
1115         {
1116             const int cellV = osdGetBatteryAverageCellVoltage();
1117             buff[0] = osdGetBatterySymbol(cellV);
1118             tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
1119             break;
1120         }
1121
1122     case OSD_DEBUG:
1123         tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
1124         break;
1125
1126     case OSD_PITCH_ANGLE:
1127     case OSD_ROLL_ANGLE:
1128         {
1129             const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
1130             tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
1131             break;
1132         }
1133
1134     case OSD_MAIN_BATT_USAGE:
1135         {
1136             // Set length of indicator bar
1137             #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
1138
1139             // Calculate constrained value
1140             const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
1141
1142             // Calculate mAh used progress
1143             const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
1144
1145             // Create empty battery indicator bar
1146             buff[0] = SYM_PB_START;
1147             for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
1148                 buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
1149             }
1150             buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
1151             if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
1152                 buff[1 + mAhUsedProgress] = SYM_PB_END;
1153             }
1154             buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
1155             break;
1156         }
1157
1158     case OSD_DISARMED:
1159         if (!ARMING_FLAG(ARMED)) {
1160             tfp_sprintf(buff, "DISARMED");
1161         } else {
1162             if (!lastArmState) {  // previously disarmed - blank out the message one time
1163                 tfp_sprintf(buff, "        ");
1164             }
1165         }
1166         break;
1167
1168     case OSD_NUMERICAL_HEADING:
1169         {
1170             const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
1171             tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
1172             break;
1173         }
1174 #ifdef USE_VARIO
1175     case OSD_NUMERICAL_VARIO:
1176         {
1177             const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
1178             const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH;
1179             tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
1180             break;
1181         }
1182 #endif
1183
1184 #ifdef USE_ESC_SENSOR
1185     case OSD_ESC_TMP:
1186         if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1187             tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
1188         }
1189         break;
1190
1191     case OSD_ESC_RPM:
1192         if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1193             tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm));
1194         }
1195         break;
1196 #endif
1197
1198 #ifdef USE_RTC_TIME
1199     case OSD_RTC_DATETIME:
1200         osdFormatRtcDateTime(&buff[0]);
1201         break;
1202 #endif
1203
1204 #ifdef USE_OSD_ADJUSTMENTS
1205     case OSD_ADJUSTMENT_RANGE:
1206         if (getAdjustmentsRangeName()) {
1207             tfp_sprintf(buff, "%s: %3d", getAdjustmentsRangeName(), getAdjustmentsRangeValue());
1208         }
1209         break;
1210 #endif
1211
1212 #ifdef USE_ADC_INTERNAL
1213     case OSD_CORE_TEMPERATURE:
1214         tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius()), osdGetTemperatureSymbolForSelectedUnit());
1215         break;
1216 #endif
1217
1218 #ifdef USE_BLACKBOX
1219     case OSD_LOG_STATUS:
1220         if (!isBlackboxDeviceWorking()) {
1221             tfp_sprintf(buff, "L-");
1222         } else if (isBlackboxDeviceFull()) {
1223             tfp_sprintf(buff, "L>");
1224         } else {
1225             tfp_sprintf(buff, "L%d", blackboxGetLogNumber());
1226         }
1227         break;
1228 #endif
1229
1230     default:
1231         return false;
1232     }
1233
1234     displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
1235
1236     return true;
1237 }
1238
1239 #ifdef USE_OSD_STICK_OVERLAY
1240 static void osdDrawStickOverlayAxis(uint8_t xpos, uint8_t ypos)
1241 {
1242
1243     for (unsigned x = 0; x < OSD_STICK_OVERLAY_WIDTH; x++) {
1244         for (unsigned  y = 0; y < OSD_STICK_OVERLAY_HEIGHT; y++) {
1245             // draw the axes, vertical and horizonal
1246             if ((x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) && (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
1247                 displayWriteChar(osdDisplayPort, xpos + x, ypos + y, STICK_OVERLAY_CROSS_CHAR);
1248             } else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
1249                 displayWriteChar(osdDisplayPort, xpos + x, ypos + y, STICK_OVERLAY_VERTICAL_CHAR);
1250             } else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
1251                 displayWriteChar(osdDisplayPort, xpos + x, ypos + y, STICK_OVERLAY_HORIZONTAL_CHAR);
1252             }
1253         }
1254     }
1255 }
1256
1257 static void osdDrawStickOverlayAxisItem(osd_items_e osd_item)
1258 {
1259     osdDrawStickOverlayAxis(OSD_X(osdConfig()->item_pos[osd_item]),
1260                             OSD_Y(osdConfig()->item_pos[osd_item]));
1261 }
1262
1263 static void osdDrawStickOverlayPos(osd_items_e osd_item, uint8_t xpos, uint8_t ypos)
1264 {
1265
1266     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[osd_item]);
1267     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[osd_item]);
1268
1269     displayWriteChar(osdDisplayPort, elemPosX + xpos, elemPosY + ypos, STICK_OVERLAY_CURSOR_CHAR);
1270 }
1271
1272 static void osdDrawStickOverlayCursor(osd_items_e osd_item)
1273 {
1274     rc_alias_e vertical_channel, horizontal_channel;
1275
1276     if (osd_item == OSD_STICK_OVERLAY_LEFT) {
1277         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_vertical;
1278         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_horizontal;
1279     } else {
1280         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_vertical;
1281         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_horizontal;
1282     }
1283
1284     uint8_t x_pos =  (uint8_t)scaleRange(constrain(rcData[horizontal_channel], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_WIDTH);
1285     uint8_t y_pos =  (uint8_t)scaleRange(PWM_RANGE_MAX - constrain(rcData[vertical_channel], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_HEIGHT) + OSD_STICK_OVERLAY_HEIGHT - 1;
1286
1287     osdDrawStickOverlayPos(osd_item, x_pos, y_pos);
1288 }
1289 #endif
1290
1291 static void osdDrawElements(void)
1292 {
1293     displayClearScreen(osdDisplayPort);
1294
1295     // Hide OSD when OSDSW mode is active
1296     if (IS_RC_MODE_ACTIVE(BOXOSD)) {
1297         return;
1298     }
1299
1300     osdGForce = 0.0f;
1301     if (sensors(SENSOR_ACC)) {
1302         // only calculate the G force if the element is visible or the stat is enabled
1303         if (VISIBLE(osdConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE)) {
1304             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1305                 const float a = accAverage[axis];
1306                 osdGForce += a * a;
1307             }
1308             osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
1309         }
1310         osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
1311         osdDrawSingleElement(OSD_G_FORCE);
1312     }
1313
1314
1315     for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
1316         osdDrawSingleElement(osdElementDisplayOrder[i]);
1317     }
1318
1319 #ifdef USE_GPS
1320     if (sensors(SENSOR_GPS)) {
1321         osdDrawSingleElement(OSD_GPS_SATS);
1322         osdDrawSingleElement(OSD_GPS_SPEED);
1323         osdDrawSingleElement(OSD_GPS_LAT);
1324         osdDrawSingleElement(OSD_GPS_LON);
1325         osdDrawSingleElement(OSD_HOME_DIST);
1326         osdDrawSingleElement(OSD_HOME_DIR);
1327         osdDrawSingleElement(OSD_FLIGHT_DIST);
1328     }
1329 #endif // GPS
1330
1331 #ifdef USE_ESC_SENSOR
1332     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1333         osdDrawSingleElement(OSD_ESC_TMP);
1334         osdDrawSingleElement(OSD_ESC_RPM);
1335     }
1336 #endif
1337
1338 #ifdef USE_BLACKBOX
1339     if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
1340         osdDrawSingleElement(OSD_LOG_STATUS);
1341     }
1342 #endif
1343
1344 #ifdef USE_OSD_STICK_OVERLAY
1345     if (VISIBLE(osdConfig()->item_pos[OSD_STICK_OVERLAY_LEFT])) {
1346         osdDrawStickOverlayAxisItem(OSD_STICK_OVERLAY_LEFT);
1347         osdDrawStickOverlayCursor(OSD_STICK_OVERLAY_LEFT);
1348     }
1349
1350     if (VISIBLE(osdConfig()->item_pos[OSD_STICK_OVERLAY_RIGHT])) {
1351         osdDrawStickOverlayAxisItem(OSD_STICK_OVERLAY_RIGHT);
1352         osdDrawStickOverlayCursor(OSD_STICK_OVERLAY_RIGHT);
1353     }
1354 #endif
1355 }
1356
1357 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
1358 {
1359     // Position elements near centre of screen and disabled by default
1360     for (int i = 0; i < OSD_ITEM_COUNT; i++) {
1361         osdConfig->item_pos[i] = OSD_POS(10, 7);
1362     }
1363
1364     // Always enable warnings elements by default
1365     osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | OSD_PROFILE_1_FLAG;
1366
1367     // Default to old fixed positions for these elements
1368     osdConfig->item_pos[OSD_CROSSHAIRS]         = OSD_POS(13, 6);
1369     osdConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
1370     osdConfig->item_pos[OSD_HORIZON_SIDEBARS]   = OSD_POS(14, 6);
1371
1372     // Enable the default stats
1373     osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
1374     osdStatSetState(OSD_STAT_MAX_SPEED, true);
1375     osdStatSetState(OSD_STAT_MIN_BATTERY, true);
1376     osdStatSetState(OSD_STAT_MIN_RSSI, true);
1377     osdStatSetState(OSD_STAT_MAX_CURRENT, true);
1378     osdStatSetState(OSD_STAT_USED_MAH, true);
1379     osdStatSetState(OSD_STAT_BLACKBOX, true);
1380     osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, true);
1381     osdStatSetState(OSD_STAT_TIMER_2, true);
1382
1383     osdConfig->units = OSD_UNIT_METRIC;
1384
1385     // Enable all warnings by default
1386     for (int i=0; i < OSD_WARNING_COUNT; i++) {
1387         osdWarnSetState(i, true);
1388     }
1389
1390     osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10);
1391     osdConfig->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10);
1392
1393     osdConfig->overlay_radio_mode = 2;
1394
1395     osdConfig->rssi_alarm = 20;
1396     osdConfig->cap_alarm  = 2200;
1397     osdConfig->alt_alarm  = 100; // meters or feet depend on configuration
1398     osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
1399     osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
1400     osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
1401     osdConfig->core_temp_alarm = 70; // a temperature above 70C should produce a warning, lockups have been reported above 80C
1402
1403     osdConfig->ahMaxPitch = 20; // 20 degrees
1404     osdConfig->ahMaxRoll = 40; // 40 degrees
1405
1406     osdConfig->osdProfileIndex = 1;
1407     osdConfig->ahInvert = false;
1408 }
1409
1410 static void osdDrawLogo(int x, int y)
1411 {
1412     // display logo and help
1413     int fontOffset = 160;
1414     for (int row = 0; row < 4; row++) {
1415         for (int column = 0; column < 24; column++) {
1416             if (fontOffset <= SYM_END_OF_FONT)
1417                 displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
1418         }
1419     }
1420 }
1421
1422 void osdInit(displayPort_t *osdDisplayPortToUse)
1423 {
1424     if (!osdDisplayPortToUse) {
1425         return;
1426     }
1427
1428     STATIC_ASSERT(OSD_POS_MAX == OSD_POS(31,31), OSD_POS_MAX_incorrect);
1429
1430     osdDisplayPort = osdDisplayPortToUse;
1431 #ifdef USE_CMS
1432     cmsDisplayPortRegister(osdDisplayPort);
1433 #endif
1434
1435     armState = ARMING_FLAG(ARMED);
1436
1437     memset(blinkBits, 0, sizeof(blinkBits));
1438
1439     displayClearScreen(osdDisplayPort);
1440
1441     osdDrawLogo(3, 1);
1442
1443     char string_buffer[30];
1444     tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
1445     displayWrite(osdDisplayPort, 20, 6, string_buffer);
1446 #ifdef USE_CMS
1447     displayWrite(osdDisplayPort, 7, 8,  CMS_STARTUP_HELP_TEXT1);
1448     displayWrite(osdDisplayPort, 11, 9, CMS_STARTUP_HELP_TEXT2);
1449     displayWrite(osdDisplayPort, 11, 10, CMS_STARTUP_HELP_TEXT3);
1450 #endif
1451
1452 #ifdef USE_RTC_TIME
1453     char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
1454     if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
1455         displayWrite(osdDisplayPort, 5, 12, dateTimeBuffer);
1456     }
1457 #endif
1458
1459     displayResync(osdDisplayPort);
1460
1461     resumeRefreshAt = micros() + (4 * REFRESH_1S);
1462 #ifdef USE_OSD_PROFILES
1463     changeOsdProfileIndex(osdConfig()->osdProfileIndex);
1464 #endif
1465 }
1466
1467 bool osdInitialized(void)
1468 {
1469     return osdDisplayPort;
1470 }
1471
1472 void osdUpdateAlarms(void)
1473 {
1474     // This is overdone?
1475
1476     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
1477
1478     if (getRssiPercent() < osdConfig()->rssi_alarm) {
1479         SET_BLINK(OSD_RSSI_VALUE);
1480     } else {
1481         CLR_BLINK(OSD_RSSI_VALUE);
1482     }
1483
1484     if (getBatteryState() == BATTERY_OK) {
1485         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1486         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1487     } else {
1488         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
1489         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
1490     }
1491
1492 #ifdef USE_GPS
1493     if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5) || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())) {
1494         SET_BLINK(OSD_GPS_SATS);
1495     } else {
1496         CLR_BLINK(OSD_GPS_SATS);
1497     }
1498 #endif //USE_GPS
1499
1500     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1501         const uint16_t timer = osdConfig()->timers[i];
1502         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
1503         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
1504         if (alarmTime != 0 && time >= alarmTime) {
1505             SET_BLINK(OSD_ITEM_TIMER_1 + i);
1506         } else {
1507             CLR_BLINK(OSD_ITEM_TIMER_1 + i);
1508         }
1509     }
1510
1511     if (getMAhDrawn() >= osdConfig()->cap_alarm) {
1512         SET_BLINK(OSD_MAH_DRAWN);
1513         SET_BLINK(OSD_MAIN_BATT_USAGE);
1514         SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1515     } else {
1516         CLR_BLINK(OSD_MAH_DRAWN);
1517         CLR_BLINK(OSD_MAIN_BATT_USAGE);
1518         CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1519     }
1520
1521     if (alt >= osdConfig()->alt_alarm) {
1522         SET_BLINK(OSD_ALTITUDE);
1523     } else {
1524         CLR_BLINK(OSD_ALTITUDE);
1525     }
1526
1527 #ifdef USE_ESC_SENSOR
1528     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1529         // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
1530         if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
1531             SET_BLINK(OSD_ESC_TMP);
1532         } else {
1533             CLR_BLINK(OSD_ESC_TMP);
1534         }
1535     }
1536 #endif
1537 }
1538
1539 void osdResetAlarms(void)
1540 {
1541     CLR_BLINK(OSD_RSSI_VALUE);
1542     CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1543     CLR_BLINK(OSD_WARNINGS);
1544     CLR_BLINK(OSD_GPS_SATS);
1545     CLR_BLINK(OSD_MAH_DRAWN);
1546     CLR_BLINK(OSD_ALTITUDE);
1547     CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1548     CLR_BLINK(OSD_MAIN_BATT_USAGE);
1549     CLR_BLINK(OSD_ITEM_TIMER_1);
1550     CLR_BLINK(OSD_ITEM_TIMER_2);
1551     CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1552     CLR_BLINK(OSD_ESC_TMP);
1553 }
1554
1555 static void osdResetStats(void)
1556 {
1557     stats.max_current  = 0;
1558     stats.max_speed    = 0;
1559     stats.min_voltage  = 500;
1560     stats.min_rssi     = 99; // percent
1561     stats.max_altitude = 0;
1562     stats.max_distance = 0;
1563     stats.armed_time   = 0;
1564     stats.max_g_force  = 0;
1565     stats.max_esc_temp = 0;
1566     stats.max_esc_rpm  = 0;
1567     stats.min_link_quality = 99; // percent
1568 }
1569
1570 static void osdUpdateStats(void)
1571 {
1572     int16_t value = 0;
1573 #ifdef USE_GPS
1574     switch (osdConfig()->units) {
1575     case OSD_UNIT_IMPERIAL:
1576         value = CM_S_TO_MPH(gpsSol.groundSpeed);
1577         break;
1578     default:
1579         value = CM_S_TO_KM_H(gpsSol.groundSpeed);
1580         break;
1581     }
1582 #endif
1583     if (stats.max_speed < value) {
1584         stats.max_speed = value;
1585     }
1586
1587     value = getBatteryVoltage();
1588     if (stats.min_voltage > value) {
1589         stats.min_voltage = value;
1590     }
1591
1592     value = getAmperage() / 100;
1593     if (stats.max_current < value) {
1594         stats.max_current = value;
1595     }
1596
1597     value = getRssiPercent();
1598     if (stats.min_rssi > value) {
1599         stats.min_rssi = value;
1600     }
1601
1602     int32_t altitudeCm = getEstimatedAltitudeCm();
1603     if (stats.max_altitude < altitudeCm) {
1604         stats.max_altitude = altitudeCm;
1605     }
1606
1607     if (stats.max_g_force < osdGForce) {
1608         stats.max_g_force = osdGForce;
1609     }
1610
1611 #ifdef USE_RX_LINK_QUALITY_INFO
1612     value = rxGetLinkQualityPercent();
1613     if (stats.min_link_quality > value) {
1614         stats.min_link_quality = value;
1615     }
1616 #endif
1617
1618 #ifdef USE_GPS
1619     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1620         value = GPS_distanceToHome;
1621
1622         if (stats.max_distance < GPS_distanceToHome) {
1623             stats.max_distance = GPS_distanceToHome;
1624         }
1625     }
1626 #endif
1627 #ifdef USE_ESC_SENSOR
1628     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1629         value = escDataCombined->temperature;
1630         if (stats.max_esc_temp < value) {
1631             stats.max_esc_temp = value;
1632         }
1633         value = calcEscRpm(escDataCombined->rpm);
1634         if (stats.max_esc_rpm < value) {
1635             stats.max_esc_rpm = value;
1636         }
1637     }
1638 #endif
1639 }
1640
1641 #ifdef USE_BLACKBOX
1642
1643 static void osdGetBlackboxStatusString(char * buff)
1644 {
1645     bool storageDeviceIsWorking = isBlackboxDeviceWorking();
1646     uint32_t storageUsed = 0;
1647     uint32_t storageTotal = 0;
1648
1649     switch (blackboxConfig()->device) {
1650 #ifdef USE_SDCARD
1651     case BLACKBOX_DEVICE_SDCARD:
1652         if (storageDeviceIsWorking) {
1653             storageTotal = sdcard_getMetadata()->numBlocks / 2000;
1654             storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
1655         }
1656         break;
1657 #endif
1658
1659 #ifdef USE_FLASHFS
1660     case BLACKBOX_DEVICE_FLASH:
1661         if (storageDeviceIsWorking) {
1662             const flashGeometry_t *geometry = flashfsGetGeometry();
1663             storageTotal = geometry->totalSize / 1024;
1664             storageUsed = flashfsGetOffset() / 1024;
1665         }
1666         break;
1667 #endif
1668
1669     default:
1670         break;
1671     }
1672
1673     if (storageDeviceIsWorking) {
1674         const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
1675         tfp_sprintf(buff, "%d%%", storageUsedPercent);
1676     } else {
1677         tfp_sprintf(buff, "FAULT");
1678     }
1679 }
1680 #endif
1681
1682 static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
1683 {
1684     displayWrite(osdDisplayPort, 2, y, text);
1685     displayWrite(osdDisplayPort, 20, y, ":");
1686     displayWrite(osdDisplayPort, 22, y, value);
1687 }
1688
1689 /*
1690  * Test if there's some stat enabled
1691  */
1692 static bool isSomeStatEnabled(void)
1693 {
1694     return (osdConfig()->enabled_stats != 0);
1695 }
1696
1697 // *** IMPORTANT ***
1698 // The order of the OSD stats as displayed on-screen must match the osd_stats_e enumeration.
1699 // This is because the fields are presented in the configurator in the order of the enumeration
1700 // and we want the configuration order to match the on-screen display order.  If you change the
1701 // display order you *must* update the osd_stats_e enumeration to match. Additionally the
1702 // changes to the stats display order *must* be implemented in the configurator otherwise the
1703 // stats selections will not be populated correctly and the settings will become corrupted.
1704
1705 static void osdShowStats(uint16_t endBatteryVoltage)
1706 {
1707     uint8_t top = 2;
1708     char buff[OSD_ELEMENT_BUFFER_LENGTH];
1709
1710     displayClearScreen(osdDisplayPort);
1711     displayWrite(osdDisplayPort, 2, top++, "  --- STATS ---");
1712
1713     if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
1714         bool success = false;
1715 #ifdef USE_RTC_TIME
1716         success = osdFormatRtcDateTime(&buff[0]);
1717 #endif
1718         if (!success) {
1719             tfp_sprintf(buff, "NO RTC");
1720         }
1721
1722         displayWrite(osdDisplayPort, 2, top++, buff);
1723     }
1724
1725     if (osdStatGetState(OSD_STAT_TIMER_1)) {
1726         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
1727         osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
1728     }
1729
1730     if (osdStatGetState(OSD_STAT_TIMER_2)) {
1731         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
1732         osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
1733     }
1734
1735 #ifdef USE_GPS
1736     if (osdStatGetState(OSD_STAT_MAX_SPEED) && featureIsEnabled(FEATURE_GPS)) {
1737         itoa(stats.max_speed, buff, 10);
1738         osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
1739     }
1740
1741     if (osdStatGetState(OSD_STAT_MAX_DISTANCE) && featureIsEnabled(FEATURE_GPS)) {
1742         tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
1743         osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
1744     }
1745 #endif
1746
1747     if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
1748         tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT);
1749         osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
1750     }
1751
1752     if (osdStatGetState(OSD_STAT_END_BATTERY)) {
1753         tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT);
1754         osdDisplayStatisticLabel(top++, "END BATTERY", buff);
1755     }
1756
1757     if (osdStatGetState(OSD_STAT_BATTERY)) {
1758         tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
1759         osdDisplayStatisticLabel(top++, "BATTERY", buff);
1760     }
1761
1762     if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
1763         itoa(stats.min_rssi, buff, 10);
1764         strcat(buff, "%");
1765         osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
1766     }
1767
1768     if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
1769         if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
1770             itoa(stats.max_current, buff, 10);
1771             strcat(buff, "A");
1772             osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
1773         }
1774
1775         if (osdStatGetState(OSD_STAT_USED_MAH)) {
1776             tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
1777             osdDisplayStatisticLabel(top++, "USED MAH", buff);
1778         }
1779     }
1780
1781     if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
1782         osdFormatAltitudeString(buff, stats.max_altitude);
1783         osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
1784     }
1785
1786 #ifdef USE_BLACKBOX
1787     if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
1788         osdGetBlackboxStatusString(buff);
1789         osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
1790     }
1791
1792     if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
1793         itoa(blackboxGetLogNumber(), buff, 10);
1794         osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
1795     }
1796 #endif
1797
1798     if (osdStatGetState(OSD_STAT_MAX_G_FORCE) && sensors(SENSOR_ACC)) {
1799         const int gForce = lrintf(stats.max_g_force * 10);
1800         tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10);
1801         osdDisplayStatisticLabel(top++, "MAX G-FORCE", buff);
1802     }
1803
1804 #ifdef USE_ESC_SENSOR
1805     if (osdStatGetState(OSD_STAT_MAX_ESC_TEMP)) {
1806         tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
1807         osdDisplayStatisticLabel(top++, "MAX ESC TEMP", buff);
1808     }
1809
1810     if (osdStatGetState(OSD_STAT_MAX_ESC_RPM)) {
1811         itoa(stats.max_esc_rpm, buff, 10);
1812         osdDisplayStatisticLabel(top++, "MAX ESC RPM", buff);
1813     }
1814 #endif
1815
1816 #ifdef USE_RX_LINK_QUALITY_INFO
1817     if (osdStatGetState(OSD_STAT_MIN_LINK_QUALITY)) {
1818         itoa(stats.min_link_quality, buff, 10);
1819         strcat(buff, "%");
1820         osdDisplayStatisticLabel(top++, "MIN LINK", buff);
1821     }
1822 #endif
1823
1824 #ifdef USE_GPS
1825     if (osdStatGetState(OSD_STAT_FLIGHT_DISTANCE) && featureIsEnabled(FEATURE_GPS)) {
1826         const uint32_t distanceFlown = GPS_distanceFlownInCm / 100;
1827         tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(distanceFlown), osdGetMetersToSelectedUnitSymbol());
1828         osdDisplayStatisticLabel(top++, "FLIGHT DISTANCE", buff);
1829     }
1830 #endif
1831
1832 }
1833
1834 static void osdShowArmed(void)
1835 {
1836     displayClearScreen(osdDisplayPort);
1837     displayWrite(osdDisplayPort, 12, 7, "ARMED");
1838 }
1839
1840 STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
1841 {
1842     static timeUs_t lastTimeUs = 0;
1843     static bool osdStatsEnabled = false;
1844     static bool osdStatsVisible = false;
1845     static timeUs_t osdStatsRefreshTimeUs;
1846     static uint16_t endBatteryVoltage;
1847
1848     // detect arm/disarm
1849     if (armState != ARMING_FLAG(ARMED)) {
1850         if (ARMING_FLAG(ARMED)) {
1851             osdStatsEnabled = false;
1852             osdStatsVisible = false;
1853             osdResetStats();
1854             osdShowArmed();
1855             resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
1856         } else if (isSomeStatEnabled()
1857                    && !suppressStatsDisplay
1858                    && (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
1859                        || !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
1860             osdStatsEnabled = true;
1861             resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
1862             endBatteryVoltage = getBatteryVoltage();
1863         }
1864
1865         armState = ARMING_FLAG(ARMED);
1866     }
1867
1868
1869     if (ARMING_FLAG(ARMED)) {
1870         osdUpdateStats();
1871         timeUs_t deltaT = currentTimeUs - lastTimeUs;
1872         flyTime += deltaT;
1873         stats.armed_time += deltaT;
1874     } else if (osdStatsEnabled) {  // handle showing/hiding stats based on OSD disable switch position
1875         if (displayIsGrabbed(osdDisplayPort)) {
1876             osdStatsEnabled = false;
1877             resumeRefreshAt = 0;
1878             stats.armed_time = 0;
1879         } else {
1880             if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
1881                 osdStatsVisible = false;
1882                 displayClearScreen(osdDisplayPort);
1883             } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
1884                 if (!osdStatsVisible) {
1885                     osdStatsVisible = true;
1886                     osdStatsRefreshTimeUs = 0;
1887                 }
1888                 if (currentTimeUs >= osdStatsRefreshTimeUs) {
1889                     osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
1890                     osdShowStats(endBatteryVoltage);
1891                 }
1892             }
1893         }
1894     }
1895     lastTimeUs = currentTimeUs;
1896
1897     if (resumeRefreshAt) {
1898         if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
1899             // in timeout period, check sticks for activity to resume display.
1900             if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
1901                 resumeRefreshAt = currentTimeUs;
1902             }
1903             displayHeartbeat(osdDisplayPort);
1904             return;
1905         } else {
1906             displayClearScreen(osdDisplayPort);
1907             resumeRefreshAt = 0;
1908             osdStatsEnabled = false;
1909             stats.armed_time = 0;
1910         }
1911     }
1912
1913     blinkState = (currentTimeUs / 200000) % 2;
1914
1915 #ifdef USE_ESC_SENSOR
1916     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1917         escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
1918     }
1919 #endif
1920
1921 #ifdef USE_CMS
1922     if (!displayIsGrabbed(osdDisplayPort)) {
1923         osdUpdateAlarms();
1924         osdDrawElements();
1925         displayHeartbeat(osdDisplayPort);
1926 #ifdef OSD_CALLS_CMS
1927     } else {
1928         cmsUpdate(currentTimeUs);
1929 #endif
1930     }
1931 #endif
1932     lastArmState = ARMING_FLAG(ARMED);
1933 }
1934
1935 /*
1936  * Called periodically by the scheduler
1937  */
1938 void osdUpdate(timeUs_t currentTimeUs)
1939 {
1940     static uint32_t counter = 0;
1941
1942     if (isBeeperOn()) {
1943         showVisualBeeper = true;
1944     }
1945
1946 #ifdef MAX7456_DMA_CHANNEL_TX
1947     // don't touch buffers if DMA transaction is in progress
1948     if (displayIsTransferInProgress(osdDisplayPort)) {
1949         return;
1950     }
1951 #endif // MAX7456_DMA_CHANNEL_TX
1952
1953 #ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
1954     static uint32_t idlecounter = 0;
1955     if (!ARMING_FLAG(ARMED)) {
1956         if (idlecounter++ % 4 != 0) {
1957             return;
1958         }
1959     }
1960 #endif
1961
1962     // redraw values in buffer
1963 #ifdef USE_MAX7456
1964 #define DRAW_FREQ_DENOM 5
1965 #else
1966 #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud (
1967 #endif
1968 #define STATS_FREQ_DENOM    50
1969
1970     if (counter % DRAW_FREQ_DENOM == 0) {
1971         osdRefresh(currentTimeUs);
1972         showVisualBeeper = false;
1973     } else {
1974         // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
1975         displayDrawScreen(osdDisplayPort);
1976     }
1977     ++counter;
1978
1979 #ifdef USE_CMS
1980     // do not allow ARM if we are in menu
1981     if (displayIsGrabbed(osdDisplayPort)) {
1982         setArmingDisabled(ARMING_DISABLED_OSD_MENU);
1983     } else {
1984         unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
1985     }
1986 #endif
1987 }
1988
1989 void osdSuppressStats(bool flag)
1990 {
1991     suppressStatsDisplay = flag;
1992 }
1993
1994 #ifdef USE_OSD_PROFILES
1995 bool osdElementVisible(uint16_t value)
1996 {
1997     return (bool)((((value & OSD_PROFILE_MASK) >> OSD_PROFILE_BITS_POS) & osdProfile) != 0);
1998 }
1999 #endif
2000 #endif // USE_OSD