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