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/maths.h"
  50 #include "common/printf.h"
  51 #include "common/typeconversion.h"
  52 #include "common/utils.h"
  53
  54 #include "config/feature.h"
  55
  56 #include "drivers/display.h"
  57 #include "drivers/flash.h"
  58 #include "drivers/max7456_symbols.h"
  59 #include "drivers/sdcard.h"
  60 #include "drivers/time.h"
  61
  62 #include "fc/config.h"
  63 #include "fc/fc_core.h"
  64 #include "fc/rc_adjustments.h"
  65 #include "fc/rc_controls.h"
  66 #include "fc/runtime_config.h"
  67
  68 #include "flight/position.h"
  69 #include "flight/imu.h"
  70 #ifdef USE_ESC_SENSOR
  71 #include "flight/mixer.h"
  72 #endif
  73 #include "flight/pid.h"
  74
  75 #include "io/asyncfatfs/asyncfatfs.h"
  76 #include "io/beeper.h"
  77 #include "io/flashfs.h"
  78 #include "io/gps.h"
  79 #include "io/osd.h"
  80 #include "io/vtx_string.h"
  81 #include "io/vtx.h"
  82
  83 #include "pg/pg.h"
  84 #include "pg/pg_ids.h"
  85 #include "pg/rx.h"
  86
  87 #include "rx/rx.h"
  88
  89 #include "sensors/adcinternal.h"
  90 #include "sensors/barometer.h"
  91 #include "sensors/battery.h"
  92 #include "sensors/esc_sensor.h"
  93 #include "sensors/sensors.h"
  94
  95 #ifdef USE_HARDWARE_REVISION_DETECTION
  96 #include "hardware_revision.h"
  97 #endif
  98
  99 #define VIDEO_BUFFER_CHARS_PAL    480
 100 #define FULL_CIRCLE 360
 101
 102 const char * const osdTimerSourceNames[] = {
 103     "ON TIME  ",
 104     "TOTAL ARM",
 105     "LAST ARM "
 106 };
 107
 108 // Blink control
 109
 110 static bool blinkState = true;
 111 static bool showVisualBeeper = false;
 112
 113 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
 114 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
 115 #define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
 116 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 117 #define BLINK(item) (IS_BLINK(item) && blinkState)
 118
 119 // Things in both OSD and CMS
 120
 121 #define IS_HI(X)  (rcData[X] > 1750)
 122 #define IS_LO(X)  (rcData[X] < 1250)
 123 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
 124
 125 static timeUs_t flyTime = 0;
 126
 127 typedef struct statistic_s {
 128     timeUs_t armed_time;
 129     int16_t max_speed;
 130     int16_t min_voltage; // /10
 131     int16_t max_current; // /10
 132     int16_t min_rssi;
 133     int32_t max_altitude;
 134     int16_t max_distance;
 135 } statistic_t;
 136
 137 static statistic_t stats;
 138
 139 timeUs_t resumeRefreshAt = 0;
 140 #define REFRESH_1S    1000 * 1000
 141
 142 static uint8_t armState;
 143 static bool lastArmState;
 144
 145 static displayPort_t *osdDisplayPort;
 146
 147 #ifdef USE_ESC_SENSOR
 148 static escSensorData_t *escDataCombined;
 149 #endif
 150
 151 #define AH_SYMBOL_COUNT 9
 152 #define AH_SIDEBAR_WIDTH_POS 7
 153 #define AH_SIDEBAR_HEIGHT_POS 3
 154
 155 static const char compassBar[] = {
 156   SYM_HEADING_W,
 157   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 158   SYM_HEADING_N,
 159   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 160   SYM_HEADING_E,
 161   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 162   SYM_HEADING_S,
 163   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 164   SYM_HEADING_W,
 165   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 166   SYM_HEADING_N,
 167   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE
 168 };
 169
 170 static const uint8_t osdElementDisplayOrder[] = {
 171     OSD_MAIN_BATT_VOLTAGE,
 172     OSD_RSSI_VALUE,
 173     OSD_CROSSHAIRS,
 174     OSD_HORIZON_SIDEBARS,
 175     OSD_ITEM_TIMER_1,
 176     OSD_ITEM_TIMER_2,
 177     OSD_REMAINING_TIME_ESTIMATE,
 178     OSD_FLYMODE,
 179     OSD_THROTTLE_POS,
 180     OSD_VTX_CHANNEL,
 181     OSD_CURRENT_DRAW,
 182     OSD_MAH_DRAWN,
 183     OSD_CRAFT_NAME,
 184     OSD_ALTITUDE,
 185     OSD_ROLL_PIDS,
 186     OSD_PITCH_PIDS,
 187     OSD_YAW_PIDS,
 188     OSD_POWER,
 189     OSD_PIDRATE_PROFILE,
 190     OSD_WARNINGS,
 191     OSD_AVG_CELL_VOLTAGE,
 192     OSD_DEBUG,
 193     OSD_PITCH_ANGLE,
 194     OSD_ROLL_ANGLE,
 195     OSD_MAIN_BATT_USAGE,
 196     OSD_DISARMED,
 197     OSD_NUMERICAL_HEADING,
 198     OSD_NUMERICAL_VARIO,
 199     OSD_COMPASS_BAR,
 200     OSD_ANTI_GRAVITY
 201 };
 202
 203 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3);
 204
 205 /**
 206  * Gets the correct altitude symbol for the current unit system
 207  */
 208 static char osdGetMetersToSelectedUnitSymbol(void)
 209 {
 210     switch (osdConfig()->units) {
 211     case OSD_UNIT_IMPERIAL:
 212         return SYM_FT;
 213     default:
 214         return SYM_M;
 215     }
 216 }
 217
 218 /**
 219  * Gets average battery cell voltage in 0.01V units.
 220  */
 221 static int osdGetBatteryAverageCellVoltage(void)
 222 {
 223     return (getBatteryVoltage() * 10) / getBatteryCellCount();
 224 }
 225
 226 static char osdGetBatterySymbol(int cellVoltage)
 227 {
 228     if (getBatteryState() == BATTERY_CRITICAL) {
 229         return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
 230     } else {
 231         // Calculate a symbol offset using cell voltage over full cell voltage range
 232         const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7);
 233         return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
 234     }
 235 }
 236
 237 /**
 238  * Converts altitude based on the current unit system.
 239  * @param meters Value in meters to convert
 240  */
 241 static int32_t osdGetMetersToSelectedUnit(int32_t meters)
 242 {
 243     switch (osdConfig()->units) {
 244     case OSD_UNIT_IMPERIAL:
 245         return (meters * 328) / 100; // Convert to feet / 100
 246     default:
 247         return meters;               // Already in metre / 100
 248     }
 249 }
 250
 251 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 252 STATIC_UNIT_TESTED int osdConvertTemperatureToSelectedUnit(int tempInDeciDegrees)
 253 {
 254     switch (osdConfig()->units) {
 255     case OSD_UNIT_IMPERIAL:
 256         return ((tempInDeciDegrees * 9) / 5) + 320;
 257     default:
 258         return tempInDeciDegrees;
 259     }
 260 }
 261
 262 static char osdGetTemperatureSymbolForSelectedUnit(void)
 263 {
 264     switch (osdConfig()->units) {
 265     case OSD_UNIT_IMPERIAL:
 266         return 'F';
 267     default:
 268         return 'C';
 269     }
 270 }
 271 #endif
 272
 273 static void osdFormatAltitudeString(char * buff, int altitude)
 274 {
 275     const int alt = osdGetMetersToSelectedUnit(altitude) / 10;
 276
 277     tfp_sprintf(buff, "%5d %c", alt, osdGetMetersToSelectedUnitSymbol());
 278     buff[5] = buff[4];
 279     buff[4] = '.';
 280 }
 281
 282 static void osdFormatPID(char * buff, const char * label, const pid8_t * pid)
 283 {
 284     tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
 285 }
 286
 287 static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
 288 {
 289     heading += FULL_CIRCLE;  // Ensure positive value
 290
 291     // Split input heading 0..359 into sectors 0..(directions-1), but offset
 292     // by half a sector so that sector 0 gets centered around heading 0.
 293     // We multiply heading by directions to not loose precision in divisions
 294     // In this way each segment will be a FULL_CIRCLE length
 295     int direction = (heading * directions + FULL_CIRCLE / 2) /  FULL_CIRCLE; // scale with rounding
 296     direction %= directions; // normalize
 297
 298     return direction; // return segment number
 299 }
 300
 301 static uint8_t osdGetDirectionSymbolFromHeading(int heading)
 302 {
 303     heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
 304
 305     // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
 306     // Our symbols are Down=0, Right=4, Up=8 and Left=12
 307     // There're 16 arrow symbols. Transform it.
 308     heading = 16 - heading;
 309     heading = (heading + 8) % 16;
 310
 311     return SYM_ARROW_SOUTH + heading;
 312 }
 313
 314 static char osdGetTimerSymbol(osd_timer_source_e src)
 315 {
 316     switch (src) {
 317     case OSD_TIMER_SRC_ON:
 318         return SYM_ON_M;
 319     case OSD_TIMER_SRC_TOTAL_ARMED:
 320     case OSD_TIMER_SRC_LAST_ARMED:
 321         return SYM_FLY_M;
 322     default:
 323         return ' ';
 324     }
 325 }
 326
 327 static timeUs_t osdGetTimerValue(osd_timer_source_e src)
 328 {
 329     switch (src) {
 330     case OSD_TIMER_SRC_ON:
 331         return micros();
 332     case OSD_TIMER_SRC_TOTAL_ARMED:
 333         return flyTime;
 334     case OSD_TIMER_SRC_LAST_ARMED:
 335         return stats.armed_time;
 336     default:
 337         return 0;
 338     }
 339 }
 340
 341 STATIC_UNIT_TESTED void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
 342 {
 343     int seconds = time / 1000000;
 344     const int minutes = seconds / 60;
 345     seconds = seconds % 60;
 346
 347     switch (precision) {
 348     case OSD_TIMER_PREC_SECOND:
 349     default:
 350         tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
 351         break;
 352     case OSD_TIMER_PREC_HUNDREDTHS:
 353         {
 354             const int hundredths = (time / 10000) % 100;
 355             tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
 356             break;
 357         }
 358     }
 359 }
 360
 361 STATIC_UNIT_TESTED void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
 362 {
 363     const uint16_t timer = osdConfig()->timers[timerIndex];
 364     const uint8_t src = OSD_TIMER_SRC(timer);
 365
 366     if (showSymbol) {
 367         *(buff++) = osdGetTimerSymbol(src);
 368     }
 369
 370     osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
 371 }
 372
 373 #ifdef USE_GPS
 374 static void osdFormatCoordinate(char *buff, char sym, int32_t val)
 375 {
 376     // latitude maximum integer width is 3 (-90).
 377     // longitude maximum integer width is 4 (-180).
 378     // We show 7 decimals, so we need to use 12 characters:
 379     // eg: s-180.1234567z   s=symbol, z=zero terminator, decimal separator  between 0 and 1
 380
 381     static const int coordinateMaxLength = 13;//12 for the number (4 + dot + 7) + 1 for the symbol
 382
 383     buff[0] = sym;
 384     const int32_t integerPart = val / GPS_DEGREES_DIVIDER;
 385     const int32_t decimalPart = labs(val % GPS_DEGREES_DIVIDER);
 386     const int written = tfp_sprintf(buff + 1, "%d.%07d", integerPart, decimalPart);
 387     // pad with blanks to coordinateMaxLength
 388     for (int pos = 1 + written; pos < coordinateMaxLength; ++pos) {
 389         buff[pos] = SYM_BLANK;
 390     }
 391     buff[coordinateMaxLength] = '\0';
 392 }
 393 #endif // USE_GPS
 394
 395 #ifdef USE_RTC_TIME
 396 static bool osdFormatRtcDateTime(char *buffer)
 397 {
 398     dateTime_t dateTime;
 399     if (!rtcGetDateTime(&dateTime)) {
 400         buffer[0] = '\0';
 401
 402         return false;
 403     }
 404
 405     dateTimeFormatLocalShort(buffer, &dateTime);
 406
 407     return true;
 408 }
 409 #endif
 410
 411 static void osdFormatMessage(char *buff, size_t size, const char *message)
 412 {
 413     memset(buff, SYM_BLANK, size);
 414     if (message) {
 415         memcpy(buff, message, strlen(message));
 416     }
 417     // Ensure buff is zero terminated
 418     buff[size - 1] = '\0';
 419 }
 420
 421 void osdStatSetState(uint8_t statIndex, bool enabled)
 422 {
 423     if (enabled) {
 424         osdConfigMutable()->enabled_stats |= (1 << statIndex);
 425     } else {
 426         osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
 427     }
 428 }
 429
 430 bool osdStatGetState(uint8_t statIndex)
 431 {
 432     return osdConfig()->enabled_stats & (1 << statIndex);
 433 }
 434
 435 void osdWarnSetState(uint8_t warningIndex, bool enabled)
 436 {
 437     if (enabled) {
 438         osdConfigMutable()->enabledWarnings |= (1 << warningIndex);
 439     } else {
 440         osdConfigMutable()->enabledWarnings &= ~(1 << warningIndex);
 441     }
 442 }
 443
 444 bool osdWarnGetState(uint8_t warningIndex)
 445 {
 446     return osdConfig()->enabledWarnings & (1 << warningIndex);
 447 }
 448
 449 static bool osdDrawSingleElement(uint8_t item)
 450 {
 451     if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
 452         return false;
 453     }
 454
 455     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
 456     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
 457     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
 458
 459     switch (item) {
 460     case OSD_RSSI_VALUE:
 461         {
 462             uint16_t osdRssi = getRssi() * 100 / 1024; // change range
 463             if (osdRssi >= 100)
 464                 osdRssi = 99;
 465
 466             tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi);
 467             break;
 468         }
 469
 470     case OSD_MAIN_BATT_VOLTAGE:
 471         buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage());
 472         tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
 473         break;
 474
 475     case OSD_CURRENT_DRAW:
 476         {
 477             const int32_t amperage = getAmperage();
 478             tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
 479             break;
 480         }
 481
 482     case OSD_MAH_DRAWN:
 483         tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH);
 484         break;
 485
 486 #ifdef USE_GPS
 487     case OSD_GPS_SATS:
 488         tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
 489         break;
 490
 491     case OSD_GPS_SPEED:
 492         // 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)
 493         switch (osdConfig()->units) {
 494         case OSD_UNIT_IMPERIAL:
 495             tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed));
 496             break;
 497         default:
 498             tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
 499             break;
 500         }
 501         break;
 502
 503     case OSD_GPS_LAT:
 504         // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH
 505         osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat);
 506         break;
 507
 508     case OSD_GPS_LON:
 509         // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST
 510         osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon);
 511         break;
 512
 513     case OSD_HOME_DIR:
 514         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 515             if (GPS_distanceToHome > 0) {
 516                 const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 517                 buff[0] = osdGetDirectionSymbolFromHeading(h);
 518             } else {
 519                 // We don't have a HOME symbol in the font, by now we use this
 520                 buff[0] = SYM_THR1;
 521             }
 522
 523         } else {
 524             // We use this symbol when we don't have a FIX
 525             buff[0] = SYM_COLON;
 526         }
 527
 528         buff[1] = 0;
 529
 530         break;
 531
 532     case OSD_HOME_DIST:
 533         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 534             const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome);
 535             tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
 536         } else {
 537             // We use this symbol when we don't have a FIX
 538             buff[0] = SYM_COLON;
 539             // overwrite any previous distance with blanks
 540             memset(buff + 1, SYM_BLANK, 6);
 541             buff[7] = '\0';
 542         }
 543         break;
 544
 545 #endif // GPS
 546
 547     case OSD_COMPASS_BAR:
 548         memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
 549         buff[9] = 0;
 550         break;
 551
 552     case OSD_ALTITUDE:
 553         osdFormatAltitudeString(buff, getEstimatedAltitude());
 554         break;
 555
 556     case OSD_ITEM_TIMER_1:
 557     case OSD_ITEM_TIMER_2:
 558         osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1);
 559         break;
 560
 561     case OSD_REMAINING_TIME_ESTIMATE:
 562         {
 563             const int mAhDrawn = getMAhDrawn();
 564             const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn);
 565
 566             if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) {
 567                 tfp_sprintf(buff, "--:--");
 568             } else if (mAhDrawn > osdConfig()->cap_alarm) {
 569                 tfp_sprintf(buff, "00:00");
 570             } else {
 571                 osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time);
 572             }
 573             break;
 574         }
 575
 576     case OSD_FLYMODE:
 577         {
 578             if (FLIGHT_MODE(FAILSAFE_MODE)) {
 579                 strcpy(buff, "!FS!");
 580             } else if (FLIGHT_MODE(ANGLE_MODE)) {
 581                 strcpy(buff, "STAB");
 582             } else if (FLIGHT_MODE(HORIZON_MODE)) {
 583                 strcpy(buff, "HOR ");
 584             } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 585                 strcpy(buff, "RESC");
 586             } else if (isAirmodeActive()) {
 587                 strcpy(buff, "AIR ");
 588             } else {
 589                 strcpy(buff, "ACRO");
 590             }
 591
 592             break;
 593         }
 594
 595     case OSD_ANTI_GRAVITY:
 596         {
 597             if (pidItermAccelerator() > 1.0f) {
 598                 strcpy(buff, "AG");
 599             }
 600
 601             break;
 602         }
 603
 604     case OSD_CRAFT_NAME:
 605         // 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.
 606         //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered.
 607
 608         if (strlen(pilotConfig()->name) == 0) {
 609             strcpy(buff, "CRAFT_NAME");
 610         } else {
 611             unsigned i;
 612             for (i = 0; i < MAX_NAME_LENGTH; i++) {
 613                 if (pilotConfig()->name[i]) {
 614                     buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
 615                 } else {
 616                     break;
 617                 }
 618             }
 619             buff[i] = '\0';
 620         }
 621
 622         break;
 623
 624     case OSD_THROTTLE_POS:
 625         buff[0] = SYM_THR;
 626         buff[1] = SYM_THR1;
 627         tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
 628         break;
 629
 630 #if defined(USE_VTX_COMMON)
 631     case OSD_VTX_CHANNEL:
 632         {
 633             const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band];
 634             const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel];
 635             uint8_t vtxPower = vtxSettingsConfig()->power;
 636             const vtxDevice_t *vtxDevice = vtxCommonDevice();
 637             if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
 638                 vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
 639             }
 640             tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
 641             break;
 642         }
 643 #endif
 644
 645     case OSD_CROSSHAIRS:
 646         buff[0] = SYM_AH_CENTER_LINE;
 647         buff[1] = SYM_AH_CENTER;
 648         buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 649         buff[3] = 0;
 650         break;
 651
 652     case OSD_ARTIFICIAL_HORIZON:
 653         {
 654             // Get pitch and roll limits in tenths of degrees
 655             const int maxPitch = osdConfig()->ahMaxPitch * 10;
 656             const int maxRoll = osdConfig()->ahMaxRoll * 10;
 657             const int rollAngle = constrain(attitude.values.roll, -maxRoll, maxRoll);
 658             int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch);
 659             // Convert pitchAngle to y compensation value
 660             // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
 661             pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
 662
 663             for (int x = -4; x <= 4; x++) {
 664                 const int y = ((-rollAngle * x) / 64) - pitchAngle;
 665                 if (y >= 0 && y <= 81) {
 666                     displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
 667                 }
 668             }
 669
 670             return true;
 671         }
 672
 673     case OSD_HORIZON_SIDEBARS:
 674         {
 675             // Draw AH sides
 676             const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
 677             const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
 678             for (int y = -hudheight; y <= hudheight; y++) {
 679                 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION);
 680                 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION);
 681             }
 682
 683             // AH level indicators
 684             displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
 685             displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
 686
 687             return true;
 688         }
 689
 690     case OSD_ROLL_PIDS:
 691         osdFormatPID(buff, "ROL", &currentPidProfile->pid[PID_ROLL]);
 692         break;
 693
 694     case OSD_PITCH_PIDS:
 695         osdFormatPID(buff, "PIT", &currentPidProfile->pid[PID_PITCH]);
 696         break;
 697
 698     case OSD_YAW_PIDS:
 699         osdFormatPID(buff, "YAW", &currentPidProfile->pid[PID_YAW]);
 700         break;
 701
 702     case OSD_POWER:
 703         tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000);
 704         break;
 705
 706     case OSD_PIDRATE_PROFILE:
 707         tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
 708         break;
 709
 710     case OSD_WARNINGS:
 711         {
 712
 713 #define OSD_WARNINGS_MAX_SIZE 11
 714 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
 715
 716             STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
 717
 718             const batteryState_e batteryState = getBatteryState();
 719
 720 #ifdef USE_DSHOT
 721             if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
 722                 int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - micros()) / 1e5;
 723                 if (armingDelayTime < 0) {
 724                     armingDelayTime = 0;
 725                 }
 726                 if (armingDelayTime >= (DSHOT_BEACON_GUARD_DELAY_US / 1e5 - 5)) {
 727                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " BEACON ON"); // Display this message for the first 0.5 seconds
 728                 } else {
 729                     char armingDelayMessage[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
 730                     tfp_sprintf(armingDelayMessage, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
 731                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDelayMessage);
 732                 }
 733                 break;
 734             }
 735 #endif
 736
 737             if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
 738                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW");
 739                 break;
 740             }
 741
 742 #ifdef USE_ADC_INTERNAL
 743             uint8_t coreTemperature = getCoreTemperatureCelsius();
 744             if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
 745                 char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
 746                 tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
 747
 748                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg);
 749
 750                 break;
 751             }
 752 #endif
 753
 754 #ifdef USE_ESC_SENSOR
 755             // Show warning if we lose motor output, the ESC is overheating or excessive current draw
 756             if (feature(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
 757                 char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
 758                 unsigned pos = 0;
 759
 760                 const char *title = "ESC";
 761
 762                 // center justify message
 763                 while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
 764                     escWarningMsg[pos++] = ' ';
 765                 }
 766
 767                 strcpy(escWarningMsg + pos, title);
 768                 pos += strlen(title);
 769
 770                 unsigned i = 0;
 771                 unsigned escWarningCount = 0;
 772                 while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
 773                     escSensorData_t *escData = getEscSensorData(i);
 774                     const char motorNumber = '1' + i;
 775                     // if everything is OK just display motor number else R, T or C
 776                     char warnFlag = motorNumber;
 777                     if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
 778                         warnFlag = 'R';
 779                     }
 780                     if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
 781                         warnFlag = 'T';
 782                     }
 783                     if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
 784                         warnFlag = 'C';
 785                     }
 786
 787                     escWarningMsg[pos++] = warnFlag;
 788
 789                     if (warnFlag != motorNumber) {
 790                         escWarningCount++;
 791                     }
 792
 793                     i++;
 794                 }
 795
 796                 escWarningMsg[pos] = '\0';
 797
 798                 if (escWarningCount > 0) {
 799                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg);
 800                 }
 801                 break;
 802             }
 803 #endif
 804
 805             // Warn when in flip over after crash mode
 806             if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashMode()) {
 807                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP");
 808                 break;
 809             }
 810
 811             // Show most severe reason for arming being disabled
 812             if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE) && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
 813                 const armingDisableFlags_e flags = getArmingDisableFlags();
 814                 for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) {
 815                     if (flags & (1 << i)) {
 816                         osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]);
 817                         break;
 818                     }
 819                 }
 820                 break;
 821             }
 822
 823             if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
 824                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
 825                 break;
 826             }
 827
 828             // Show warning if battery is not fresh
 829             if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK)
 830                   && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
 831                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL");
 832                 break;
 833             }
 834
 835             // Visual beeper
 836             if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) {
 837                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "  * * * *");
 838                 break;
 839             }
 840
 841             osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL);
 842             break;
 843         }
 844
 845     case OSD_AVG_CELL_VOLTAGE:
 846         {
 847             const int cellV = osdGetBatteryAverageCellVoltage();
 848             buff[0] = osdGetBatterySymbol(cellV);
 849             tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
 850             break;
 851         }
 852
 853     case OSD_DEBUG:
 854         tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
 855         break;
 856
 857     case OSD_PITCH_ANGLE:
 858     case OSD_ROLL_ANGLE:
 859         {
 860             const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
 861             tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
 862             break;
 863         }
 864
 865     case OSD_MAIN_BATT_USAGE:
 866         {
 867             // Set length of indicator bar
 868             #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
 869
 870             // Calculate constrained value
 871             const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
 872
 873             // Calculate mAh used progress
 874             const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
 875
 876             // Create empty battery indicator bar
 877             buff[0] = SYM_PB_START;
 878             for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
 879                 buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
 880             }
 881             buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
 882             if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
 883                 buff[1 + mAhUsedProgress] = SYM_PB_END;
 884             }
 885             buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
 886             break;
 887         }
 888
 889     case OSD_DISARMED:
 890         if (!ARMING_FLAG(ARMED)) {
 891             tfp_sprintf(buff, "DISARMED");
 892         } else {
 893             if (!lastArmState) {  // previously disarmed - blank out the message one time
 894                 tfp_sprintf(buff, "        ");
 895             }
 896         }
 897         break;
 898
 899     case OSD_NUMERICAL_HEADING:
 900         {
 901             const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 902             tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
 903             break;
 904         }
 905
 906     case OSD_NUMERICAL_VARIO:
 907         {
 908             const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
 909             const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH;
 910             tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
 911             break;
 912         }
 913
 914 #ifdef USE_ESC_SENSOR
 915     case OSD_ESC_TMP:
 916         if (feature(FEATURE_ESC_SENSOR)) {
 917             tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
 918         }
 919         break;
 920
 921     case OSD_ESC_RPM:
 922         if (feature(FEATURE_ESC_SENSOR)) {
 923             tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm));
 924         }
 925         break;
 926 #endif
 927
 928 #ifdef USE_RTC_TIME
 929     case OSD_RTC_DATETIME:
 930         osdFormatRtcDateTime(&buff[0]);
 931         break;
 932 #endif
 933
 934 #ifdef USE_OSD_ADJUSTMENTS
 935     case OSD_ADJUSTMENT_RANGE:
 936         tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue);
 937         break;
 938 #endif
 939
 940 #ifdef USE_ADC_INTERNAL
 941     case OSD_CORE_TEMPERATURE:
 942         tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
 943         break;
 944 #endif
 945
 946     default:
 947         return false;
 948     }
 949
 950     displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
 951
 952     return true;
 953 }
 954
 955 static void osdDrawElements(void)
 956 {
 957     displayClearScreen(osdDisplayPort);
 958
 959     // Hide OSD when OSDSW mode is active
 960     if (IS_RC_MODE_ACTIVE(BOXOSD)) {
 961         return;
 962     }
 963
 964     if (sensors(SENSOR_ACC)) {
 965         osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
 966     }
 967
 968
 969     for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
 970         osdDrawSingleElement(osdElementDisplayOrder[i]);
 971     }
 972
 973 #ifdef USE_GPS
 974     if (sensors(SENSOR_GPS)) {
 975         osdDrawSingleElement(OSD_GPS_SATS);
 976         osdDrawSingleElement(OSD_GPS_SPEED);
 977         osdDrawSingleElement(OSD_GPS_LAT);
 978         osdDrawSingleElement(OSD_GPS_LON);
 979         osdDrawSingleElement(OSD_HOME_DIST);
 980         osdDrawSingleElement(OSD_HOME_DIR);
 981     }
 982 #endif // GPS
 983
 984 #ifdef USE_ESC_SENSOR
 985     if (feature(FEATURE_ESC_SENSOR)) {
 986         osdDrawSingleElement(OSD_ESC_TMP);
 987         osdDrawSingleElement(OSD_ESC_RPM);
 988     }
 989 #endif
 990
 991 #ifdef USE_RTC_TIME
 992     osdDrawSingleElement(OSD_RTC_DATETIME);
 993 #endif
 994
 995 #ifdef USE_OSD_ADJUSTMENTS
 996     osdDrawSingleElement(OSD_ADJUSTMENT_RANGE);
 997 #endif
 998
 999 #ifdef USE_ADC_INTERNAL
