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