src/main/osd/osd_elements.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     *****************************************
  23     Instructions for adding new OSD Elements:
  24     *****************************************
  25
  26     First add the new element to the osd_items_e enumeration in osd/osd.h. The
  27     element must be added to the end just before OSD_ITEM_COUNT.
  28
  29     Next add the element to the osdElementDisplayOrder array defined in this file.
  30     If the element needs special runtime conditional processing then it should be added
  31     to the osdAddActiveElements() function instead.
  32
  33     Create the function to "draw" the element.
  34     ------------------------------------------
  35     It should be named like "osdElementSomething()" where the "Something" describes
  36     the element. The drawing function should only render the dynamic portions of the
  37     element. If the element has static (unchanging) portions then those should be
  38     rendered in the background function. The exception to this is elements that are
  39     expected to blink (have a warning associated). In this case the entire element
  40     must be handled in the main draw function and you can't use the background capability.
  41
  42     Add the mapping from the element ID added in the first step to the function
  43     created in the third step to the osdElementDrawFunction array.
  44
  45     Create the function to draw the element's static (background) portion.
  46     ---------------------------------------------------------------------
  47     If an element has static (unchanging) portions then create a function to draw only those
  48     parts. It should be named like "osdBackgroundSomething()" where the "Something" matches
  49     the related element function.
  50
  51     Add the mapping for the element ID to the background drawing function to the
  52     osdElementBackgroundFunction array.
  53
  54     Accelerometer reqirement:
  55     -------------------------
  56     If the new element utilizes the accelerometer, add it to the osdElementsNeedAccelerometer() function.
  57
  58     Finally add a CLI parameter for the new element in cli/settings.c.
  59 */
  60
  61 #include <stdbool.h>
  62 #include <stdint.h>
  63 #include <stdlib.h>
  64 #include <string.h>
  65 #include <ctype.h>
  66 #include <math.h>
  67
  68 #include "platform.h"
  69
  70 #ifdef USE_OSD
  71
  72 #include "blackbox/blackbox.h"
  73 #include "blackbox/blackbox_io.h"
  74
  75 #include "build/build_config.h"
  76 #include "build/debug.h"
  77
  78 #include "common/axis.h"
  79 #include "common/maths.h"
  80 #include "common/printf.h"
  81 #include "common/typeconversion.h"
  82 #include "common/utils.h"
  83
  84 #include "config/config.h"
  85 #include "config/feature.h"
  86
  87 #include "drivers/display.h"
  88 #include "drivers/max7456_symbols.h"
  89 #include "drivers/dshot.h"
  90 #include "drivers/time.h"
  91 #include "drivers/vtx_common.h"
  92
  93 #include "fc/controlrate_profile.h"
  94 #include "fc/core.h"
  95 #include "fc/rc_adjustments.h"
  96 #include "fc/rc_controls.h"
  97 #include "fc/rc_modes.h"
  98 #include "fc/rc.h"
  99 #include "fc/runtime_config.h"
 100
 101 #include "flight/gps_rescue.h"
 102 #include "flight/failsafe.h"
 103 #include "flight/position.h"
 104 #include "flight/imu.h"
 105 #include "flight/mixer.h"
 106 #include "flight/pid.h"
 107
 108 #include "io/beeper.h"
 109 #include "io/gps.h"
 110 #include "io/vtx.h"
 111
 112 #include "osd/osd.h"
 113 #include "osd/osd_elements.h"
 114
 115 #include "pg/motor.h"
 116
 117 #include "rx/rx.h"
 118
 119 #include "sensors/acceleration.h"
 120 #include "sensors/adcinternal.h"
 121 #include "sensors/barometer.h"
 122 #include "sensors/battery.h"
 123 #include "sensors/esc_sensor.h"
 124 #include "sensors/sensors.h"
 125
 126
 127 #define AH_SYMBOL_COUNT 9
 128 #define AH_SIDEBAR_WIDTH_POS 7
 129 #define AH_SIDEBAR_HEIGHT_POS 3
 130
 131 // Stick overlay size
 132 #define OSD_STICK_OVERLAY_WIDTH 7
 133 #define OSD_STICK_OVERLAY_HEIGHT 5
 134 #define OSD_STICK_OVERLAY_SPRITE_HEIGHT 3
 135 #define OSD_STICK_OVERLAY_VERTICAL_POSITIONS (OSD_STICK_OVERLAY_HEIGHT * OSD_STICK_OVERLAY_SPRITE_HEIGHT)
 136
 137 #define FULL_CIRCLE 360
 138
 139 #ifdef USE_OSD_STICK_OVERLAY
 140 typedef struct radioControls_s {
 141     uint8_t left_vertical;
 142     uint8_t left_horizontal;
 143     uint8_t right_vertical;
 144     uint8_t right_horizontal;
 145 } radioControls_t;
 146
 147 static const radioControls_t radioModes[4] = {
 148     { PITCH,    YAW,    THROTTLE,   ROLL }, // Mode 1
 149     { THROTTLE, YAW,    PITCH,      ROLL }, // Mode 2
 150     { PITCH,    ROLL,   THROTTLE,   YAW  }, // Mode 3
 151     { THROTTLE, ROLL,   PITCH,      YAW  }, // Mode 4
 152 };
 153 #endif
 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 unsigned activeOsdElementCount = 0;
 171 static uint8_t activeOsdElementArray[OSD_ITEM_COUNT];
 172 static bool backgroundLayerSupported = false;
 173
 174 // Blink control
 175 static bool blinkState = true;
 176 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32];
 177 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
 178 #define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
 179 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 180 #define BLINK(item) (IS_BLINK(item) && blinkState)
 181
 182 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 183 typedef int (*getEscRpmOrFreqFnPtr)(int i);
 184
 185 static int getEscRpm(int i)
 186 {
 187 #ifdef USE_DSHOT_TELEMETRY
 188     if (motorConfig()->dev.useDshotTelemetry) {
 189         return 100.0f / (motorConfig()->motorPoleCount / 2.0f) * getDshotTelemetry(i);
 190     }
 191 #endif
 192 #ifdef USE_ESC_SENSOR
 193     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 194         return calcEscRpm(getEscSensorData(i)->rpm);
 195     }
 196 #endif
 197     return 0;
 198 }
 199
 200 static int getEscRpmFreq(int i)
 201 {
 202     return getEscRpm(i) / 60;
 203 }
 204
 205 static void renderOsdEscRpmOrFreq(getEscRpmOrFreqFnPtr escFnPtr, osdElementParms_t *element)
 206 {
 207     int x = element->elemPosX;
 208     int y = element->elemPosY;
 209     for (int i=0; i < getMotorCount(); i++) {
 210         char rpmStr[6];
 211         const int rpm = MIN((*escFnPtr)(i),99999);
 212         const int len = tfp_sprintf(rpmStr, "%d", rpm);
 213         rpmStr[len] = '\0';
 214         displayWrite(element->osdDisplayPort, x, y + i, DISPLAYPORT_ATTR_NONE, rpmStr);
 215     }
 216     element->drawElement = false;
 217 }
 218 #endif
 219
 220 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 221 int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
 222 {
 223     switch (osdConfig()->units) {
 224     case OSD_UNIT_IMPERIAL:
 225         return lrintf(((tempInDegreesCelcius * 9.0f) / 5) + 32);
 226     default:
 227         return tempInDegreesCelcius;
 228     }
 229 }
 230 #endif
 231
 232 static void osdFormatAltitudeString(char * buff, int32_t altitudeCm)
 233 {
 234     const int alt = osdGetMetersToSelectedUnit(altitudeCm) / 10;
 235
 236     int pos = 0;
 237     buff[pos++] = SYM_ALTITUDE;
 238     if (alt < 0) {
 239         buff[pos++] = '-';
 240     }
 241     tfp_sprintf(buff + pos, "%01d.%01d%c", abs(alt) / 10 , abs(alt) % 10, osdGetMetersToSelectedUnitSymbol());
 242 }
 243
 244 #ifdef USE_GPS
 245 static void osdFormatCoordinate(char *buff, char sym, int32_t val)
 246 {
 247     // latitude maximum integer width is 3 (-90).
 248     // longitude maximum integer width is 4 (-180).
 249     // We show 7 decimals, so we need to use 12 characters:
 250     // eg: s-180.1234567z   s=symbol, z=zero terminator, decimal separator  between 0 and 1
 251
 252     int pos = 0;
 253     buff[pos++] = sym;
 254     if (val < 0) {
 255         buff[pos++] = '-';
 256         val = -val;
 257     }
 258     tfp_sprintf(buff + pos, "%d.%07d", val / GPS_DEGREES_DIVIDER, val % GPS_DEGREES_DIVIDER);
 259 }
 260 #endif // USE_GPS
 261
 262 void osdFormatDistanceString(char *ptr, int distance, char leadingSymbol)
 263 {
 264     const int convertedDistance = osdGetMetersToSelectedUnit(distance);
 265     char unitSymbol;
 266     char unitSymbolExtended;
 267     int unitTransition;
 268
 269     if (leadingSymbol != SYM_NONE) {
 270         *ptr++ = leadingSymbol;
 271     }
 272     switch (osdConfig()->units) {
 273     case OSD_UNIT_IMPERIAL:
 274         unitTransition = 5280;
 275         unitSymbol = SYM_FT;
 276         unitSymbolExtended = SYM_MILES;
 277         break;
 278     default:
 279         unitTransition = 1000;
 280         unitSymbol = SYM_M;
 281         unitSymbolExtended = SYM_KM;
 282         break;
 283     }
 284
 285     if (convertedDistance < unitTransition) {
 286         tfp_sprintf(ptr, "%d%c", convertedDistance, unitSymbol);
 287     } else {
 288         const int displayDistance = convertedDistance * 100 / unitTransition;
 289         if (displayDistance >= 1000) { // >= 10 miles or km - 1 decimal place
 290             tfp_sprintf(ptr, "%d.%d%c", displayDistance / 100, (displayDistance / 10) % 10, unitSymbolExtended);
 291         } else {                     // < 10 miles or km - 2 decimal places
 292             tfp_sprintf(ptr, "%d.%02d%c", displayDistance / 100, displayDistance % 100, unitSymbolExtended);
 293         }
 294     }
 295 }
 296
 297 static void osdFormatPID(char * buff, const char * label, const pidf_t * pid)
 298 {
 299     tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
 300 }
 301
 302 #ifdef USE_RTC_TIME
 303 bool osdFormatRtcDateTime(char *buffer)
 304 {
 305     dateTime_t dateTime;
 306     if (!rtcGetDateTime(&dateTime)) {
 307         buffer[0] = '\0';
 308
 309         return false;
 310     }
 311
 312     dateTimeFormatLocalShort(buffer, &dateTime);
 313
 314     return true;
 315 }
 316 #endif
 317
 318 void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
 319 {
 320     int seconds = time / 1000000;
 321     const int minutes = seconds / 60;
 322     seconds = seconds % 60;
 323
 324     switch (precision) {
 325     case OSD_TIMER_PREC_SECOND:
 326     default:
 327         tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
 328         break;
 329     case OSD_TIMER_PREC_HUNDREDTHS:
 330         {
 331             const int hundredths = (time / 10000) % 100;
 332             tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
 333             break;
 334         }
 335     case OSD_TIMER_PREC_TENTHS:
 336         {
 337             const int tenths = (time / 100000) % 10;
 338             tfp_sprintf(buff, "%02d:%02d.%01d", minutes, seconds, tenths);
 339             break;
 340         }
 341     }
 342 }
 343
 344 static char osdGetTimerSymbol(osd_timer_source_e src)
 345 {
 346     switch (src) {
 347     case OSD_TIMER_SRC_ON:
 348         return SYM_ON_M;
 349     case OSD_TIMER_SRC_TOTAL_ARMED:
 350     case OSD_TIMER_SRC_LAST_ARMED:
 351         return SYM_FLY_M;
 352     case OSD_TIMER_SRC_ON_OR_ARMED:
 353         return ARMING_FLAG(ARMED) ? SYM_FLY_M : SYM_ON_M;
 354     default:
 355         return ' ';
 356     }
 357 }
 358
 359 static timeUs_t osdGetTimerValue(osd_timer_source_e src)
 360 {
 361     switch (src) {
 362     case OSD_TIMER_SRC_ON:
 363         return micros();
 364     case OSD_TIMER_SRC_TOTAL_ARMED:
 365         return osdFlyTime;
 366     case OSD_TIMER_SRC_LAST_ARMED: {
 367         statistic_t *stats = osdGetStats();
 368         return stats->armed_time;
 369     }
 370     case OSD_TIMER_SRC_ON_OR_ARMED:
 371         return ARMING_FLAG(ARMED) ? osdFlyTime : micros();
 372     default:
 373         return 0;
 374     }
 375 }
 376
 377 void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
 378 {
 379     const uint16_t timer = osdConfig()->timers[timerIndex];
 380     const uint8_t src = OSD_TIMER_SRC(timer);
 381
 382     if (showSymbol) {
 383         *(buff++) = osdGetTimerSymbol(src);
 384     }
 385
 386     osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
 387 }
 388
 389 static char osdGetBatterySymbol(int cellVoltage)
 390 {
 391     if (getBatteryState() == BATTERY_CRITICAL) {
 392         return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
 393     } else {
 394         // Calculate a symbol offset using cell voltage over full cell voltage range
 395         const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatmaxcellvoltage, 0, 8);
 396         return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
 397     }
 398 }
 399
 400 static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
 401 {
 402     heading += FULL_CIRCLE;  // Ensure positive value
 403
 404     // Split input heading 0..359 into sectors 0..(directions-1), but offset
 405     // by half a sector so that sector 0 gets centered around heading 0.
 406     // We multiply heading by directions to not loose precision in divisions
 407     // In this way each segment will be a FULL_CIRCLE length
 408     int direction = (heading * directions + FULL_CIRCLE / 2) /  FULL_CIRCLE; // scale with rounding
 409     direction %= directions; // normalize
 410
 411     return direction; // return segment number
 412 }
 413
 414 static uint8_t osdGetDirectionSymbolFromHeading(int heading)
 415 {
 416     heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
 417
 418     // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
 419     // Our symbols are Down=0, Right=4, Up=8 and Left=12
 420     // There're 16 arrow symbols. Transform it.
 421     heading = 16 - heading;
 422     heading = (heading + 8) % 16;
 423
 424     return SYM_ARROW_SOUTH + heading;
 425 }
 426
 427
 428 /**
 429  * Converts altitude based on the current unit system.
 430  * @param meters Value in meters to convert
 431  */
 432 int32_t osdGetMetersToSelectedUnit(int32_t meters)
 433 {
 434     switch (osdConfig()->units) {
 435     case OSD_UNIT_IMPERIAL:
 436         return (meters * 328) / 100; // Convert to feet / 100
 437     default:
 438         return meters;               // Already in metre / 100
 439     }
 440 }
 441
 442 /**
 443  * Gets the correct altitude symbol for the current unit system
 444  */
 445 char osdGetMetersToSelectedUnitSymbol(void)
 446 {
 447     switch (osdConfig()->units) {
 448     case OSD_UNIT_IMPERIAL:
 449         return SYM_FT;
 450     default:
 451         return SYM_M;
 452     }
 453 }
 454
 455 /**
 456  * Converts speed based on the current unit system.
 457  * @param value in cm/s to convert
 458  */
 459 int32_t osdGetSpeedToSelectedUnit(int32_t value)
 460 {
 461     switch (osdConfig()->units) {
 462     case OSD_UNIT_IMPERIAL:
 463         return CM_S_TO_MPH(value);
 464     default:
 465         return CM_S_TO_KM_H(value);
 466     }
 467 }
 468
 469 /**
 470  * Gets the correct speed symbol for the current unit system
 471  */
 472 char osdGetSpeedToSelectedUnitSymbol(void)
 473 {
 474     switch (osdConfig()->units) {
 475     case OSD_UNIT_IMPERIAL:
 476         return SYM_MPH;
 477     default:
 478         return SYM_KPH;
 479     }
 480 }
 481
 482 char osdGetVarioToSelectedUnitSymbol(void)
 483 {
 484     switch (osdConfig()->units) {
 485     case OSD_UNIT_IMPERIAL:
 486         return SYM_FTPS;
 487     default:
 488         return SYM_MPS;
 489     }
 490 }
 491
 492 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 493 char osdGetTemperatureSymbolForSelectedUnit(void)
 494 {
 495     switch (osdConfig()->units) {
 496     case OSD_UNIT_IMPERIAL:
 497         return SYM_F;
 498     default:
 499         return SYM_C;
 500     }
 501 }
 502 #endif
 503
 504 // *************************
 505 // Element drawing functions
 506 // *************************
 507
 508 #ifdef USE_OSD_ADJUSTMENTS
 509 static void osdElementAdjustmentRange(osdElementParms_t *element)
 510 {
 511     const char *name = getAdjustmentsRangeName();
 512     if (name) {
 513         tfp_sprintf(element->buff, "%s: %3d", name, getAdjustmentsRangeValue());
 514     }
 515 }
 516 #endif // USE_OSD_ADJUSTMENTS
 517
 518 static void osdElementAltitude(osdElementParms_t *element)
 519 {
 520     bool haveBaro = false;
 521     bool haveGps = false;
 522 #ifdef USE_BARO
 523     haveBaro = sensors(SENSOR_BARO);
 524 #endif // USE_BARO
 525 #ifdef USE_GPS
 526     haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
 527 #endif // USE_GPS
 528     if (haveBaro || haveGps) {
 529         osdFormatAltitudeString(element->buff, getEstimatedAltitudeCm());
 530     } else {
 531         element->buff[0] = SYM_ALTITUDE;
 532         element->buff[1] = SYM_HYPHEN; // We use this symbol when we don't have a valid measure
 533         element->buff[2] = '\0';
 534     }
 535 }
 536
 537 #ifdef USE_ACC
 538 static void osdElementAngleRollPitch(osdElementParms_t *element)
 539 {
 540     const int angle = (element->item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
 541     tfp_sprintf(element->buff, "%c%c%02d.%01d", (element->item == OSD_PITCH_ANGLE) ? SYM_PITCH : SYM_ROLL , angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
 542 }
 543 #endif
 544
 545 static void osdElementAntiGravity(osdElementParms_t *element)
 546 {
 547     if (pidOsdAntiGravityActive()) {
 548         strcpy(element->buff, "AG");
 549     }
 550 }
 551
 552 #ifdef USE_ACC
 553 static void osdElementArtificialHorizon(osdElementParms_t *element)
 554 {
 555     // Get pitch and roll limits in tenths of degrees
 556     const int maxPitch = osdConfig()->ahMaxPitch * 10;
 557     const int maxRoll = osdConfig()->ahMaxRoll * 10;
 558     const int ahSign = osdConfig()->ahInvert ? -1 : 1;
 559     const int rollAngle = constrain(attitude.values.roll * ahSign, -maxRoll, maxRoll);
 560     int pitchAngle = constrain(attitude.values.pitch * ahSign, -maxPitch, maxPitch);
 561     // Convert pitchAngle to y compensation value
 562     // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
 563     if (maxPitch > 0) {
 564         pitchAngle = ((pitchAngle * 25) / maxPitch);
 565     }
 566     pitchAngle -= 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
 567
 568     for (int x = -4; x <= 4; x++) {
 569         const int y = ((-rollAngle * x) / 64) - pitchAngle;
 570         if (y >= 0 && y <= 81) {
 571             displayWriteChar(element->osdDisplayPort, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), DISPLAYPORT_ATTR_NONE, (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
 572         }
 573     }
 574
 575     element->drawElement = false;  // element already drawn
 576 }
 577 #endif // USE_ACC
 578
 579 static void osdElementAverageCellVoltage(osdElementParms_t *element)
 580 {
 581     const int cellV = getBatteryAverageCellVoltage();
 582     element->buff[0] = osdGetBatterySymbol(cellV);
 583     tfp_sprintf(element->buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
 584 }
 585
 586 static void osdElementCompassBar(osdElementParms_t *element)
 587 {
 588     memcpy(element->buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
 589     element->buff[9] = 0;
 590 }
 591
 592 #ifdef USE_ADC_INTERNAL
 593 static void osdElementCoreTemperature(osdElementParms_t *element)
 594 {
 595     tfp_sprintf(element->buff, "C%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius()), osdGetTemperatureSymbolForSelectedUnit());
 596 }
 597 #endif // USE_ADC_INTERNAL
 598
 599 static void osdBackgroundCraftName(osdElementParms_t *element)
 600 {
 601     if (strlen(pilotConfig()->name) == 0) {
 602         strcpy(element->buff, "CRAFT_NAME");
 603     } else {
 604         unsigned i;
 605         for (i = 0; i < MAX_NAME_LENGTH; i++) {
 606             if (pilotConfig()->name[i]) {
 607                 element->buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
 608             } else {
 609                 break;
 610             }
 611         }
 612         element->buff[i] = '\0';
 613     }
 614 }
 615
 616 #ifdef USE_ACC
 617 static void osdElementCrashFlipArrow(osdElementParms_t *element)
 618 {
 619     int rollAngle = attitude.values.roll / 10;
 620     const int pitchAngle = attitude.values.pitch / 10;
 621     if (abs(rollAngle) > 90) {
 622         rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
 623     }
 624
 625     if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) {
 626         if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
 627             if (pitchAngle > 0) {
 628                 if (rollAngle > 0) {
 629                     element->buff[0] = SYM_ARROW_WEST + 2;
 630                 } else {
 631                     element->buff[0] = SYM_ARROW_EAST - 2;
 632                 }
 633             } else {
 634                 if (rollAngle > 0) {
 635                     element->buff[0] = SYM_ARROW_WEST - 2;
 636                 } else {
 637                     element->buff[0] = SYM_ARROW_EAST + 2;
 638                 }
 639             }
 640         } else {
 641             if (abs(pitchAngle) > abs(rollAngle)) {
 642                 if (pitchAngle > 0) {
 643                     element->buff[0] = SYM_ARROW_SOUTH;
 644                 } else {
 645                     element->buff[0] = SYM_ARROW_NORTH;
 646                 }
 647             } else {
 648                 if (rollAngle > 0) {
 649                     element->buff[0] = SYM_ARROW_WEST;
 650                 } else {
 651                     element->buff[0] = SYM_ARROW_EAST;
 652                 }
 653             }
 654         }
 655         element->buff[1] = '\0';
 656     }
 657 }
 658 #endif // USE_ACC
 659
 660 static void osdBackgroundCrosshairs(osdElementParms_t *element)
 661 {
 662     element->buff[0] = SYM_AH_CENTER_LINE;
 663     element->buff[1] = SYM_AH_CENTER;
 664     element->buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 665     element->buff[3] = 0;
 666 }
 667
 668 static void osdElementCurrentDraw(osdElementParms_t *element)
 669 {
 670     const int32_t amperage = getAmperage();
 671     tfp_sprintf(element->buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
 672 }
 673
 674 static void osdElementDebug(osdElementParms_t *element)
 675 {
 676     tfp_sprintf(element->buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
 677 }
 678
 679 static void osdElementDisarmed(osdElementParms_t *element)
 680 {
 681     if (!ARMING_FLAG(ARMED)) {
 682         tfp_sprintf(element->buff, "DISARMED");
 683     }
 684 }
 685
 686 static void osdBackgroundDisplayName(osdElementParms_t *element)
 687 {
 688     if (strlen(pilotConfig()->displayName) == 0) {
 689         strcpy(element->buff, "DISPLAY_NAME");
 690     } else {
 691         unsigned i;
 692         for (i = 0; i < MAX_NAME_LENGTH; i++) {
 693             if (pilotConfig()->displayName[i]) {
 694                 element->buff[i] = toupper((unsigned char)pilotConfig()->displayName[i]);
 695             } else {
 696                 break;
 697             }
 698         }
 699         element->buff[i] = '\0';
 700     }
 701 }
 702
 703 #ifdef USE_PROFILE_NAMES
 704 static void osdElementRateProfileName(osdElementParms_t *element)
 705 {
 706     if (strlen(currentControlRateProfile->profileName) == 0) {
 707         tfp_sprintf(element->buff, "RATE_%u", getCurrentControlRateProfileIndex() + 1);
 708     } else {
 709         unsigned i;
 710         for (i = 0; i < MAX_PROFILE_NAME_LENGTH; i++) {
 711             if (currentControlRateProfile->profileName[i]) {
 712                 element->buff[i] = toupper((unsigned char)currentControlRateProfile->profileName[i]);
 713             } else {
 714                 break;
 715             }
 716         }
 717         element->buff[i] = '\0';
 718     }
 719 }
 720
 721 static void osdElementPidProfileName(osdElementParms_t *element)
 722 {
 723     if (strlen(currentPidProfile->profileName) == 0) {
 724         tfp_sprintf(element->buff, "PID_%u", getCurrentPidProfileIndex() + 1);
 725     } else {
 726         unsigned i;
 727         for (i = 0; i < MAX_PROFILE_NAME_LENGTH; i++) {
 728             if (currentPidProfile->profileName[i]) {
 729                 element->buff[i] = toupper((unsigned char)currentPidProfile->profileName[i]);
 730             } else {
 731                 break;
 732             }
 733         }
 734         element->buff[i] = '\0';
 735     }
 736 }
 737 #endif
 738
 739 #ifdef USE_OSD_PROFILES
 740 static void osdElementOsdProfileName(osdElementParms_t *element)
 741 {
 742     uint8_t profileIndex = getCurrentOsdProfileIndex();
 743
 744     if (strlen(osdConfig()->profile[profileIndex - 1]) == 0) {
 745         tfp_sprintf(element->buff, "OSD_%u", profileIndex);
 746     } else {
 747         unsigned i;
 748         for (i = 0; i < OSD_PROFILE_NAME_LENGTH; i++) {
 749             if (osdConfig()->profile[profileIndex - 1][i]) {
 750                 element->buff[i] = toupper((unsigned char)osdConfig()->profile[profileIndex - 1][i]);
 751             } else {
 752                 break;
 753             }
 754         }
 755         element->buff[i] = '\0';
 756     }
 757 }
 758 #endif
 759
 760 #ifdef USE_ESC_SENSOR
 761 static void osdElementEscTemperature(osdElementParms_t *element)
 762 {
 763     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 764         tfp_sprintf(element->buff, "E%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(osdEscDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
 765     }
 766 }
 767 #endif // USE_ESC_SENSOR
 768
 769 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 770 static void osdElementEscRpm(osdElementParms_t *element)
 771 {
 772     renderOsdEscRpmOrFreq(&getEscRpm,element);
 773 }
 774
 775 static void osdElementEscRpmFreq(osdElementParms_t *element)
 776 {
 777     renderOsdEscRpmOrFreq(&getEscRpmFreq,element);
 778 }
 779 #endif
 780
 781 static void osdElementFlymode(osdElementParms_t *element)
 782 {
 783     // Note that flight mode display has precedence in what to display.
 784     //  1. FS
 785     //  2. GPS RESCUE
 786     //  3. ANGLE, HORIZON, ACRO TRAINER
 787     //  4. AIR
 788     //  5. ACRO
 789
 790     if (FLIGHT_MODE(FAILSAFE_MODE)) {
 791         strcpy(element->buff, "!FS!");
 792     } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 793         strcpy(element->buff, "RESC");
 794     } else if (FLIGHT_MODE(HEADFREE_MODE)) {
 795         strcpy(element->buff, "HEAD");
 796     } else if (FLIGHT_MODE(ANGLE_MODE)) {
 797         strcpy(element->buff, "STAB");
 798     } else if (FLIGHT_MODE(HORIZON_MODE)) {
 799         strcpy(element->buff, "HOR ");
 800     } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
 801         strcpy(element->buff, "ATRN");
 802     } else if (airmodeIsEnabled()) {
 803         strcpy(element->buff, "AIR ");
 804     } else {
 805         strcpy(element->buff, "ACRO");
 806     }
 807 }
 808
 809 #ifdef USE_ACC
 810 static void osdElementGForce(osdElementParms_t *element)
 811 {
 812     const int gForce = lrintf(osdGForce * 10);
 813     tfp_sprintf(element->buff, "%01d.%01dG", gForce / 10, gForce % 10);
 814 }
 815 #endif // USE_ACC
 816
 817 #ifdef USE_GPS
 818 static void osdElementGpsFlightDistance(osdElementParms_t *element)
 819 {
 820     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 821         osdFormatDistanceString(element->buff, GPS_distanceFlownInCm / 100, SYM_TOTAL_DISTANCE);
 822     } else {
 823         // We use this symbol when we don't have a FIX
 824         tfp_sprintf(element->buff, "%c%c", SYM_TOTAL_DISTANCE, SYM_HYPHEN);
 825     }
 826 }
 827
 828 static void osdElementGpsHomeDirection(osdElementParms_t *element)
 829 {
 830     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 831         if (GPS_distanceToHome > 0) {
 832             const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 833             element->buff[0] = osdGetDirectionSymbolFromHeading(h);
 834         } else {
 835             element->buff[0] = SYM_OVER_HOME;
 836         }
 837
 838     } else {
 839         // We use this symbol when we don't have a FIX
 840         element->buff[0] = SYM_HYPHEN;
 841     }
 842
 843     element->buff[1] = 0;
 844 }
 845
 846 static void osdElementGpsHomeDistance(osdElementParms_t *element)
 847 {
 848     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 849         osdFormatDistanceString(element->buff, GPS_distanceToHome, SYM_HOMEFLAG);
 850     } else {
 851         element->buff[0] = SYM_HOMEFLAG;
 852         // We use this symbol when we don't have a FIX
 853         element->buff[1] = SYM_HYPHEN;
 854         element->buff[2] = '\0';
 855     }
 856 }
 857
 858 static void osdElementGpsLatitude(osdElementParms_t *element)
 859 {
 860     osdFormatCoordinate(element->buff, SYM_LAT, gpsSol.llh.lat);
 861 }
 862
 863 static void osdElementGpsLongitude(osdElementParms_t *element)
 864 {
 865     osdFormatCoordinate(element->buff, SYM_LON, gpsSol.llh.lon);
 866 }
 867
 868 static void osdElementGpsSats(osdElementParms_t *element)
 869 {
 870     if (osdConfig()->gps_sats_show_hdop) {
 871         tfp_sprintf(element->buff, "%c%c%2d %d.%d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat, gpsSol.hdop / 100, (gpsSol.hdop / 10) % 10);
 872     } else {
 873         tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
 874     }
 875 }
 876
 877 static void osdElementGpsSpeed(osdElementParms_t *element)
 878 {
 879     tfp_sprintf(element->buff, "%c%3d%c", SYM_SPEED, osdGetSpeedToSelectedUnit(gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed), osdGetSpeedToSelectedUnitSymbol());
 880 }
 881 #endif // USE_GPS
 882
 883 static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
 884 {
 885     // Draw AH sides
 886     const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
 887     const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
 888     for (int y = -hudheight; y <= hudheight; y++) {
 889         displayWriteChar(element->osdDisplayPort, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NONE, SYM_AH_DECORATION);
 890         displayWriteChar(element->osdDisplayPort, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NONE, SYM_AH_DECORATION);
 891     }
 892
 893     // AH level indicators
 894     displayWriteChar(element->osdDisplayPort, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_ATTR_NONE, SYM_AH_LEFT);
 895     displayWriteChar(element->osdDisplayPort, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_ATTR_NONE, SYM_AH_RIGHT);
 896
 897     element->drawElement = false;  // element already drawn
 898 }
 899
 900 #ifdef USE_RX_LINK_QUALITY_INFO
 901 static void osdElementLinkQuality(osdElementParms_t *element)
 902 {
 903     uint16_t osdLinkQuality = 0;
 904     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-300
 905         osdLinkQuality = rxGetLinkQuality()  / 3.41;
 906         tfp_sprintf(element->buff, "%c%3d", SYM_LINK_QUALITY, osdLinkQuality);
 907     } else { // 0-9
 908         osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
 909         if (osdLinkQuality >= 10) {
 910             osdLinkQuality = 9;
 911         }
 912         tfp_sprintf(element->buff, "%c%1d", SYM_LINK_QUALITY, osdLinkQuality);
 913     }
 914 }
 915 #endif // USE_RX_LINK_QUALITY_INFO
 916
 917 #ifdef USE_BLACKBOX
 918 static void osdElementLogStatus(osdElementParms_t *element)
 919 {
 920     if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
 921         if (!isBlackboxDeviceWorking()) {
 922             tfp_sprintf(element->buff, "%c!", SYM_BBLOG);
 923         } else if (isBlackboxDeviceFull()) {
 924             tfp_sprintf(element->buff, "%c>", SYM_BBLOG);
 925         } else {
 926             int32_t logNumber = blackboxGetLogNumber();
 927             if (logNumber >= 0) {
 928                 tfp_sprintf(element->buff, "%c%d", SYM_BBLOG, logNumber);
 929             } else {
 930                 tfp_sprintf(element->buff, "%c", SYM_BBLOG);
 931             }
 932         }
 933     }
 934 }
 935 #endif // USE_BLACKBOX
 936
 937 static void osdElementMahDrawn(osdElementParms_t *element)
 938 {
 939     tfp_sprintf(element->buff, "%4d%c", getMAhDrawn(), SYM_MAH);
 940 }
 941
 942 static void osdElementMainBatteryUsage(osdElementParms_t *element)
 943 {
 944     // Set length of indicator bar
 945     #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
 946
 947     // Calculate constrained value
 948     const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
 949
 950     // Calculate mAh used progress
 951     const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
 952
 953     // Create empty battery indicator bar
 954     element->buff[0] = SYM_PB_START;
 955     for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
 956         element->buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
 957     }
 958     element->buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
 959     if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
 960         element->buff[1 + mAhUsedProgress] = SYM_PB_END;
 961     }
 962     element->buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
 963 }
 964
 965 static void osdElementMainBatteryVoltage(osdElementParms_t *element)
 966 {
 967     const int batteryVoltage = (getBatteryVoltage() + 5) / 10;
 968
 969     element->buff[0] = osdGetBatterySymbol(getBatteryAverageCellVoltage());
 970     if (batteryVoltage >= 100) {
 971         tfp_sprintf(element->buff + 1, "%d.%d%c", batteryVoltage / 10, batteryVoltage % 10, SYM_VOLT);
 972     } else {
 973         tfp_sprintf(element->buff + 1, "%d.%d0%c", batteryVoltage / 10, batteryVoltage % 10, SYM_VOLT);
 974     }
 975 }
 976
 977 static void osdElementMotorDiagnostics(osdElementParms_t *element)
 978 {
 979     int i = 0;
 980     const bool motorsRunning = areMotorsRunning();
 981     for (; i < getMotorCount(); i++) {
 982         if (motorsRunning) {
 983             element->buff[i] =  0x88 - scaleRange(motor[i], motorOutputLow, motorOutputHigh, 0, 8);
 984         } else {
 985             element->buff[i] =  0x88;
 986         }
 987     }
 988     element->buff[i] = '\0';
 989 }
 990
 991 static void osdElementNumericalHeading(osdElementParms_t *element)
 992 {
 993     const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 994     tfp_sprintf(element->buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
 995 }
 996
 997 #ifdef USE_VARIO
 998 static void osdElementNumericalVario(osdElementParms_t *element)
 999 {
1000     bool haveBaro = false;
1001     bool haveGps = false;
1002 #ifdef USE_BARO
1003     haveBaro = sensors(SENSOR_BARO);
1004 #endif // USE_BARO
1005 #ifdef USE_GPS
1006     haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
1007 #endif // USE_GPS
1008     if (haveBaro || haveGps) {
1009         const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
1010         const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SMALL_DOWN : SYM_ARROW_SMALL_UP;
1011         tfp_sprintf(element->buff, "%c%01d.%01d%c", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10), osdGetVarioToSelectedUnitSymbol());
1012     } else {
1013         // We use this symbol when we don't have a valid measure
1014         element->buff[0] = SYM_HYPHEN;
1015         element->buff[1] = '\0';
1016     }
1017 }
1018 #endif // USE_VARIO
1019
1020 static void osdElementPidRateProfile(osdElementParms_t *element)
1021 {
1022     tfp_sprintf(element->buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
1023 }
1024
1025 static void osdElementPidsPitch(osdElementParms_t *element)
1026 {
1027     osdFormatPID(element->buff, "PIT", &currentPidProfile->pid[PID_PITCH]);
1028 }
1029
1030 static void osdElementPidsRoll(osdElementParms_t *element)
1031 {
1032     osdFormatPID(element->buff, "ROL", &currentPidProfile->pid[PID_ROLL]);
1033 }
1034
1035 static void osdElementPidsYaw(osdElementParms_t *element)
1036 {
1037     osdFormatPID(element->buff, "YAW", &currentPidProfile->pid[PID_YAW]);
1038 }
1039
1040 static void osdElementPower(osdElementParms_t *element)
1041 {
1042     tfp_sprintf(element->buff, "%4dW", getAmperage() * getBatteryVoltage() / 10000);
1043 }
1044
1045 static void osdElementRcChannels(osdElementParms_t *element)
1046 {
1047     const uint8_t xpos = element->elemPosX;
1048     const uint8_t ypos = element->elemPosY;
1049
1050     for (int i = 0; i < OSD_RCCHANNELS_COUNT; i++) {
1051         if (osdConfig()->rcChannels[i] >= 0) {
1052             // Translate (1000, 2000) to (-1000, 1000)
1053             int data = scaleRange(rcData[osdConfig()->rcChannels[i]], PWM_RANGE_MIN, PWM_RANGE_MAX, -1000, 1000);
1054             // Opt for the simplest formatting for now.
1055             // Decimal notation can be added when tfp_sprintf supports float among fancy options.
1056             char fmtbuf[6];
1057             tfp_sprintf(fmtbuf, "%5d", data);
1058             displayWrite(element->osdDisplayPort, xpos, ypos + i, DISPLAYPORT_ATTR_NONE, fmtbuf);
1059         }
1060     }
1061
1062     element->drawElement = false;  // element already drawn
1063 }
1064
1065 static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
1066 {
1067     const int mAhDrawn = getMAhDrawn();
1068
1069     if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) {  // also handles the mAhDrawn == 0 condition
1070         tfp_sprintf(element->buff, "--:--");
1071     } else if (mAhDrawn > osdConfig()->cap_alarm) {
1072         tfp_sprintf(element->buff, "00:00");
1073     } else {
1074         const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)osdFlyTime) / mAhDrawn);
1075         osdFormatTime(element->buff, OSD_TIMER_PREC_SECOND, remaining_time);
1076     }
1077 }
1078
1079 static void osdElementRssi(osdElementParms_t *element)
1080 {
1081     uint16_t osdRssi = getRssi() * 100 / 1024; // change range
1082     if (osdRssi >= 100) {
1083         osdRssi = 99;
1084     }
1085
1086     tfp_sprintf(element->buff, "%c%2d", SYM_RSSI, osdRssi);
1087 }
1088
1089 #ifdef USE_RTC_TIME
1090 static void osdElementRtcTime(osdElementParms_t *element)
1091 {
1092     osdFormatRtcDateTime(&element->buff[0]);
1093 }
1094 #endif // USE_RTC_TIME
1095
1096 #ifdef USE_RX_RSSI_DBM
1097 static void osdElementRssiDbm(osdElementParms_t *element)
1098 {
1099     tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, getRssiDbm() * -1);
1100 }
1101 #endif // USE_RX_RSSI_DBM
1102
1103 #ifdef USE_OSD_STICK_OVERLAY
1104 static void osdBackgroundStickOverlay(osdElementParms_t *element)
1105 {
1106     const uint8_t xpos = element->elemPosX;
1107     const uint8_t ypos = element->elemPosY;
1108
1109     // Draw the axis first
1110     for (unsigned x = 0; x < OSD_STICK_OVERLAY_WIDTH; x++) {
1111         for (unsigned  y = 0; y < OSD_STICK_OVERLAY_HEIGHT; y++) {
1112             // draw the axes, vertical and horizonal
1113             if ((x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) && (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
1114                 displayWriteChar(element->osdDisplayPort, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_CENTER);
1115             } else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
1116                 displayWriteChar(element->osdDisplayPort, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_VERTICAL);
1117             } else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
1118                 displayWriteChar(element->osdDisplayPort, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_HORIZONTAL);
1119             }
1120         }
1121     }
1122
1123     element->drawElement = false;  // element already drawn
1124 }
1125
1126 static void osdElementStickOverlay(osdElementParms_t *element)
1127 {
1128     const uint8_t xpos = element->elemPosX;
1129     const uint8_t ypos = element->elemPosY;
1130
1131     // Now draw the cursor
1132     rc_alias_e vertical_channel, horizontal_channel;
1133
1134     if (element->item == OSD_STICK_OVERLAY_LEFT) {
1135         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_vertical;
1136         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_horizontal;
1137     } else {
1138         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_vertical;
1139         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_horizontal;
1140     }
1141
1142     const uint8_t cursorX = scaleRange(constrain(rcData[horizontal_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_WIDTH);
1143     const uint8_t cursorY = OSD_STICK_OVERLAY_VERTICAL_POSITIONS - 1 - scaleRange(constrain(rcData[vertical_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_VERTICAL_POSITIONS);
1144     const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (cursorY % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
1145
1146     displayWriteChar(element->osdDisplayPort, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_ATTR_NONE, cursor);
1147
1148     element->drawElement = false;  // element already drawn
1149 }
1150 #endif // USE_OSD_STICK_OVERLAY
1151
1152 static void osdElementThrottlePosition(osdElementParms_t *element)
1153 {
1154     tfp_sprintf(element->buff, "%c%3d", SYM_THR, calculateThrottlePercent());
1155 }
1156
1157 static void osdElementTimer(osdElementParms_t *element)
1158 {
1159     osdFormatTimer(element->buff, true, true, element->item - OSD_ITEM_TIMER_1);
1160 }
1161
1162 #ifdef USE_VTX_COMMON
1163 static void osdElementVtxChannel(osdElementParms_t *element)
1164 {
1165     const vtxDevice_t *vtxDevice = vtxCommonDevice();
1166     const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, vtxSettingsConfig()->band);
1167     const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, vtxSettingsConfig()->channel);
1168     unsigned vtxStatus = 0;
1169     uint8_t vtxPower = vtxSettingsConfig()->power;
1170     if (vtxDevice) {
1171         vtxCommonGetStatus(vtxDevice, &vtxStatus);
1172
1173         if (vtxSettingsConfig()->lowPowerDisarm) {
1174             vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
1175         }
1176     }
1177     const char *vtxPowerLabel = vtxCommonLookupPowerName(vtxDevice, vtxPower);
1178
1179     char vtxStatusIndicator = '\0';
1180     if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
1181         vtxStatusIndicator = 'D';
1182     } else if (vtxStatus & VTX_STATUS_PIT_MODE) {
1183         vtxStatusIndicator = 'P';
1184     }
1185
1186     if (vtxStatus & VTX_STATUS_LOCKED) {
1187         tfp_sprintf(element->buff, "-:-:-:L");
1188     } else if (vtxStatusIndicator) {
1189         tfp_sprintf(element->buff, "%c:%s:%s:%c", vtxBandLetter, vtxChannelName, vtxPowerLabel, vtxStatusIndicator);
1190     } else {
1191         tfp_sprintf(element->buff, "%c:%s:%s", vtxBandLetter, vtxChannelName, vtxPowerLabel);
1192     }
1193 }
1194 #endif // USE_VTX_COMMON
1195
1196 static void osdElementWarnings(osdElementParms_t *element)
1197 {
1198 #define OSD_WARNINGS_MAX_SIZE 12
1199 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
1200
1201     STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= OSD_ELEMENT_BUFFER_LENGTH, osd_warnings_size_exceeds_buffer_size);
1202
1203     const batteryState_e batteryState = getBatteryState();
1204     const timeUs_t currentTimeUs = micros();
1205
1206     static timeUs_t armingDisabledUpdateTimeUs;
1207     static unsigned armingDisabledDisplayIndex;
1208
1209     CLR_BLINK(OSD_WARNINGS);
1210
1211     // Cycle through the arming disabled reasons
1212     if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE)) {
1213         if (IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
1214             const armingDisableFlags_e armSwitchOnlyFlag = 1 << (ARMING_DISABLE_FLAGS_COUNT - 1);
1215             armingDisableFlags_e flags = getArmingDisableFlags();
1216
1217             // Remove the ARMSWITCH flag unless it's the only one
1218             if ((flags & armSwitchOnlyFlag) && (flags != armSwitchOnlyFlag)) {
1219                 flags -= armSwitchOnlyFlag;
1220             }
1221
1222             // Rotate to the next arming disabled reason after a 0.5 second time delay
1223             // or if the current flag is no longer set
1224             if ((currentTimeUs - armingDisabledUpdateTimeUs > 5e5) || !(flags & (1 << armingDisabledDisplayIndex))) {
1225                 if (armingDisabledUpdateTimeUs == 0) {
1226                     armingDisabledDisplayIndex = ARMING_DISABLE_FLAGS_COUNT - 1;
1227                 }
1228                 armingDisabledUpdateTimeUs = currentTimeUs;
1229
1230                 do {
1231                     if (++armingDisabledDisplayIndex >= ARMING_DISABLE_FLAGS_COUNT) {
1232                         armingDisabledDisplayIndex = 0;
1233                     }
1234                 } while (!(flags & (1 << armingDisabledDisplayIndex)));
1235             }
1236
1237             tfp_sprintf(element->buff, "%s", armingDisableFlagNames[armingDisabledDisplayIndex]);
1238             element->attr = DISPLAYPORT_ATTR_WARNING;
1239             return;
1240         } else {
1241             armingDisabledUpdateTimeUs = 0;
1242         }
1243     }
1244
1245 #ifdef USE_DSHOT
1246     if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
1247         int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - currentTimeUs) / 1e5;
1248         if (armingDelayTime < 0) {
1249             armingDelayTime = 0;
1250         }
1251         if (armingDelayTime >= (DSHOT_BEACON_GUARD_DELAY_US / 1e5 - 5)) {
1252             tfp_sprintf(element->buff, " BEACON ON"); // Display this message for the first 0.5 seconds
1253         } else {
1254             tfp_sprintf(element->buff, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
1255         }
1256         element->attr = DISPLAYPORT_ATTR_INFO;
1257         return;
1258     }
1259 #endif // USE_DSHOT
1260     if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
1261         tfp_sprintf(element->buff, "FAIL SAFE");
1262         element->attr = DISPLAYPORT_ATTR_CRITICAL;
1263         SET_BLINK(OSD_WARNINGS);
1264         return;
1265     }
1266
1267     // Warn when in flip over after crash mode
1268     if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashActive()) {
1269         tfp_sprintf(element->buff, "CRASH FLIP");
1270         element->attr = DISPLAYPORT_ATTR_INFO;
1271         return;
1272     }
1273
1274 #ifdef USE_LAUNCH_CONTROL
1275     // Warn when in launch control mode
1276     if (osdWarnGetState(OSD_WARNING_LAUNCH_CONTROL) && isLaunchControlActive()) {
1277 #ifdef USE_ACC
1278         if (sensors(SENSOR_ACC)) {
1279             const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
1280             tfp_sprintf(element->buff, "LAUNCH %d", pitchAngle);
1281         } else
1282 #endif // USE_ACC
1283         {
1284             tfp_sprintf(element->buff, "LAUNCH");
1285         }
1286
1287         // Blink the message if the throttle is within 10% of the launch setting
1288         if ( calculateThrottlePercent() >= MAX(currentPidProfile->launchControlThrottlePercent - 10, 0)) {
1289             SET_BLINK(OSD_WARNINGS);
1290         }
1291
1292         element->attr = DISPLAYPORT_ATTR_INFO;
1293         return;
1294     }
1295 #endif // USE_LAUNCH_CONTROL
1296
1297     // RSSI
1298     if (osdWarnGetState(OSD_WARNING_RSSI) && (getRssiPercent() < osdConfig()->rssi_alarm)) {
1299         tfp_sprintf(element->buff, "RSSI LOW");
1300         element->attr = DISPLAYPORT_ATTR_WARNING;
1301         SET_BLINK(OSD_WARNINGS);
1302         return;
1303     }
1304 #ifdef USE_RX_RSSI_DBM
1305     // rssi dbm
1306     if (osdWarnGetState(OSD_WARNING_RSSI_DBM) && (getRssiDbm() > osdConfig()->rssi_dbm_alarm)) {
1307         tfp_sprintf(element->buff, "RSSI DBM");
1308         element->attr = DISPLAYPORT_ATTR_WARNING;
1309         SET_BLINK(OSD_WARNINGS);
1310         return;
1311     }
1312 #endif // USE_RX_RSSI_DBM
1313
1314 #ifdef USE_RX_LINK_QUALITY_INFO
1315     // Link Quality
1316     if (osdWarnGetState(OSD_WARNING_LINK_QUALITY) && (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm)) {
1317         tfp_sprintf(element->buff, "LINK QUALITY");
1318         element->attr = DISPLAYPORT_ATTR_WARNING;
1319         SET_BLINK(OSD_WARNINGS);
1320         return;
1321     }
1322 #endif // USE_RX_LINK_QUALITY_INFO
1323
1324     if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
1325         tfp_sprintf(element->buff, " LAND NOW");
1326         element->attr = DISPLAYPORT_ATTR_CRITICAL;
1327         SET_BLINK(OSD_WARNINGS);
1328         return;
1329     }
1330
1331 #ifdef USE_GPS_RESCUE
1332     if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
1333        ARMING_FLAG(ARMED) &&
1334        gpsRescueIsConfigured() &&
1335        !gpsRescueIsDisabled() &&
1336        !gpsRescueIsAvailable()) {
1337         tfp_sprintf(element->buff, "RESCUE N/A");
1338         element->attr = DISPLAYPORT_ATTR_WARNING;
1339         SET_BLINK(OSD_WARNINGS);
1340         return;
1341     }
1342
1343     if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_DISABLED) &&
1344        ARMING_FLAG(ARMED) &&
1345        gpsRescueIsConfigured() &&
1346        gpsRescueIsDisabled()) {
1347
1348         statistic_t *stats = osdGetStats();
1349         if (cmpTimeUs(stats->armed_time, OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US) < 0) {
1350             tfp_sprintf(element->buff, "RESCUE OFF");
1351             element->attr = DISPLAYPORT_ATTR_WARNING;
1352             SET_BLINK(OSD_WARNINGS);
1353             return;
1354         }
1355     }
1356
1357 #endif // USE_GPS_RESCUE
1358
1359     // Show warning if in HEADFREE flight mode
1360     if (FLIGHT_MODE(HEADFREE_MODE)) {
1361         tfp_sprintf(element->buff, "HEADFREE");
1362         element->attr = DISPLAYPORT_ATTR_WARNING;
1363         SET_BLINK(OSD_WARNINGS);
1364         return;
1365     }
1366
1367 #ifdef USE_ADC_INTERNAL
1368     const int16_t coreTemperature = getCoreTemperatureCelsius();
1369     if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
1370         tfp_sprintf(element->buff, "CORE %c: %3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
1371         element->attr = DISPLAYPORT_ATTR_WARNING;
1372         SET_BLINK(OSD_WARNINGS);
1373         return;
1374     }
1375 #endif // USE_ADC_INTERNAL
1376
1377 #ifdef USE_ESC_SENSOR
1378     // Show warning if we lose motor output, the ESC is overheating or excessive current draw
1379     if (featureIsEnabled(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
1380         char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
1381         unsigned pos = 0;
1382
1383         const char *title = "ESC";
1384
1385         // center justify message
1386         while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
1387             escWarningMsg[pos++] = ' ';
1388         }
1389
1390         strcpy(escWarningMsg + pos, title);
1391         pos += strlen(title);
1392
1393         unsigned i = 0;
1394         unsigned escWarningCount = 0;
1395         while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
1396             escSensorData_t *escData = getEscSensorData(i);
1397             const char motorNumber = '1' + i;
1398             // if everything is OK just display motor number else R, T or C
1399             char warnFlag = motorNumber;
1400             if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
1401                 warnFlag = 'R';
1402             }
1403             if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
1404                 warnFlag = 'T';
1405             }
1406             if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
1407                 warnFlag = 'C';
1408             }
1409
1410             escWarningMsg[pos++] = warnFlag;
1411
1412             if (warnFlag != motorNumber) {
1413                 escWarningCount++;
1414             }
1415
1416             i++;
1417         }
1418
1419         escWarningMsg[pos] = '\0';
1420
1421         if (escWarningCount > 0) {
1422             tfp_sprintf(element->buff, "%s", escWarningMsg);
1423             element->attr = DISPLAYPORT_ATTR_WARNING;
1424             SET_BLINK(OSD_WARNINGS);
1425             return;
1426         }
1427     }
1428 #endif // USE_ESC_SENSOR
1429
1430     if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
1431         tfp_sprintf(element->buff, "LOW BATTERY");
1432         element->attr = DISPLAYPORT_ATTR_WARNING;
1433         SET_BLINK(OSD_WARNINGS);
1434         return;
1435     }
1436
1437 #ifdef USE_RC_SMOOTHING_FILTER
1438     // Show warning if rc smoothing hasn't initialized the filters
1439     if (osdWarnGetState(OSD_WARNING_RC_SMOOTHING) && ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
1440         tfp_sprintf(element->buff, "RCSMOOTHING");
1441         element->attr = DISPLAYPORT_ATTR_WARNING;
1442         SET_BLINK(OSD_WARNINGS);
1443         return;
1444     }
1445 #endif // USE_RC_SMOOTHING_FILTER
1446
1447     // Show warning if battery is not fresh
1448     if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !(ARMING_FLAG(ARMED) || ARMING_FLAG(WAS_EVER_ARMED)) && (getBatteryState() == BATTERY_OK)
1449           && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
1450         tfp_sprintf(element->buff, "BATT < FULL");
1451         element->attr = DISPLAYPORT_ATTR_INFO;
1452         return;
1453     }
1454
1455     // Visual beeper
1456     if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && osdGetVisualBeeperState()) {
1457         tfp_sprintf(element->buff, "  * * * *");
1458         element->attr = DISPLAYPORT_ATTR_INFO;
1459         return;
1460     }
1461
1462 }
1463
1464 // Define the order in which the elements are drawn.
1465 // Elements positioned later in the list will overlay the earlier
1466 // ones if their character positions overlap
1467 // Elements that need special runtime conditional processing should be added
1468 // to osdAddActiveElements()
1469
1470 static const uint8_t osdElementDisplayOrder[] = {
1471     OSD_MAIN_BATT_VOLTAGE,
1472     OSD_RSSI_VALUE,
1473     OSD_CROSSHAIRS,
1474     OSD_HORIZON_SIDEBARS,
1475     OSD_ITEM_TIMER_1,
1476     OSD_ITEM_TIMER_2,
1477     OSD_REMAINING_TIME_ESTIMATE,
1478     OSD_FLYMODE,
1479     OSD_THROTTLE_POS,
1480     OSD_VTX_CHANNEL,
1481     OSD_CURRENT_DRAW,
1482     OSD_MAH_DRAWN,
1483     OSD_CRAFT_NAME,
1484     OSD_ALTITUDE,
1485     OSD_ROLL_PIDS,
1486     OSD_PITCH_PIDS,
1487     OSD_YAW_PIDS,
1488     OSD_POWER,
1489     OSD_PIDRATE_PROFILE,
1490     OSD_WARNINGS,
1491     OSD_AVG_CELL_VOLTAGE,
1492     OSD_DEBUG,
1493     OSD_PITCH_ANGLE,
1494     OSD_ROLL_ANGLE,
1495     OSD_MAIN_BATT_USAGE,
1496     OSD_DISARMED,
1497     OSD_NUMERICAL_HEADING,
1498 #ifdef USE_VARIO
1499     OSD_NUMERICAL_VARIO,
1500 #endif
1501     OSD_COMPASS_BAR,
1502     OSD_ANTI_GRAVITY,
1503 #ifdef USE_BLACKBOX
1504     OSD_LOG_STATUS,
1505 #endif
1506     OSD_MOTOR_DIAG,
1507 #ifdef USE_ACC
1508     OSD_FLIP_ARROW,
1509 #endif
1510     OSD_DISPLAY_NAME,
1511 #ifdef USE_RTC_TIME
1512     OSD_RTC_DATETIME,
1513 #endif
1514 #ifdef USE_OSD_ADJUSTMENTS
1515     OSD_ADJUSTMENT_RANGE,
1516 #endif
1517 #ifdef USE_ADC_INTERNAL
1518     OSD_CORE_TEMPERATURE,
1519 #endif
1520 #ifdef USE_RX_LINK_QUALITY_INFO
1521     OSD_LINK_QUALITY,
1522 #endif
1523 #ifdef USE_RX_RSSI_DBM
1524     OSD_RSSI_DBM_VALUE,
1525 #endif
1526 #ifdef USE_OSD_STICK_OVERLAY
1527     OSD_STICK_OVERLAY_LEFT,
1528     OSD_STICK_OVERLAY_RIGHT,
1529 #endif
1530 #ifdef USE_PROFILE_NAMES
1531     OSD_RATE_PROFILE_NAME,
1532     OSD_PID_PROFILE_NAME,
1533 #endif
1534 #ifdef USE_OSD_PROFILES
1535     OSD_PROFILE_NAME,
1536 #endif
1537     OSD_RC_CHANNELS,
1538 };
1539
1540 // Define the mapping between the OSD element id and the function to draw it
1541
1542 const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
1543     [OSD_RSSI_VALUE]              = osdElementRssi,
1544     [OSD_MAIN_BATT_VOLTAGE]       = osdElementMainBatteryVoltage,
1545     [OSD_CROSSHAIRS]              = NULL,  // only has background
1546 #ifdef USE_ACC
1547     [OSD_ARTIFICIAL_HORIZON]      = osdElementArtificialHorizon,
1548 #endif
1549     [OSD_HORIZON_SIDEBARS]        = NULL,  // only has background
1550     [OSD_ITEM_TIMER_1]            = osdElementTimer,
1551     [OSD_ITEM_TIMER_2]            = osdElementTimer,
1552     [OSD_FLYMODE]                 = osdElementFlymode,
1553     [OSD_CRAFT_NAME]              = NULL,  // only has background
1554     [OSD_THROTTLE_POS]            = osdElementThrottlePosition,
1555 #ifdef USE_VTX_COMMON
1556     [OSD_VTX_CHANNEL]             = osdElementVtxChannel,
1557 #endif
1558     [OSD_CURRENT_DRAW]            = osdElementCurrentDraw,
1559     [OSD_MAH_DRAWN]               = osdElementMahDrawn,
1560 #ifdef USE_GPS
1561     [OSD_GPS_SPEED]               = osdElementGpsSpeed,
1562     [OSD_GPS_SATS]                = osdElementGpsSats,
1563 #endif
1564     [OSD_ALTITUDE]                = osdElementAltitude,
1565     [OSD_ROLL_PIDS]               = osdElementPidsRoll,
1566     [OSD_PITCH_PIDS]              = osdElementPidsPitch,
1567     [OSD_YAW_PIDS]                = osdElementPidsYaw,
1568     [OSD_POWER]                   = osdElementPower,
1569     [OSD_PIDRATE_PROFILE]         = osdElementPidRateProfile,
1570     [OSD_WARNINGS]                = osdElementWarnings,
1571     [OSD_AVG_CELL_VOLTAGE]        = osdElementAverageCellVoltage,
1572 #ifdef USE_GPS
1573     [OSD_GPS_LON]                 = osdElementGpsLongitude,
1574     [OSD_GPS_LAT]                 = osdElementGpsLatitude,
1575 #endif
1576     [OSD_DEBUG]                   = osdElementDebug,
1577 #ifdef USE_ACC
1578     [OSD_PITCH_ANGLE]             = osdElementAngleRollPitch,
1579     [OSD_ROLL_ANGLE]              = osdElementAngleRollPitch,
1580 #endif
1581     [OSD_MAIN_BATT_USAGE]         = osdElementMainBatteryUsage,
1582     [OSD_DISARMED]                = osdElementDisarmed,
1583 #ifdef USE_GPS
1584     [OSD_HOME_DIR]                = osdElementGpsHomeDirection,
1585     [OSD_HOME_DIST]               = osdElementGpsHomeDistance,
1586 #endif
1587     [OSD_NUMERICAL_HEADING]       = osdElementNumericalHeading,
1588 #ifdef USE_VARIO
1589     [OSD_NUMERICAL_VARIO]         = osdElementNumericalVario,
1590 #endif
1591     [OSD_COMPASS_BAR]             = osdElementCompassBar,
1592 #ifdef USE_ESC_SENSOR
1593     [OSD_ESC_TMP]                 = osdElementEscTemperature,
1594 #endif
1595 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1596     [OSD_ESC_RPM]                 = osdElementEscRpm,
1597 #endif
1598     [OSD_REMAINING_TIME_ESTIMATE] = osdElementRemainingTimeEstimate,
1599 #ifdef USE_RTC_TIME
1600     [OSD_RTC_DATETIME]            = osdElementRtcTime,
1601 #endif
1602 #ifdef USE_OSD_ADJUSTMENTS
1603     [OSD_ADJUSTMENT_RANGE]        = osdElementAdjustmentRange,
1604 #endif
1605 #ifdef USE_ADC_INTERNAL
1606     [OSD_CORE_TEMPERATURE]        = osdElementCoreTemperature,
1607 #endif
1608     [OSD_ANTI_GRAVITY]            = osdElementAntiGravity,
1609 #ifdef USE_ACC
1610     [OSD_G_FORCE]                 = osdElementGForce,
1611 #endif
1612     [OSD_MOTOR_DIAG]              = osdElementMotorDiagnostics,
1613 #ifdef USE_BLACKBOX
1614     [OSD_LOG_STATUS]              = osdElementLogStatus,
1615 #endif
1616 #ifdef USE_ACC
1617     [OSD_FLIP_ARROW]              = osdElementCrashFlipArrow,
1618 #endif
1619 #ifdef USE_RX_LINK_QUALITY_INFO
1620     [OSD_LINK_QUALITY]            = osdElementLinkQuality,
1621 #endif
1622 #ifdef USE_GPS
1623     [OSD_FLIGHT_DIST]             = osdElementGpsFlightDistance,
1624 #endif
1625 #ifdef USE_OSD_STICK_OVERLAY
1626     [OSD_STICK_OVERLAY_LEFT]      = osdElementStickOverlay,
1627     [OSD_STICK_OVERLAY_RIGHT]     = osdElementStickOverlay,
1628 #endif
1629     [OSD_DISPLAY_NAME]            = NULL,  // only has background
1630 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1631     [OSD_ESC_RPM_FREQ]            = osdElementEscRpmFreq,
1632 #endif
1633 #ifdef USE_PROFILE_NAMES
1634     [OSD_RATE_PROFILE_NAME]       = osdElementRateProfileName,
1635     [OSD_PID_PROFILE_NAME]        = osdElementPidProfileName,
1636 #endif
1637 #ifdef USE_OSD_PROFILES
1638     [OSD_PROFILE_NAME]            = osdElementOsdProfileName,
1639 #endif
1640 #ifdef USE_RX_RSSI_DBM
1641     [OSD_RSSI_DBM_VALUE]          = osdElementRssiDbm,
1642 #endif
1643     [OSD_RC_CHANNELS]             = osdElementRcChannels,
1644 };
1645
1646 // Define the mapping between the OSD element id and the function to draw its background (static part)
1647 // Only necessary to define the entries that actually have a background function
1648
1649 const osdElementDrawFn osdElementBackgroundFunction[OSD_ITEM_COUNT] = {
1650     [OSD_CROSSHAIRS]              = osdBackgroundCrosshairs,
1651     [OSD_HORIZON_SIDEBARS]        = osdBackgroundHorizonSidebars,
1652     [OSD_CRAFT_NAME]              = osdBackgroundCraftName,
1653 #ifdef USE_OSD_STICK_OVERLAY
1654     [OSD_STICK_OVERLAY_LEFT]      = osdBackgroundStickOverlay,
1655     [OSD_STICK_OVERLAY_RIGHT]     = osdBackgroundStickOverlay,
1656 #endif
1657     [OSD_DISPLAY_NAME]            = osdBackgroundDisplayName,
1658 };
1659
1660 static void osdAddActiveElement(osd_items_e element)
1661 {
1662     if (VISIBLE(osdConfig()->item_pos[element])) {
1663         activeOsdElementArray[activeOsdElementCount++] = element;
1664     }
1665 }
1666
1667 // Examine the elements and build a list of only the active (enabled)
1668 // ones to speed up rendering.
1669
1670 void osdAddActiveElements(void)
1671 {
1672     activeOsdElementCount = 0;
1673
1674 #ifdef USE_ACC
1675     if (sensors(SENSOR_ACC)) {
1676         osdAddActiveElement(OSD_ARTIFICIAL_HORIZON);
1677         osdAddActiveElement(OSD_G_FORCE);
1678     }
1679 #endif
1680
1681     for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
1682         osdAddActiveElement(osdElementDisplayOrder[i]);
1683     }
1684
1685 #ifdef USE_GPS
1686     if (sensors(SENSOR_GPS)) {
1687         osdAddActiveElement(OSD_GPS_SATS);
1688         osdAddActiveElement(OSD_GPS_SPEED);
1689         osdAddActiveElement(OSD_GPS_LAT);
1690         osdAddActiveElement(OSD_GPS_LON);
1691         osdAddActiveElement(OSD_HOME_DIST);
1692         osdAddActiveElement(OSD_HOME_DIR);
1693         osdAddActiveElement(OSD_FLIGHT_DIST);
1694     }
1695 #endif // GPS
1696 #ifdef USE_ESC_SENSOR
1697     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1698         osdAddActiveElement(OSD_ESC_TMP);
1699     }
1700 #endif
1701
1702 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1703     if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || (motorConfig()->dev.useDshotTelemetry)) {
1704         osdAddActiveElement(OSD_ESC_RPM);
1705         osdAddActiveElement(OSD_ESC_RPM_FREQ);
1706     }
1707 #endif
1708 }
1709
1710 static void osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
1711 {
1712     if (!osdElementDrawFunction[item]) {
1713         // Element has no drawing function
1714         return;
1715     }
1716     if (BLINK(item)) {
1717         return;
1718     }
1719
1720     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
1721     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
1722     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
1723
1724     osdElementParms_t element;
1725     element.item = item;
1726     element.elemPosX = elemPosX;
1727     element.elemPosY = elemPosY;
1728     element.buff = (char *)&buff;
1729     element.osdDisplayPort = osdDisplayPort;
1730     element.drawElement = true;
1731     element.attr = DISPLAYPORT_ATTR_NONE;
1732
1733     // Call the element drawing function
1734     osdElementDrawFunction[item](&element);
1735     if (element.drawElement) {
1736         displayWrite(osdDisplayPort, elemPosX, elemPosY, element.attr, buff);
1737     }
1738 }
1739
1740 static void osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_t item)
1741 {
1742     if (!osdElementBackgroundFunction[item]) {
1743         // Element has no background drawing function
1744         return;
1745     }
1746
1747     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
1748     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
1749     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
1750
1751     osdElementParms_t element;
1752     element.item = item;
1753     element.elemPosX = elemPosX;
1754     element.elemPosY = elemPosY;
1755     element.buff = (char *)&buff;
1756     element.osdDisplayPort = osdDisplayPort;
1757     element.drawElement = true;
1758
1759     // Call the element background drawing function
1760     osdElementBackgroundFunction[item](&element);
1761     if (element.drawElement) {
1762         displayWrite(osdDisplayPort, elemPosX, elemPosY, DISPLAYPORT_ATTR_NONE, buff);
1763     }
1764 }
1765
1766 void osdDrawActiveElements(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs)
1767 {
1768 #ifdef USE_GPS
1769     static bool lastGpsSensorState;
1770     // Handle the case that the GPS_SENSOR may be delayed in activation
1771     // or deactivate if communication is lost with the module.
1772     const bool currentGpsSensorState = sensors(SENSOR_GPS);
1773     if (lastGpsSensorState != currentGpsSensorState) {
1774         lastGpsSensorState = currentGpsSensorState;
1775         osdAnalyzeActiveElements();
1776     }
1777 #endif // USE_GPS
1778
1779     blinkState = (currentTimeUs / 200000) % 2;
1780
1781     for (unsigned i = 0; i < activeOsdElementCount; i++) {
1782         if (!backgroundLayerSupported) {
1783             // If the background layer isn't supported then we
1784             // have to draw the element's static layer as well.
1785             osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[i]);
1786         }
1787         osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[i]);
1788     }
1789 }
1790
1791 void osdDrawActiveElementsBackground(displayPort_t *osdDisplayPort)
1792 {
1793     if (backgroundLayerSupported) {
1794         displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_BACKGROUND);
1795         displayClearScreen(osdDisplayPort);
1796         for (unsigned i = 0; i < activeOsdElementCount; i++) {
1797             osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[i]);
1798         }
1799         displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND);
1800     }
1801 }
1802
1803 void osdElementsInit(bool backgroundLayerFlag)
1804 {
1805     backgroundLayerSupported = backgroundLayerFlag;
1806     activeOsdElementCount = 0;
1807 }
1808
1809 void osdResetAlarms(void)
1810 {
1811     memset(blinkBits, 0, sizeof(blinkBits));
1812 }
1813
1814 void osdUpdateAlarms(void)
1815 {
1816     // This is overdone?
1817
1818     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
1819
1820     if (getRssiPercent() < osdConfig()->rssi_alarm) {
1821         SET_BLINK(OSD_RSSI_VALUE);
1822     } else {
1823         CLR_BLINK(OSD_RSSI_VALUE);
1824     }
1825
1826 #ifdef USE_RX_LINK_QUALITY_INFO
1827     if (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm) {
1828         SET_BLINK(OSD_LINK_QUALITY);
1829     } else {
1830         CLR_BLINK(OSD_LINK_QUALITY);
1831     }
1832 #endif // USE_RX_LINK_QUALITY_INFO
1833
1834     if (getBatteryState() == BATTERY_OK) {
1835         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1836         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1837     } else {
1838         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
1839         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
1840     }
1841
1842 #ifdef USE_GPS
1843     if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5)
1844 #ifdef USE_GPS_RESCUE
1845             || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
1846 #endif
1847             ) {
1848         SET_BLINK(OSD_GPS_SATS);
1849     } else {
1850         CLR_BLINK(OSD_GPS_SATS);
1851     }
1852 #endif //USE_GPS
1853
1854     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1855         const uint16_t timer = osdConfig()->timers[i];
1856         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
1857         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
1858         if (alarmTime != 0 && time >= alarmTime) {
1859             SET_BLINK(OSD_ITEM_TIMER_1 + i);
1860         } else {
1861             CLR_BLINK(OSD_ITEM_TIMER_1 + i);
1862         }
1863     }
1864
1865     if (getMAhDrawn() >= osdConfig()->cap_alarm) {
1866         SET_BLINK(OSD_MAH_DRAWN);
1867         SET_BLINK(OSD_MAIN_BATT_USAGE);
1868         SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1869     } else {
1870         CLR_BLINK(OSD_MAH_DRAWN);
1871         CLR_BLINK(OSD_MAIN_BATT_USAGE);
1872         CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1873     }
1874
1875     if ((alt >= osdConfig()->alt_alarm) && ARMING_FLAG(ARMED)) {
1876         SET_BLINK(OSD_ALTITUDE);
1877     } else {
1878         CLR_BLINK(OSD_ALTITUDE);
1879     }
1880
1881 #ifdef USE_GPS
1882     if (sensors(SENSOR_GPS) && ARMING_FLAG(ARMED) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1883         if (osdConfig()->distance_alarm && GPS_distanceToHome >= osdConfig()->distance_alarm) {
1884             SET_BLINK(OSD_HOME_DIST);
1885         } else {
1886             CLR_BLINK(OSD_HOME_DIST);
1887         }
1888     } else {
1889         CLR_BLINK(OSD_HOME_DIST);;
1890     }
1891 #endif
1892
1893 #ifdef USE_ESC_SENSOR
1894     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1895         // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
1896         if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
1897             SET_BLINK(OSD_ESC_TMP);
1898         } else {
1899             CLR_BLINK(OSD_ESC_TMP);
1900         }
1901     }
1902 #endif
1903 }
1904
1905 #ifdef USE_ACC
1906 static bool osdElementIsActive(osd_items_e element)
1907 {
1908     for (unsigned i = 0; i < activeOsdElementCount; i++) {
1909         if (activeOsdElementArray[i] == element) {
1910             return true;
1911         }
1912     }
1913     return false;
1914 }
1915
1916 // Determine if any active elements need the ACC
1917 bool osdElementsNeedAccelerometer(void)
1918 {
1919     return osdElementIsActive(OSD_ARTIFICIAL_HORIZON) ||
1920            osdElementIsActive(OSD_PITCH_ANGLE) ||
1921            osdElementIsActive(OSD_ROLL_ANGLE) ||
1922            osdElementIsActive(OSD_G_FORCE) ||
1923            osdElementIsActive(OSD_FLIP_ARROW);
1924 }
1925
1926 #endif // USE_ACC
1927
1928 #endif // USE_OSD