1000     osdDrawSingleElement(OSD_CORE_TEMPERATURE);
1001 #endif
1002 }
1003
1004 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
1005 {
1006     // Position elements near centre of screen and disabled by default
1007     for (int i = 0; i < OSD_ITEM_COUNT; i++) {
1008         osdConfig->item_pos[i] = OSD_POS(10, 7);
1009     }
1010
1011     // Always enable warnings elements by default
1012     osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | VISIBLE_FLAG;
1013
1014     // Default to old fixed positions for these elements
1015     osdConfig->item_pos[OSD_CROSSHAIRS]         = OSD_POS(13, 6);
1016     osdConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
1017     osdConfig->item_pos[OSD_HORIZON_SIDEBARS]   = OSD_POS(14, 6);
1018
1019     // Enable the default stats
1020     osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
1021     osdStatSetState(OSD_STAT_MAX_SPEED, true);
1022     osdStatSetState(OSD_STAT_MIN_BATTERY, true);
1023     osdStatSetState(OSD_STAT_MIN_RSSI, true);
1024     osdStatSetState(OSD_STAT_MAX_CURRENT, true);
1025     osdStatSetState(OSD_STAT_USED_MAH, true);
1026     osdStatSetState(OSD_STAT_BLACKBOX, true);
1027     osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, true);
1028     osdStatSetState(OSD_STAT_TIMER_2, true);
1029
1030     osdConfig->units = OSD_UNIT_METRIC;
1031
1032     // Enable all warnings by default
1033     for (int i=0; i < OSD_WARNING_COUNT; i++) {
1034         osdWarnSetState(i, true);
1035     }
1036
1037     osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10);
1038     osdConfig->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10);
1039
1040     osdConfig->rssi_alarm = 20;
1041     osdConfig->cap_alarm  = 2200;
1042     osdConfig->alt_alarm  = 100; // meters or feet depend on configuration
1043     osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
1044     osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
1045     osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
1046     osdConfig->core_temp_alarm = 70; // a temperature above 70C should produce a warning, lockups have been reported above 80C
1047
1048     osdConfig->ahMaxPitch = 20; // 20 degrees
1049     osdConfig->ahMaxRoll = 40; // 40 degrees
1050 }
1051
1052 static void osdDrawLogo(int x, int y)
1053 {
1054     // display logo and help
1055     int fontOffset = 160;
1056     for (int row = 0; row < 4; row++) {
1057         for (int column = 0; column < 24; column++) {
1058             if (fontOffset <= SYM_END_OF_FONT)
1059                 displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
1060         }
1061     }
1062 }
1063
1064 void osdInit(displayPort_t *osdDisplayPortToUse)
1065 {
1066     if (!osdDisplayPortToUse) {
1067         return;
1068     }
1069
1070     BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
1071
1072     osdDisplayPort = osdDisplayPortToUse;
1073 #ifdef USE_CMS
1074     cmsDisplayPortRegister(osdDisplayPort);
1075 #endif
1076
1077     armState = ARMING_FLAG(ARMED);
1078
1079     memset(blinkBits, 0, sizeof(blinkBits));
1080
1081     displayClearScreen(osdDisplayPort);
1082
1083     osdDrawLogo(3, 1);
1084
1085     char string_buffer[30];
1086     tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
1087     displayWrite(osdDisplayPort, 20, 6, string_buffer);
1088 #ifdef USE_CMS
1089     displayWrite(osdDisplayPort, 7, 8,  CMS_STARTUP_HELP_TEXT1);
1090     displayWrite(osdDisplayPort, 11, 9, CMS_STARTUP_HELP_TEXT2);
1091     displayWrite(osdDisplayPort, 11, 10, CMS_STARTUP_HELP_TEXT3);
1092 #endif
1093
1094 #ifdef USE_RTC_TIME
1095     char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
1096     if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
1097         displayWrite(osdDisplayPort, 5, 12, dateTimeBuffer);
1098     }
1099 #endif
1100
1101     displayResync(osdDisplayPort);
1102
1103     resumeRefreshAt = micros() + (4 * REFRESH_1S);
1104 }
1105
1106 void osdUpdateAlarms(void)
1107 {
1108     // This is overdone?
1109
1110     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
1111
1112     if (getRssiPercent() < osdConfig()->rssi_alarm) {
1113         SET_BLINK(OSD_RSSI_VALUE);
1114     } else {
1115         CLR_BLINK(OSD_RSSI_VALUE);
1116     }
1117
1118     // Determine if the OSD_WARNINGS should blink
1119     if (getBatteryState() != BATTERY_OK
1120            && (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) || osdWarnGetState(OSD_WARNING_BATTERY_WARNING))
1121 #ifdef USE_DSHOT
1122            && (!isTryingToArm())
1123 #endif
1124        ) {
1125         SET_BLINK(OSD_WARNINGS);
1126     } else {
1127         CLR_BLINK(OSD_WARNINGS);
1128     }
1129
1130     if (getBatteryState() == BATTERY_OK) {
1131         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1132         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1133     } else {
1134         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
1135         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
1136     }
1137
1138     if (STATE(GPS_FIX) == 0) {
1139         SET_BLINK(OSD_GPS_SATS);
1140     } else {
1141         CLR_BLINK(OSD_GPS_SATS);
1142     }
1143
1144     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1145         const uint16_t timer = osdConfig()->timers[i];
1146         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
1147         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
1148         if (alarmTime != 0 && time >= alarmTime) {
1149             SET_BLINK(OSD_ITEM_TIMER_1 + i);
1150         } else {
1151             CLR_BLINK(OSD_ITEM_TIMER_1 + i);
1152         }
1153     }
1154
1155     if (getMAhDrawn() >= osdConfig()->cap_alarm) {
1156         SET_BLINK(OSD_MAH_DRAWN);
1157         SET_BLINK(OSD_MAIN_BATT_USAGE);
1158         SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1159     } else {
1160         CLR_BLINK(OSD_MAH_DRAWN);
1161         CLR_BLINK(OSD_MAIN_BATT_USAGE);
1162         CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1163     }
1164
1165     if (alt >= osdConfig()->alt_alarm) {
1166         SET_BLINK(OSD_ALTITUDE);
1167     } else {
1168         CLR_BLINK(OSD_ALTITUDE);
1169     }
1170
1171 #ifdef USE_ESC_SENSOR
1172     if (feature(FEATURE_ESC_SENSOR)) {
1173         // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
1174         if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
1175             SET_BLINK(OSD_ESC_TMP);
1176         } else {
1177             CLR_BLINK(OSD_ESC_TMP);
1178         }
1179     }
1180 #endif
1181 }
1182
1183 void osdResetAlarms(void)
1184 {
1185     CLR_BLINK(OSD_RSSI_VALUE);
1186     CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1187     CLR_BLINK(OSD_WARNINGS);
1188     CLR_BLINK(OSD_GPS_SATS);
1189     CLR_BLINK(OSD_MAH_DRAWN);
1190     CLR_BLINK(OSD_ALTITUDE);
1191     CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1192     CLR_BLINK(OSD_MAIN_BATT_USAGE);
1193     CLR_BLINK(OSD_ITEM_TIMER_1);
1194     CLR_BLINK(OSD_ITEM_TIMER_2);
1195     CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1196     CLR_BLINK(OSD_ESC_TMP);
1197 }
1198
1199 static void osdResetStats(void)
1200 {
1201     stats.max_current  = 0;
1202     stats.max_speed    = 0;
1203     stats.min_voltage  = 500;
1204     stats.max_current  = 0;
1205     stats.min_rssi     = 99;
1206     stats.max_altitude = 0;
1207     stats.max_distance = 0;
1208     stats.armed_time   = 0;
1209 }
1210
1211 static void osdUpdateStats(void)
1212 {
1213     int16_t value = 0;
1214 #ifdef USE_GPS
1215     switch (osdConfig()->units) {
1216     case OSD_UNIT_IMPERIAL:
1217         value = CM_S_TO_MPH(gpsSol.groundSpeed);
1218         break;
1219     default:
1220         value = CM_S_TO_KM_H(gpsSol.groundSpeed);
1221         break;
1222     }
1223 #endif
1224     if (stats.max_speed < value) {
1225         stats.max_speed = value;
1226     }
1227
1228     value = getBatteryVoltage();
1229     if (stats.min_voltage > value) {
1230         stats.min_voltage = value;
1231     }
1232
1233     value = getAmperage() / 100;
1234     if (stats.max_current < value) {
1235         stats.max_current = value;
1236     }
1237
1238     value = getRssiPercent();
1239     if (stats.min_rssi > value) {
1240         stats.min_rssi = value;
1241     }
1242
1243     int altitude = getEstimatedAltitude();
1244     if (stats.max_altitude < altitude) {
1245         stats.max_altitude = altitude;
1246     }
1247
1248 #ifdef USE_GPS
1249     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1250         value = GPS_distanceToHome;
1251
1252         if (stats.max_distance < GPS_distanceToHome) {
1253             stats.max_distance = GPS_distanceToHome;
1254         }
1255     }
1256 #endif
1257 }
1258
1259 #ifdef USE_BLACKBOX
1260 static void osdGetBlackboxStatusString(char * buff)
1261 {
1262     bool storageDeviceIsWorking = false;
1263     uint32_t storageUsed = 0;
1264     uint32_t storageTotal = 0;
1265
1266     switch (blackboxConfig()->device) {
1267 #ifdef USE_SDCARD
1268     case BLACKBOX_DEVICE_SDCARD:
1269         storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY);
1270         if (storageDeviceIsWorking) {
1271             storageTotal = sdcard_getMetadata()->numBlocks / 2000;
1272             storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
1273         }
1274         break;
1275 #endif
1276
1277 #ifdef USE_FLASHFS
1278     case BLACKBOX_DEVICE_FLASH:
1279         storageDeviceIsWorking = flashfsIsReady();
1280         if (storageDeviceIsWorking) {
1281             const flashGeometry_t *geometry = flashfsGetGeometry();
1282             storageTotal = geometry->totalSize / 1024;
1283             storageUsed = flashfsGetOffset() / 1024;
1284         }
1285         break;
1286 #endif
1287
1288     default:
1289         break;
1290     }
1291
1292     if (storageDeviceIsWorking) {
1293         const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
1294         tfp_sprintf(buff, "%d%%", storageUsedPercent);
1295     } else {
1296         tfp_sprintf(buff, "FAULT");
1297     }
1298 }
1299 #endif
1300
1301 static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
1302 {
1303     displayWrite(osdDisplayPort, 2, y, text);
1304     displayWrite(osdDisplayPort, 20, y, ":");
1305     displayWrite(osdDisplayPort, 22, y, value);
1306 }
1307
1308 /*
1309  * Test if there's some stat enabled
1310  */
1311 static bool isSomeStatEnabled(void)
1312 {
1313     return (osdConfig()->enabled_stats != 0);
1314 }
1315
1316 // *** IMPORTANT ***
1317 // The order of the OSD stats as displayed on-screen must match the osd_stats_e enumeration.
1318 // This is because the fields are presented in the configurator in the order of the enumeration
1319 // and we want the configuration order to match the on-screen display order.  If you change the
1320 // display order you *must* update the osd_stats_e enumeration to match. Additionally the
1321 // changes to the stats display order *must* be implemented in the configurator otherwise the
1322 // stats selections will not be populated correctly and the settings will become corrupted.
1323
1324 static void osdShowStats(uint16_t endBatteryVoltage)
1325 {
1326     uint8_t top = 2;
1327     char buff[OSD_ELEMENT_BUFFER_LENGTH];
1328
1329     displayClearScreen(osdDisplayPort);
1330     displayWrite(osdDisplayPort, 2, top++, "  --- STATS ---");
1331
1332     if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
1333         bool success = false;
1334 #ifdef USE_RTC_TIME
1335         success = osdFormatRtcDateTime(&buff[0]);
1336 #endif
1337         if (!success) {
1338             tfp_sprintf(buff, "NO RTC");
1339         }
1340
1341         displayWrite(osdDisplayPort, 2, top++, buff);
1342     }
1343
1344     if (osdStatGetState(OSD_STAT_TIMER_1)) {
1345         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
1346         osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
1347     }
1348
1349     if (osdStatGetState(OSD_STAT_TIMER_2)) {
1350         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
1351         osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
1352     }
1353
1354     if (osdStatGetState(OSD_STAT_MAX_SPEED) && STATE(GPS_FIX)) {
1355         itoa(stats.max_speed, buff, 10);
1356         osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
1357     }
1358
1359     if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) {
1360         tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
1361         osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
1362     }
1363
1364     if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
1365         tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT);
1366         osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
1367     }
1368
1369     if (osdStatGetState(OSD_STAT_END_BATTERY)) {
1370         tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT);
1371         osdDisplayStatisticLabel(top++, "END BATTERY", buff);
1372     }
1373
1374     if (osdStatGetState(OSD_STAT_BATTERY)) {
1375         tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
1376         osdDisplayStatisticLabel(top++, "BATTERY", buff);
1377     }
1378
1379     if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
1380         itoa(stats.min_rssi, buff, 10);
1381         strcat(buff, "%");
1382         osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
1383     }
1384
1385     if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
1386         if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
1387             itoa(stats.max_current, buff, 10);
1388             strcat(buff, "A");
1389             osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
1390         }
1391
1392         if (osdStatGetState(OSD_STAT_USED_MAH)) {
1393             tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
1394             osdDisplayStatisticLabel(top++, "USED MAH", buff);
1395         }
1396     }
1397
1398     if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
1399         osdFormatAltitudeString(buff, stats.max_altitude);
1400         osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
1401     }
1402
1403 #ifdef USE_BLACKBOX
1404     if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
1405         osdGetBlackboxStatusString(buff);
1406         osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
1407     }
1408
1409     if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
1410         itoa(blackboxGetLogNumber(), buff, 10);
1411         osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
1412     }
1413 #endif
1414
1415 }
1416
1417 static void osdShowArmed(void)
1418 {
1419     displayClearScreen(osdDisplayPort);
1420     displayWrite(osdDisplayPort, 12, 7, "ARMED");
1421 }
1422
1423 STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
1424 {
1425     static timeUs_t lastTimeUs = 0;
1426     static bool osdStatsEnabled = false;
1427     static bool osdStatsVisible = false;
1428     static timeUs_t osdStatsRefreshTimeUs;
1429     static uint16_t endBatteryVoltage;
1430
1431     // detect arm/disarm
1432     if (armState != ARMING_FLAG(ARMED)) {
1433         if (ARMING_FLAG(ARMED)) {
1434             osdStatsEnabled = false;
1435             osdStatsVisible = false;
1436             osdResetStats();
1437             osdShowArmed();
1438             resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
1439         } else if (isSomeStatEnabled()
1440                    && (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
1441                        || !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
1442             osdStatsEnabled = true;
1443             resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
1444             endBatteryVoltage = getBatteryVoltage();
1445         }
1446
1447         armState = ARMING_FLAG(ARMED);
1448     }
1449
1450
1451     if (ARMING_FLAG(ARMED)) {
1452         osdUpdateStats();
1453         timeUs_t deltaT = currentTimeUs - lastTimeUs;
1454         flyTime += deltaT;
1455         stats.armed_time += deltaT;
1456     } else if (osdStatsEnabled) {  // handle showing/hiding stats based on OSD disable switch position
1457         if (displayIsGrabbed(osdDisplayPort)) {
1458             osdStatsEnabled = false;
1459             resumeRefreshAt = 0;
1460             stats.armed_time = 0;
1461         } else {
1462             if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
1463                 osdStatsVisible = false;
1464                 displayClearScreen(osdDisplayPort);
1465             } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
1466                 if (!osdStatsVisible) {
1467                     osdStatsVisible = true;
1468                     osdStatsRefreshTimeUs = 0;
1469                 }
1470                 if (currentTimeUs >= osdStatsRefreshTimeUs) {
1471                     osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
1472                     osdShowStats(endBatteryVoltage);
1473                 }
1474             }
1475         }
1476     }
1477     lastTimeUs = currentTimeUs;
1478
1479     if (resumeRefreshAt) {
1480         if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
1481             // in timeout period, check sticks for activity to resume display.
1482             if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
1483                 resumeRefreshAt = currentTimeUs;
1484             }
1485             displayHeartbeat(osdDisplayPort);
1486             return;
1487         } else {
1488             displayClearScreen(osdDisplayPort);
1489             resumeRefreshAt = 0;
1490             osdStatsEnabled = false;
1491             stats.armed_time = 0;
1492         }
1493     }
1494
1495     blinkState = (currentTimeUs / 200000) % 2;
1496
1497 #ifdef USE_ESC_SENSOR
1498     if (feature(FEATURE_ESC_SENSOR)) {
1499         escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
1500     }
1501 #endif
1502
1503 #ifdef USE_CMS
1504     if (!displayIsGrabbed(osdDisplayPort)) {
1505         osdUpdateAlarms();
1506         osdDrawElements();
1507         displayHeartbeat(osdDisplayPort);
1508 #ifdef OSD_CALLS_CMS
1509     } else {
1510         cmsUpdate(currentTimeUs);
1511 #endif
1512     }
1513 #endif
1514     lastArmState = ARMING_FLAG(ARMED);
1515 }
1516
1517 /*
1518  * Called periodically by the scheduler
1519  */
1520 void osdUpdate(timeUs_t currentTimeUs)
1521 {
1522     static uint32_t counter = 0;
1523
1524     if (isBeeperOn()) {
1525         showVisualBeeper = true;
1526     }
1527
1528 #ifdef MAX7456_DMA_CHANNEL_TX
1529     // don't touch buffers if DMA transaction is in progress
1530     if (displayIsTransferInProgress(osdDisplayPort)) {
1531         return;
1532     }
1533 #endif // MAX7456_DMA_CHANNEL_TX
1534
1535 #ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
1536     static uint32_t idlecounter = 0;
1537     if (!ARMING_FLAG(ARMED)) {
1538         if (idlecounter++ % 4 != 0) {
1539             return;
1540         }
1541     }
1542 #endif
1543
1544     // redraw values in buffer
1545 #ifdef USE_MAX7456
1546 #define DRAW_FREQ_DENOM 5
1547 #else
1548 #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud (
1549 #endif
1550 #define STATS_FREQ_DENOM    50
1551
1552     if (counter % DRAW_FREQ_DENOM == 0) {
1553         osdRefresh(currentTimeUs);
1554         showVisualBeeper = false;
1555     } else {
1556         // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
1557         displayDrawScreen(osdDisplayPort);
1558     }
1559     ++counter;
1560
1561 #ifdef USE_CMS
1562     // do not allow ARM if we are in menu
1563     if (displayIsGrabbed(osdDisplayPort)) {
1564         setArmingDisabled(ARMING_DISABLED_OSD_MENU);
1565     } else {
1566         unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
1567     }
1568 #endif
1569 }
1570
1571 #endif // USE_OSD