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