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 osdBackgroundCraftName(osdElementParms_t *element)
 787 {
 788     if (strlen(pilotConfig()->name) == 0) {
 789         strcpy(element->buff, "CRAFT_NAME");
 790     } else {
 791         unsigned i;
 792         for (i = 0; i < MAX_NAME_LENGTH; i++) {
 793             if (pilotConfig()->name[i]) {
 794                 element->buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
 795             } else {
 796                 break;
 797             }
 798         }
 799         element->buff[i] = '\0';
 800     }
 801 }
 802
 803 #ifdef USE_ACC
 804 static void osdElementCrashFlipArrow(osdElementParms_t *element)
 805 {
 806     int rollAngle = attitude.values.roll / 10;
 807     const int pitchAngle = attitude.values.pitch / 10;
 808     if (abs(rollAngle) > 90) {
 809         rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
 810     }
 811
 812     if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) {
 813         if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
 814             if (pitchAngle > 0) {
 815                 if (rollAngle > 0) {
 816                     element->buff[0] = SYM_ARROW_WEST + 2;
 817                 } else {
 818                     element->buff[0] = SYM_ARROW_EAST - 2;
 819                 }
 820             } else {
 821                 if (rollAngle > 0) {
 822                     element->buff[0] = SYM_ARROW_WEST - 2;
 823                 } else {
 824                     element->buff[0] = SYM_ARROW_EAST + 2;
 825                 }
 826             }
 827         } else {
 828             if (abs(pitchAngle) > abs(rollAngle)) {
 829                 if (pitchAngle > 0) {
 830                     element->buff[0] = SYM_ARROW_SOUTH;
 831                 } else {
 832                     element->buff[0] = SYM_ARROW_NORTH;
 833                 }
 834             } else {
 835                 if (rollAngle > 0) {
 836                     element->buff[0] = SYM_ARROW_WEST;
 837                 } else {
 838                     element->buff[0] = SYM_ARROW_EAST;
 839                 }
 840             }
 841         }
 842         element->buff[1] = '\0';
 843     }
 844 }
 845 #endif // USE_ACC
 846
 847 static void osdElementCrosshairs(osdElementParms_t *element)
 848 {
 849     element->buff[0] = SYM_AH_CENTER_LINE;
 850     element->buff[1] = SYM_AH_CENTER;
 851     element->buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 852     element->buff[3] = 0;
 853 }
 854
 855 static void osdElementCurrentDraw(osdElementParms_t *element)
 856 {
 857     const float amperage = fabsf(getAmperage() / 100.0f);
 858     osdPrintFloat(element->buff, SYM_NONE, amperage, "%3u", 2, false, SYM_AMP);
 859 }
 860
 861 static void osdElementDebug(osdElementParms_t *element)
 862 {
 863     tfp_sprintf(element->buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
 864 }
 865
 866 static void osdElementDisarmed(osdElementParms_t *element)
 867 {
 868     if (!ARMING_FLAG(ARMED)) {
 869         tfp_sprintf(element->buff, "DISARMED");
 870     }
 871 }
 872
 873 static void osdBackgroundDisplayName(osdElementParms_t *element)
 874 {
 875     if (strlen(pilotConfig()->displayName) == 0) {
 876         strcpy(element->buff, "DISPLAY_NAME");
 877     } else {
 878         unsigned i;
 879         for (i = 0; i < MAX_NAME_LENGTH; i++) {
 880             if (pilotConfig()->displayName[i]) {
 881                 element->buff[i] = toupper((unsigned char)pilotConfig()->displayName[i]);
 882             } else {
 883                 break;
 884             }
 885         }
 886         element->buff[i] = '\0';
 887     }
 888 }
 889
 890 #ifdef USE_PERSISTENT_STATS
 891 static void osdElementTotalFlights(osdElementParms_t *element)
 892 {
 893     const int32_t total_flights = statsConfig()->stats_total_flights;
 894     tfp_sprintf(element->buff, "#%d", total_flights);
 895 }
 896 #endif
 897
 898 #ifdef USE_PROFILE_NAMES
 899 static void osdElementRateProfileName(osdElementParms_t *element)
 900 {
 901     if (strlen(currentControlRateProfile->profileName) == 0) {
 902         tfp_sprintf(element->buff, "RATE_%u", getCurrentControlRateProfileIndex() + 1);
 903     } else {
 904         unsigned i;
 905         for (i = 0; i < MAX_PROFILE_NAME_LENGTH; i++) {
 906             if (currentControlRateProfile->profileName[i]) {
 907                 element->buff[i] = toupper((unsigned char)currentControlRateProfile->profileName[i]);
 908             } else {
 909                 break;
 910             }
 911         }
 912         element->buff[i] = '\0';
 913     }
 914 }
 915
 916 static void osdElementPidProfileName(osdElementParms_t *element)
 917 {
 918     if (strlen(currentPidProfile->profileName) == 0) {
 919         tfp_sprintf(element->buff, "PID_%u", getCurrentPidProfileIndex() + 1);
 920     } else {
 921         unsigned i;
 922         for (i = 0; i < MAX_PROFILE_NAME_LENGTH; i++) {
 923             if (currentPidProfile->profileName[i]) {
 924                 element->buff[i] = toupper((unsigned char)currentPidProfile->profileName[i]);
 925             } else {
 926                 break;
 927             }
 928         }
 929         element->buff[i] = '\0';
 930     }
 931 }
 932 #endif
 933
 934 #ifdef USE_OSD_PROFILES
 935 static void osdElementOsdProfileName(osdElementParms_t *element)
 936 {
 937     uint8_t profileIndex = getCurrentOsdProfileIndex();
 938
 939     if (strlen(osdConfig()->profile[profileIndex - 1]) == 0) {
 940         tfp_sprintf(element->buff, "OSD_%u", profileIndex);
 941     } else {
 942         unsigned i;
 943         for (i = 0; i < OSD_PROFILE_NAME_LENGTH; i++) {
 944             if (osdConfig()->profile[profileIndex - 1][i]) {
 945                 element->buff[i] = toupper((unsigned char)osdConfig()->profile[profileIndex - 1][i]);
 946             } else {
 947                 break;
 948             }
 949         }
 950         element->buff[i] = '\0';
 951     }
 952 }
 953 #endif
 954
 955 #ifdef USE_ESC_SENSOR
 956 static void osdElementEscTemperature(osdElementParms_t *element)
 957 {
 958     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 959         tfp_sprintf(element->buff, "E%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(osdEscDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
 960     }
 961 }
 962 #endif // USE_ESC_SENSOR
 963
 964 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 965 static void osdElementEscRpm(osdElementParms_t *element)
 966 {
 967     renderOsdEscRpmOrFreq(&getEscRpm,element);
 968 }
 969
 970 static void osdElementEscRpmFreq(osdElementParms_t *element)
 971 {
 972     renderOsdEscRpmOrFreq(&getEscRpmFreq,element);
 973 }
 974 #endif
 975
 976 static void osdElementFlymode(osdElementParms_t *element)
 977 {
 978     // Note that flight mode display has precedence in what to display.
 979     //  1. FS
 980     //  2. GPS RESCUE
 981     //  3. ANGLE, HORIZON, ACRO TRAINER
 982     //  4. AIR
 983     //  5. ACRO
 984
 985     if (FLIGHT_MODE(FAILSAFE_MODE)) {
 986         strcpy(element->buff, "!FS!");
 987     } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 988         strcpy(element->buff, "RESC");
 989     } else if (FLIGHT_MODE(HEADFREE_MODE)) {
 990         strcpy(element->buff, "HEAD");
 991     } else if (FLIGHT_MODE(ANGLE_MODE)) {
 992         strcpy(element->buff, "ANGL");
 993     } else if (FLIGHT_MODE(HORIZON_MODE)) {
 994         strcpy(element->buff, "HOR ");
 995     } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
 996         strcpy(element->buff, "ATRN");
 997     } else if (airmodeIsEnabled()) {
 998         strcpy(element->buff, "AIR ");
 999     } else {
1000         strcpy(element->buff, "ACRO");
1001     }
1002 }
1003
1004 #ifdef USE_ACC
1005 static void osdElementGForce(osdElementParms_t *element)
1006 {
1007     osdPrintFloat(element->buff, SYM_NONE, osdGForce, "", 1, true, 'G');
1008 }
1009 #endif // USE_ACC
1010
1011 #ifdef USE_GPS
1012 static void osdElementGpsFlightDistance(osdElementParms_t *element)
1013 {
1014     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1015         osdFormatDistanceString(element->buff, GPS_distanceFlownInCm / 100, SYM_TOTAL_DISTANCE);
1016     } else {
1017         // We use this symbol when we don't have a FIX
1018         tfp_sprintf(element->buff, "%c%c", SYM_TOTAL_DISTANCE, SYM_HYPHEN);
1019     }
1020 }
1021
1022 static void osdElementGpsHomeDirection(osdElementParms_t *element)
1023 {
1024     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1025         if (GPS_distanceToHome > 0) {
1026             const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
1027             element->buff[0] = osdGetDirectionSymbolFromHeading(h);
1028         } else {
1029             element->buff[0] = SYM_OVER_HOME;
1030         }
1031
1032     } else {
1033         // We use this symbol when we don't have a FIX
1034         element->buff[0] = SYM_HYPHEN;
1035     }
1036
1037     element->buff[1] = 0;
1038 }
1039
1040 static void osdElementGpsHomeDistance(osdElementParms_t *element)
1041 {
1042     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1043         osdFormatDistanceString(element->buff, GPS_distanceToHome, SYM_HOMEFLAG);
1044     } else {
1045         element->buff[0] = SYM_HOMEFLAG;
1046         // We use this symbol when we don't have a FIX
1047         element->buff[1] = SYM_HYPHEN;
1048         element->buff[2] = '\0';
1049     }
1050 }
1051
1052 static void osdElementGpsCoordinate(osdElementParms_t *element)
1053 {
1054     const gpsCoordinateType_e coordinateType = (element->item == OSD_GPS_LON) ? GPS_LONGITUDE : GPS_LATITUDE;
1055     osdFormatCoordinate(element->buff, coordinateType, element->type);
1056     if (STATE(GPS_FIX_EVER) && !STATE(GPS_FIX)) {
1057         SET_BLINK(element->item); // blink if we had a fix but have since lost it
1058     } else {
1059         CLR_BLINK(element->item);
1060     }
1061 }
1062
1063 static void osdElementGpsSats(osdElementParms_t *element)
1064 {
1065     if (!gpsIsHealthy()) {
1066         tfp_sprintf(element->buff, "%c%cNC", SYM_SAT_L, SYM_SAT_R);
1067     } else {
1068         int pos = tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
1069         if (osdConfig()->gps_sats_show_hdop) { // add on the GPS module HDOP estimate
1070             element->buff[pos++] = ' ';
1071             osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.hdop / 100.0f, "", 1, true, SYM_NONE);
1072         }
1073     }
1074 }
1075
1076 static void osdElementGpsSpeed(osdElementParms_t *element)
1077 {
1078     if (STATE(GPS_FIX)) {
1079         tfp_sprintf(element->buff, "%c%3d%c", SYM_SPEED, osdGetSpeedToSelectedUnit(gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed), osdGetSpeedToSelectedUnitSymbol());
1080     } else {
1081         tfp_sprintf(element->buff, "%c%c%c", SYM_SPEED, SYM_HYPHEN, osdGetSpeedToSelectedUnitSymbol());
1082     }
1083 }
1084
1085 static void osdElementEfficiency(osdElementParms_t *element)
1086 {
1087     int efficiency = 0;
1088     if (sensors(SENSOR_GPS) && ARMING_FLAG(ARMED) && STATE(GPS_FIX) && gpsSol.groundSpeed >= EFFICIENCY_MINIMUM_SPEED_CM_S) {
1089         const float speed = (float)osdGetSpeedToSelectedUnit(gpsSol.groundSpeed);
1090         const float mAmperage = (float)getAmperage() * 10.f; // Current in mA
1091         efficiency = lrintf(pt1FilterApply(&batteryEfficiencyFilt, (mAmperage / speed)));
1092     }
1093
1094     const char unitSymbol = osdConfig()->units == UNIT_IMPERIAL ? SYM_MILES : SYM_KM;
1095     if (efficiency > 0 && efficiency <= 9999) {
1096         tfp_sprintf(element->buff, "%4d%c/%c", efficiency, SYM_MAH, unitSymbol);
1097     } else {
1098         tfp_sprintf(element->buff, "----%c/%c", SYM_MAH, unitSymbol);
1099     }
1100 }
1101 #endif // USE_GPS
1102
1103 static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
1104 {
1105     // Draw AH sides
1106     const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
1107     const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
1108     for (int y = -hudheight; y <= hudheight; y++) {
1109         osdDisplayWriteChar(element, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NONE, SYM_AH_DECORATION);
1110         osdDisplayWriteChar(element, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NONE, SYM_AH_DECORATION);
1111     }
1112
1113     // AH level indicators
1114     osdDisplayWriteChar(element, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_ATTR_NONE, SYM_AH_LEFT);
1115     osdDisplayWriteChar(element, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_ATTR_NONE, SYM_AH_RIGHT);
1116
1117     element->drawElement = false;  // element already drawn
1118 }
1119
1120 #ifdef USE_RX_LINK_QUALITY_INFO
1121 static void osdElementLinkQuality(osdElementParms_t *element)
1122 {
1123     uint16_t osdLinkQuality = 0;
1124     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-99
1125         osdLinkQuality = rxGetLinkQuality();
1126         const uint8_t osdRfMode = rxGetRfMode();
1127         tfp_sprintf(element->buff, "%c%1d:%2d", SYM_LINK_QUALITY, osdRfMode, osdLinkQuality);
1128     } else if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_GHST) { // 0-100
1129         osdLinkQuality = rxGetLinkQuality();
1130         tfp_sprintf(element->buff, "%c%2d", SYM_LINK_QUALITY, osdLinkQuality);
1131     } else { // 0-9
1132         osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
1133         if (osdLinkQuality >= 10) {
1134             osdLinkQuality = 9;
1135         }
1136         tfp_sprintf(element->buff, "%c%1d", SYM_LINK_QUALITY, osdLinkQuality);
1137     }
1138 }
1139 #endif // USE_RX_LINK_QUALITY_INFO
1140
1141 #ifdef USE_RX_LINK_UPLINK_POWER
1142 static void osdElementTxUplinkPower(osdElementParms_t *element)
1143 {
1144     const uint16_t osdUplinkTxPowerMw = rxGetUplinkTxPwrMw();
1145     if (osdUplinkTxPowerMw < 1000) {
1146         tfp_sprintf(element->buff, "%c%3dMW", SYM_RSSI, osdUplinkTxPowerMw);
1147     } else {
1148         osdPrintFloat(element->buff, SYM_RSSI, osdUplinkTxPowerMw / 1000.0f, "", 1, false, 'W');
1149     }
1150 }
1151 #endif // USE_RX_LINK_UPLINK_POWER
1152
1153 #ifdef USE_BLACKBOX
1154 static void osdElementLogStatus(osdElementParms_t *element)
1155 {
1156     if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
1157         if (!isBlackboxDeviceWorking()) {
1158             tfp_sprintf(element->buff, "%c!", SYM_BBLOG);
1159         } else if (isBlackboxDeviceFull()) {
1160             tfp_sprintf(element->buff, "%c>", SYM_BBLOG);
1161         } else {
1162             int32_t logNumber = blackboxGetLogNumber();
1163             if (logNumber >= 0) {
1164                 tfp_sprintf(element->buff, "%c%d", SYM_BBLOG, logNumber);
1165             } else {
1166                 tfp_sprintf(element->buff, "%c", SYM_BBLOG);
1167             }
1168         }
1169     }
1170 }
1171 #endif // USE_BLACKBOX
1172
1173 static void osdElementMahDrawn(osdElementParms_t *element)
1174 {
1175     tfp_sprintf(element->buff, "%4d%c", getMAhDrawn(), SYM_MAH);
1176 }
1177
1178 static void osdElementMainBatteryUsage(osdElementParms_t *element)
1179 {
1180     // Set length of indicator bar
1181     #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
1182
1183     const int usedCapacity = getMAhDrawn();
1184     int displayBasis = usedCapacity;
1185
1186     switch (element->type) {
1187     case OSD_ELEMENT_TYPE_3:  // mAh remaining percentage (counts down as battery is used)
1188         displayBasis = constrain(batteryConfig()->batteryCapacity - usedCapacity, 0, batteryConfig()->batteryCapacity);
1189         FALLTHROUGH;
1190
1191     case OSD_ELEMENT_TYPE_4:  // mAh used percentage (counts up as battery is used)
1192         {
1193             int displayPercent = 0;
1194             if (batteryConfig()->batteryCapacity) {
1195                 displayPercent = constrain(lrintf(100.0f * displayBasis / batteryConfig()->batteryCapacity), 0, 100);
1196             }
1197             tfp_sprintf(element->buff, "%c%d%%", SYM_MAH, displayPercent);
1198             break;
1199         }
1200
1201     case OSD_ELEMENT_TYPE_2:  // mAh used graphical progress bar (grows as battery is used)
1202         displayBasis = constrain(batteryConfig()->batteryCapacity - usedCapacity, 0, batteryConfig()->batteryCapacity);
1203         FALLTHROUGH;
1204
1205     case OSD_ELEMENT_TYPE_1:  // mAh remaining graphical progress bar (shrinks as battery is used)
1206     default:
1207         {
1208             uint8_t remainingCapacityBars = 0;
1209
1210             if (batteryConfig()->batteryCapacity) {
1211                 const float batteryRemaining = constrain(batteryConfig()->batteryCapacity - displayBasis, 0, batteryConfig()->batteryCapacity);
1212                 remainingCapacityBars = ceilf((batteryRemaining / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
1213             }
1214
1215             // Create empty battery indicator bar
1216             element->buff[0] = SYM_PB_START;
1217             for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
1218                 element->buff[i] = i <= remainingCapacityBars ? SYM_PB_FULL : SYM_PB_EMPTY;
1219             }
1220             element->buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
1221             if (remainingCapacityBars > 0 && remainingCapacityBars < MAIN_BATT_USAGE_STEPS) {
1222                 element->buff[1 + remainingCapacityBars] = SYM_PB_END;
1223             }
1224             element->buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
1225             break;
1226         }
1227     }
1228 }
1229
1230 static void osdElementMainBatteryVoltage(osdElementParms_t *element)
1231 {
1232     unsigned decimalPlaces;
1233     const float batteryVoltage = getBatteryVoltage() / 100.0f;
1234
1235     if (batteryVoltage >= 10) { // if voltage is 10v or more then display only 1 decimal place
1236         decimalPlaces = 1;
1237     } else {
1238         decimalPlaces = 2;
1239     }
1240     osdPrintFloat(element->buff, osdGetBatterySymbol(getBatteryAverageCellVoltage()), batteryVoltage, "", decimalPlaces, true, SYM_VOLT);
1241 }
1242
1243 static void osdElementMotorDiagnostics(osdElementParms_t *element)
1244 {
1245     int i = 0;
1246     const bool motorsRunning = areMotorsRunning();
1247     for (; i < getMotorCount(); i++) {
1248         if (motorsRunning) {
1249             element->buff[i] =  0x88 - scaleRange(motor[i], getMotorOutputLow(), getMotorOutputHigh(), 0, 8);
1250 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
1251             if (getEscRpm(i) < MOTOR_STOPPED_THRESHOLD_RPM) {
1252                 // Motor is not spinning properly. Mark as Stopped
1253                 element->buff[i] = 'S';
1254             }
1255 #endif
1256         } else {
1257             element->buff[i] =  0x88;
1258         }
1259     }
1260     element->buff[i] = '\0';
1261 }
1262
1263 static void osdElementNumericalHeading(osdElementParms_t *element)
1264 {
1265     const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
1266     tfp_sprintf(element->buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
1267 }
1268
1269 #ifdef USE_VARIO
1270 static void osdElementNumericalVario(osdElementParms_t *element)
1271 {
1272     bool haveBaro = false;
1273     bool haveGps = false;
1274 #ifdef USE_BARO
1275     haveBaro = sensors(SENSOR_BARO);
1276 #endif // USE_BARO
1277 #ifdef USE_GPS
1278     haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
1279 #endif // USE_GPS
1280     if (haveBaro || haveGps) {
1281         const float verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()) / 100.0f;
1282         const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SMALL_DOWN : SYM_ARROW_SMALL_UP;
1283         osdPrintFloat(element->buff, directionSymbol, fabsf(verticalSpeed), "", 1, true, osdGetVarioToSelectedUnitSymbol());
1284     } else {
1285         // We use this symbol when we don't have a valid measure
1286         element->buff[0] = SYM_HYPHEN;
1287         element->buff[1] = '\0';
1288     }
1289 }
1290 #endif // USE_VARIO
1291
1292 static void osdElementPidRateProfile(osdElementParms_t *element)
1293 {
1294     tfp_sprintf(element->buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
1295 }
1296
1297 static void osdElementPidsPitch(osdElementParms_t *element)
1298 {
1299     osdFormatPID(element->buff, "PIT", &currentPidProfile->pid[PID_PITCH]);
1300 }
1301
1302 static void osdElementPidsRoll(osdElementParms_t *element)
1303 {
1304     osdFormatPID(element->buff, "ROL", &currentPidProfile->pid[PID_ROLL]);
1305 }
1306
1307 static void osdElementPidsYaw(osdElementParms_t *element)
1308 {
1309     osdFormatPID(element->buff, "YAW", &currentPidProfile->pid[PID_YAW]);
1310 }
1311
1312 static void osdElementPower(osdElementParms_t *element)
1313 {
1314     tfp_sprintf(element->buff, "%4dW", getAmperage() * getBatteryVoltage() / 10000);
1315 }
1316
1317 static void osdElementRcChannels(osdElementParms_t *element)
1318 {
1319     const uint8_t xpos = element->elemPosX;
1320     const uint8_t ypos = element->elemPosY;
1321
1322     for (int i = 0; i < OSD_RCCHANNELS_COUNT; i++) {
1323         if (osdConfig()->rcChannels[i] >= 0) {
1324             // Translate (1000, 2000) to (-1000, 1000)
1325             int data = scaleRange(rcData[osdConfig()->rcChannels[i]], PWM_RANGE_MIN, PWM_RANGE_MAX, -1000, 1000);
1326             // Opt for the simplest formatting for now.
1327             // Decimal notation can be added when tfp_sprintf supports float among fancy options.
1328             char fmtbuf[6];
1329             tfp_sprintf(fmtbuf, "%5d", data);
1330             osdDisplayWrite(element, xpos, ypos + i, DISPLAYPORT_ATTR_NONE, fmtbuf);
1331         }
1332     }
1333
1334     element->drawElement = false;  // element already drawn
1335 }
1336
1337 static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
1338 {
1339     const int mAhDrawn = getMAhDrawn();
1340
1341     if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) {  // also handles the mAhDrawn == 0 condition
1342         tfp_sprintf(element->buff, "--:--");
1343     } else if (mAhDrawn > osdConfig()->cap_alarm) {
1344         tfp_sprintf(element->buff, "00:00");
1345     } else {
1346         const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)osdFlyTime) / mAhDrawn);
1347         osdFormatTime(element->buff, OSD_TIMER_PREC_SECOND, remaining_time);
1348     }
1349 }
1350
1351 static void osdElementRssi(osdElementParms_t *element)
1352 {
1353     uint16_t osdRssi = getRssi() * 100 / 1024; // change range
1354     if (osdRssi >= 100) {
1355         osdRssi = 99;
1356     }
1357
1358     tfp_sprintf(element->buff, "%c%2d", SYM_RSSI, osdRssi);
1359 }
1360
1361 #ifdef USE_RTC_TIME
1362 static void osdElementRtcTime(osdElementParms_t *element)
1363 {
1364     osdFormatRtcDateTime(&element->buff[0]);
1365 }
1366 #endif // USE_RTC_TIME
1367
1368 #ifdef USE_RX_RSSI_DBM
1369 static void osdElementRssiDbm(osdElementParms_t *element)
1370 {
1371     tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, getRssiDbm());
1372 }
1373 #endif // USE_RX_RSSI_DBM
1374
1375 #ifdef USE_OSD_STICK_OVERLAY
1376 static void osdBackgroundStickOverlay(osdElementParms_t *element)
1377 {
1378     const uint8_t xpos = element->elemPosX;
1379     const uint8_t ypos = element->elemPosY;
1380
1381     // Draw the axis first
1382     for (unsigned x = 0; x < OSD_STICK_OVERLAY_WIDTH; x++) {
1383         for (unsigned  y = 0; y < OSD_STICK_OVERLAY_HEIGHT; y++) {
1384             // draw the axes, vertical and horizonal
1385             if ((x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) && (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
1386                 osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_CENTER);
1387             } else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
1388                 osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_VERTICAL);
1389             } else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
1390                 osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_HORIZONTAL);
1391             }
1392         }
1393     }
1394
1395     element->drawElement = false;  // element already drawn
1396 }
1397
1398 static void osdElementStickOverlay(osdElementParms_t *element)
1399 {
1400     const uint8_t xpos = element->elemPosX;
1401     const uint8_t ypos = element->elemPosY;
1402
1403     // Now draw the cursor
1404     rc_alias_e vertical_channel, horizontal_channel;
1405
1406     if (element->item == OSD_STICK_OVERLAY_LEFT) {
1407         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_vertical;
1408         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_horizontal;
1409     } else {
1410         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_vertical;
1411         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_horizontal;
1412     }
1413
1414     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);
1415     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);
1416     const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (cursorY % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
1417
1418     osdDisplayWriteChar(element, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_ATTR_NONE, cursor);
1419
1420     element->drawElement = false;  // element already drawn
1421 }
1422 #endif // USE_OSD_STICK_OVERLAY
1423
1424 static void osdElementThrottlePosition(osdElementParms_t *element)
1425 {
1426     tfp_sprintf(element->buff, "%c%3d", SYM_THR, calculateThrottlePercent());
1427 }
1428
1429 static void osdElementTimer(osdElementParms_t *element)
1430 {
1431     osdFormatTimer(element->buff, true, true, element->item - OSD_ITEM_TIMER_1);
1432 }
1433
1434 #ifdef USE_VTX_COMMON
1435 static void osdElementVtxChannel(osdElementParms_t *element)
1436 {
1437     const vtxDevice_t *vtxDevice = vtxCommonDevice();
1438     const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, vtxSettingsConfig()->band);
1439     const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, vtxSettingsConfig()->channel);
1440     unsigned vtxStatus = 0;
1441     uint8_t vtxPower = vtxSettingsConfig()->power;
1442     if (vtxDevice) {
1443         vtxCommonGetStatus(vtxDevice, &vtxStatus);
1444
1445         if (vtxSettingsConfig()->lowPowerDisarm) {
1446             vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
1447         }
1448     }
1449     const char *vtxPowerLabel = vtxCommonLookupPowerName(vtxDevice, vtxPower);
1450
1451     char vtxStatusIndicator = '\0';
1452     if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
1453         vtxStatusIndicator = 'D';
1454     } else if (vtxStatus & VTX_STATUS_PIT_MODE) {
1455         vtxStatusIndicator = 'P';
1456     }
1457
1458     if (vtxStatus & VTX_STATUS_LOCKED) {
1459         tfp_sprintf(element->buff, "-:-:-:L");
1460     } else if (vtxStatusIndicator) {
1461         tfp_sprintf(element->buff, "%c:%s:%s:%c", vtxBandLetter, vtxChannelName, vtxPowerLabel, vtxStatusIndicator);
1462     } else {
1463         tfp_sprintf(element->buff, "%c:%s:%s", vtxBandLetter, vtxChannelName, vtxPowerLabel);
1464     }
1465 }
1466 #endif // USE_VTX_COMMON
1467
1468 static void osdElementWarnings(osdElementParms_t *element)
1469 {
1470     bool elementBlinking = false;
1471     renderOsdWarning(element->buff, &elementBlinking, &element->attr);
1472     if (elementBlinking) {
1473         SET_BLINK(OSD_WARNINGS);
1474     } else {
1475         CLR_BLINK(OSD_WARNINGS);
1476     }
1477 }
1478
1479 // Define the order in which the elements are drawn.
1480 // Elements positioned later in the list will overlay the earlier
1481 // ones if their character positions overlap
1482 // Elements that need special runtime conditional processing should be added
1483 // to osdAddActiveElements()
1484
1485 static const uint8_t osdElementDisplayOrder[] = {
1486     OSD_MAIN_BATT_VOLTAGE,
1487     OSD_RSSI_VALUE,
1488     OSD_CROSSHAIRS,
1489     OSD_HORIZON_SIDEBARS,
1490     OSD_UP_DOWN_REFERENCE,
1491     OSD_ITEM_TIMER_1,
1492     OSD_ITEM_TIMER_2,
1493     OSD_REMAINING_TIME_ESTIMATE,
1494     OSD_FLYMODE,
1495     OSD_THROTTLE_POS,
1496     OSD_VTX_CHANNEL,
1497     OSD_CURRENT_DRAW,
1498     OSD_MAH_DRAWN,
1499     OSD_CRAFT_NAME,
1500     OSD_ALTITUDE,
1501     OSD_ROLL_PIDS,
1502     OSD_PITCH_PIDS,
1503     OSD_YAW_PIDS,
1504     OSD_POWER,
1505     OSD_PIDRATE_PROFILE,
1506     OSD_WARNINGS,
1507     OSD_AVG_CELL_VOLTAGE,
1508     OSD_DEBUG,
1509     OSD_PITCH_ANGLE,
1510     OSD_ROLL_ANGLE,
1511     OSD_MAIN_BATT_USAGE,
1512     OSD_DISARMED,
1513     OSD_NUMERICAL_HEADING,
1514 #ifdef USE_VARIO
1515     OSD_NUMERICAL_VARIO,
1516 #endif
1517     OSD_COMPASS_BAR,
1518     OSD_ANTI_GRAVITY,
1519 #ifdef USE_BLACKBOX
1520     OSD_LOG_STATUS,
1521 #endif
1522     OSD_MOTOR_DIAG,
1523 #ifdef USE_ACC
1524     OSD_FLIP_ARROW,
1525 #endif
1526     OSD_DISPLAY_NAME,
1527 #ifdef USE_RTC_TIME
1528     OSD_RTC_DATETIME,
1529 #endif
1530 #ifdef USE_OSD_ADJUSTMENTS
1531     OSD_ADJUSTMENT_RANGE,
1532 #endif
1533 #ifdef USE_ADC_INTERNAL
1534     OSD_CORE_TEMPERATURE,
1535 #endif
1536 #ifdef USE_RX_LINK_QUALITY_INFO
1537     OSD_LINK_QUALITY,
1538 #endif
1539 #ifdef USE_RX_LINK_UPLINK_POWER
1540     OSD_TX_UPLINK_POWER,
1541 #endif
1542 #ifdef USE_RX_RSSI_DBM
1543     OSD_RSSI_DBM_VALUE,
1544 #endif
1545 #ifdef USE_OSD_STICK_OVERLAY
1546     OSD_STICK_OVERLAY_LEFT,
1547     OSD_STICK_OVERLAY_RIGHT,
1548 #endif
1549 #ifdef USE_PROFILE_NAMES
1550     OSD_RATE_PROFILE_NAME,
1551     OSD_PID_PROFILE_NAME,
1552 #endif
1553 #ifdef USE_OSD_PROFILES
1554     OSD_PROFILE_NAME,
1555 #endif
1556     OSD_RC_CHANNELS,
1557     OSD_CAMERA_FRAME,
1558 #ifdef USE_PERSISTENT_STATS
1559     OSD_TOTAL_FLIGHTS,
1560 #endif
1561 };
1562
1563 // Define the mapping between the OSD element id and the function to draw it
1564
1565 const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
1566     [OSD_CAMERA_FRAME]            = NULL,  // only has background. Added first so it's the lowest "layer" and doesn't cover other elements
1567     [OSD_RSSI_VALUE]              = osdElementRssi,
1568     [OSD_MAIN_BATT_VOLTAGE]       = osdElementMainBatteryVoltage,
1569     [OSD_CROSSHAIRS]              = osdElementCrosshairs,  // only has background, but needs to be over other elements (like artificial horizon)
1570 #ifdef USE_ACC
1571     [OSD_ARTIFICIAL_HORIZON]      = osdElementArtificialHorizon,
1572     [OSD_UP_DOWN_REFERENCE]       = osdElementUpDownReference,
1573 #endif
1574     [OSD_HORIZON_SIDEBARS]        = NULL,  // only has background
1575     [OSD_ITEM_TIMER_1]            = osdElementTimer,
1576     [OSD_ITEM_TIMER_2]            = osdElementTimer,
1577     [OSD_FLYMODE]                 = osdElementFlymode,
1578     [OSD_CRAFT_NAME]              = NULL,  // only has background
1579     [OSD_THROTTLE_POS]            = osdElementThrottlePosition,
1580 #ifdef USE_VTX_COMMON
1581     [OSD_VTX_CHANNEL]             = osdElementVtxChannel,
1582 #endif
1583     [OSD_CURRENT_DRAW]            = osdElementCurrentDraw,
1584     [OSD_MAH_DRAWN]               = osdElementMahDrawn,
1585 #ifdef USE_GPS
1586     [OSD_GPS_SPEED]               = osdElementGpsSpeed,
1587     [OSD_GPS_SATS]                = osdElementGpsSats,
1588 #endif
1589     [OSD_ALTITUDE]                = osdElementAltitude,
1590     [OSD_ROLL_PIDS]               = osdElementPidsRoll,
1591     [OSD_PITCH_PIDS]              = osdElementPidsPitch,
1592     [OSD_YAW_PIDS]                = osdElementPidsYaw,
1593     [OSD_POWER]                   = osdElementPower,
1594     [OSD_PIDRATE_PROFILE]         = osdElementPidRateProfile,
1595     [OSD_WARNINGS]                = osdElementWarnings,
1596     [OSD_AVG_CELL_VOLTAGE]        = osdElementAverageCellVoltage,
1597 #ifdef USE_GPS
1598     [OSD_GPS_LON]                 = osdElementGpsCoordinate,
1599     [OSD_GPS_LAT]                 = osdElementGpsCoordinate,
1600 #endif
1601     [OSD_DEBUG]                   = osdElementDebug,
1602 #ifdef USE_ACC
1603     [OSD_PITCH_ANGLE]             = osdElementAngleRollPitch,
1604     [OSD_ROLL_ANGLE]              = osdElementAngleRollPitch,
1605 #endif
1606     [OSD_MAIN_BATT_USAGE]         = osdElementMainBatteryUsage,
1607     [OSD_DISARMED]                = osdElementDisarmed,
1608 #ifdef USE_GPS
1609     [OSD_HOME_DIR]                = osdElementGpsHomeDirection,
1610     [OSD_HOME_DIST]               = osdElementGpsHomeDistance,
1611 #endif
1612     [OSD_NUMERICAL_HEADING]       = osdElementNumericalHeading,
1613 #ifdef USE_VARIO
1614     [OSD_NUMERICAL_VARIO]         = osdElementNumericalVario,
1615 #endif
1616     [OSD_COMPASS_BAR]             = osdElementCompassBar,
1617 #ifdef USE_ESC_SENSOR
1618     [OSD_ESC_TMP]                 = osdElementEscTemperature,
1619 #endif
1620 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1621     [OSD_ESC_RPM]                 = osdElementEscRpm,
1622 #endif
1623     [OSD_REMAINING_TIME_ESTIMATE] = osdElementRemainingTimeEstimate,
1624 #ifdef USE_RTC_TIME
1625     [OSD_RTC_DATETIME]            = osdElementRtcTime,
1626 #endif
1627 #ifdef USE_OSD_ADJUSTMENTS
1628     [OSD_ADJUSTMENT_RANGE]        = osdElementAdjustmentRange,
1629 #endif
1630 #ifdef USE_ADC_INTERNAL
1631     [OSD_CORE_TEMPERATURE]        = osdElementCoreTemperature,
1632 #endif
1633     [OSD_ANTI_GRAVITY]            = osdElementAntiGravity,
1634 #ifdef USE_ACC
1635     [OSD_G_FORCE]                 = osdElementGForce,
1636 #endif
1637     [OSD_MOTOR_DIAG]              = osdElementMotorDiagnostics,
1638 #ifdef USE_BLACKBOX
1639     [OSD_LOG_STATUS]              = osdElementLogStatus,
1640 #endif
1641 #ifdef USE_ACC
1642     [OSD_FLIP_ARROW]              = osdElementCrashFlipArrow,
1643 #endif
1644 #ifdef USE_RX_LINK_QUALITY_INFO
1645     [OSD_LINK_QUALITY]            = osdElementLinkQuality,
1646 #endif
1647 #ifdef USE_RX_LINK_UPLINK_POWER
1648     [OSD_TX_UPLINK_POWER]         = osdElementTxUplinkPower,
1649 #endif
1650 #ifdef USE_GPS
1651     [OSD_FLIGHT_DIST]             = osdElementGpsFlightDistance,
1652 #endif
1653 #ifdef USE_OSD_STICK_OVERLAY
1654     [OSD_STICK_OVERLAY_LEFT]      = osdElementStickOverlay,
1655     [OSD_STICK_OVERLAY_RIGHT]     = osdElementStickOverlay,
1656 #endif
1657     [OSD_DISPLAY_NAME]            = NULL,  // only has background
1658 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1659     [OSD_ESC_RPM_FREQ]            = osdElementEscRpmFreq,
1660 #endif
1661 #ifdef USE_PROFILE_NAMES
1662     [OSD_RATE_PROFILE_NAME]       = osdElementRateProfileName,
1663     [OSD_PID_PROFILE_NAME]        = osdElementPidProfileName,
1664 #endif
1665 #ifdef USE_OSD_PROFILES
1666     [OSD_PROFILE_NAME]            = osdElementOsdProfileName,
1667 #endif
1668 #ifdef USE_RX_RSSI_DBM
1669     [OSD_RSSI_DBM_VALUE]          = osdElementRssiDbm,
1670 #endif
1671     [OSD_RC_CHANNELS]             = osdElementRcChannels,
1672 #ifdef USE_GPS
1673     [OSD_EFFICIENCY]              = osdElementEfficiency,
1674 #endif
1675 #ifdef USE_PERSISTENT_STATS
1676     [OSD_TOTAL_FLIGHTS]           = osdElementTotalFlights,
1677 #endif
1678 };
1679
1680 // Define the mapping between the OSD element id and the function to draw its background (static part)
1681 // Only necessary to define the entries that actually have a background function
1682
1683 const osdElementDrawFn osdElementBackgroundFunction[OSD_ITEM_COUNT] = {
1684     [OSD_CAMERA_FRAME]            = osdBackgroundCameraFrame,
1685     [OSD_HORIZON_SIDEBARS]        = osdBackgroundHorizonSidebars,
1686     [OSD_CRAFT_NAME]              = osdBackgroundCraftName,
1687 #ifdef USE_OSD_STICK_OVERLAY
1688     [OSD_STICK_OVERLAY_LEFT]      = osdBackgroundStickOverlay,
1689     [OSD_STICK_OVERLAY_RIGHT]     = osdBackgroundStickOverlay,
1690 #endif
1691     [OSD_DISPLAY_NAME]            = osdBackgroundDisplayName,
1692 };
1693
1694 static void osdAddActiveElement(osd_items_e element)
1695 {
1696     if (VISIBLE(osdElementConfig()->item_pos[element])) {
1697         activeOsdElementArray[activeOsdElementCount++] = element;
1698     }
1699 }
1700
1701 // Examine the elements and build a list of only the active (enabled)
1702 // ones to speed up rendering.
1703
1704 void osdAddActiveElements(void)
1705 {
1706     activeOsdElementCount = 0;
1707
1708 #ifdef USE_ACC
1709     if (sensors(SENSOR_ACC)) {
1710         osdAddActiveElement(OSD_ARTIFICIAL_HORIZON);
1711         osdAddActiveElement(OSD_G_FORCE);
1712         osdAddActiveElement(OSD_UP_DOWN_REFERENCE);
1713     }
1714 #endif
1715
1716     for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
1717         osdAddActiveElement(osdElementDisplayOrder[i]);
1718     }
1719
1720 #ifdef USE_GPS
1721     if (sensors(SENSOR_GPS)) {
1722         osdAddActiveElement(OSD_GPS_SATS);
1723         osdAddActiveElement(OSD_GPS_SPEED);
1724         osdAddActiveElement(OSD_GPS_LAT);
1725         osdAddActiveElement(OSD_GPS_LON);
1726         osdAddActiveElement(OSD_HOME_DIST);
1727         osdAddActiveElement(OSD_HOME_DIR);
1728         osdAddActiveElement(OSD_FLIGHT_DIST);
1729         osdAddActiveElement(OSD_EFFICIENCY);
1730     }
1731 #endif // GPS
1732 #ifdef USE_ESC_SENSOR
1733     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1734         osdAddActiveElement(OSD_ESC_TMP);
1735     }
1736 #endif
1737
1738 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1739     if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || (motorConfig()->dev.useDshotTelemetry)) {
1740         osdAddActiveElement(OSD_ESC_RPM);
1741         osdAddActiveElement(OSD_ESC_RPM_FREQ);
1742     }
1743 #endif
1744
1745 #ifdef USE_PERSISTENT_STATS
1746     osdAddActiveElement(OSD_TOTAL_FLIGHTS);
1747 #endif
1748 }
1749
1750 static void osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
1751 {
1752     if (!osdElementDrawFunction[item]) {
1753         // Element has no drawing function
1754         return;
1755     }
1756     if (!osdDisplayPort->useDeviceBlink && BLINK(item)) {
1757         return;
1758     }
1759
1760     uint8_t elemPosX = OSD_X(osdElementConfig()->item_pos[item]);
1761     uint8_t elemPosY = OSD_Y(osdElementConfig()->item_pos[item]);
1762     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
1763
1764     osdElementParms_t element;
1765     element.item = item;
1766     element.elemPosX = elemPosX;
1767     element.elemPosY = elemPosY;
1768     element.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
1769     element.buff = (char *)&buff;
1770     element.osdDisplayPort = osdDisplayPort;
1771     element.drawElement = true;
1772     element.attr = DISPLAYPORT_ATTR_NONE;
1773
1774     // Call the element drawing function
1775     osdElementDrawFunction[item](&element);
1776     if (element.drawElement) {
1777         osdDisplayWrite(&element, elemPosX, elemPosY, element.attr, buff);
1778     }
1779 }
1780
1781 static void osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_t item)
1782 {
1783     if (!osdElementBackgroundFunction[item]) {
1784         // Element has no background drawing function
1785         return;
1786     }
1787
1788     uint8_t elemPosX = OSD_X(osdElementConfig()->item_pos[item]);
1789     uint8_t elemPosY = OSD_Y(osdElementConfig()->item_pos[item]);
1790     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
1791
1792     osdElementParms_t element;
1793     element.item = item;
1794     element.elemPosX = elemPosX;
1795     element.elemPosY = elemPosY;
1796     element.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
1797     element.buff = (char *)&buff;
1798     element.osdDisplayPort = osdDisplayPort;
1799     element.drawElement = true;
1800
1801     // Call the element background drawing function
1802     osdElementBackgroundFunction[item](&element);
1803     if (element.drawElement) {
1804         osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_ATTR_NONE, buff);
1805     }
1806 }
1807
1808 static uint8_t activeElement = 0;
1809
1810 uint8_t osdGetActiveElement()
1811 {
1812     return activeElement;
1813 }
1814
1815 uint8_t osdGetActiveElementCount()
1816 {
1817     return activeOsdElementCount;
1818 }
1819
1820 // Return true if there are more elements to draw
1821 bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs)
1822 {
1823     UNUSED(currentTimeUs);
1824     bool retval = true;
1825
1826     if (activeElement >= activeOsdElementCount) {
1827         return false;
1828     }
1829
1830     if (!backgroundLayerSupported) {
1831         // If the background layer isn't supported then we
1832         // have to draw the element's static layer as well.
1833         osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[activeElement]);
1834     }
1835
1836     osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[activeElement]);
1837
1838     if (++activeElement >= activeOsdElementCount) {
1839         activeElement = 0;
1840         retval = false;
1841     }
1842
1843     return retval;
1844 }
1845
1846 void osdDrawActiveElementsBackground(displayPort_t *osdDisplayPort)
1847 {
1848     if (backgroundLayerSupported) {
1849         displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_BACKGROUND);
1850         displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_WAIT);
1851         for (unsigned i = 0; i < activeOsdElementCount; i++) {
1852             osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[i]);
1853         }
1854         displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND);
1855     }
1856 }
1857
1858 void osdElementsInit(bool backgroundLayerFlag)
1859 {
1860     backgroundLayerSupported = backgroundLayerFlag;
1861     activeOsdElementCount = 0;
1862     pt1FilterInit(&batteryEfficiencyFilt, pt1FilterGain(EFFICIENCY_CUTOFF_HZ, 1.0f / osdConfig()->framerate_hz));
1863 }
1864
1865 void osdSyncBlink() {
1866     static int blinkCount = 0;
1867
1868     // If the OSD blink is due a transition, do so
1869     // Task runs at osdConfig()->framerate_hz Hz, so this will cycle at 2Hz
1870     if (++blinkCount == ((osdConfig()->framerate_hz / OSD_BLINK_FREQUENCY_HZ) / 2)) {
1871         blinkCount = 0;
1872         blinkState = !blinkState;
1873     }
1874 }
1875
1876 void osdResetAlarms(void)
1877 {
1878     memset(blinkBits, 0, sizeof(blinkBits));
1879 }
1880
1881 void osdUpdateAlarms(void)
1882 {
1883     // This is overdone?
1884
1885     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
1886
1887     if (getRssiPercent() < osdConfig()->rssi_alarm) {
1888         SET_BLINK(OSD_RSSI_VALUE);
1889     } else {
1890         CLR_BLINK(OSD_RSSI_VALUE);
1891     }
1892
1893 #ifdef USE_RX_LINK_QUALITY_INFO
1894     if (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm) {
1895         SET_BLINK(OSD_LINK_QUALITY);
1896     } else {
1897         CLR_BLINK(OSD_LINK_QUALITY);
1898     }
1899 #endif // USE_RX_LINK_QUALITY_INFO
1900
1901     if (getBatteryState() == BATTERY_OK) {
1902         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1903         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1904     } else {
1905         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
1906         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
1907     }
1908
1909 #ifdef USE_GPS
1910     if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5)
1911 #ifdef USE_GPS_RESCUE
1912             || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
1913 #endif
1914             ) {
1915         SET_BLINK(OSD_GPS_SATS);
1916     } else {
1917         CLR_BLINK(OSD_GPS_SATS);
1918     }
1919 #endif //USE_GPS
1920
1921     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1922         const uint16_t timer = osdConfig()->timers[i];
1923         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
1924         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
1925         if (alarmTime != 0 && time >= alarmTime) {
1926             SET_BLINK(OSD_ITEM_TIMER_1 + i);
1927         } else {
1928             CLR_BLINK(OSD_ITEM_TIMER_1 + i);
1929         }
1930     }
1931
1932     if (getMAhDrawn() >= osdConfig()->cap_alarm) {
1933         SET_BLINK(OSD_MAH_DRAWN);
1934         SET_BLINK(OSD_MAIN_BATT_USAGE);
1935         SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1936     } else {
1937         CLR_BLINK(OSD_MAH_DRAWN);
1938         CLR_BLINK(OSD_MAIN_BATT_USAGE);
1939         CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1940     }
1941
1942     if ((alt >= osdConfig()->alt_alarm) && ARMING_FLAG(ARMED)) {
1943         SET_BLINK(OSD_ALTITUDE);
1944     } else {
1945         CLR_BLINK(OSD_ALTITUDE);
1946     }
1947
1948 #ifdef USE_GPS
1949     if (sensors(SENSOR_GPS) && ARMING_FLAG(ARMED) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1950         if (osdConfig()->distance_alarm && GPS_distanceToHome >= osdConfig()->distance_alarm) {
1951             SET_BLINK(OSD_HOME_DIST);
1952         } else {
1953             CLR_BLINK(OSD_HOME_DIST);
1954         }
1955     } else {
1956         CLR_BLINK(OSD_HOME_DIST);;
1957     }
1958 #endif
1959
1960 #ifdef USE_ESC_SENSOR
1961     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1962         // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
1963         if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
1964             SET_BLINK(OSD_ESC_TMP);
1965         } else {
1966             CLR_BLINK(OSD_ESC_TMP);
1967         }
1968     }
1969 #endif
1970 }
1971
1972 #ifdef USE_ACC
1973 static bool osdElementIsActive(osd_items_e element)
1974 {
1975     for (unsigned i = 0; i < activeOsdElementCount; i++) {
1976         if (activeOsdElementArray[i] == element) {
1977             return true;
1978         }
1979     }
1980     return false;
1981 }
1982
1983 // Determine if any active elements need the ACC
1984 bool osdElementsNeedAccelerometer(void)
1985 {
1986     return osdElementIsActive(OSD_ARTIFICIAL_HORIZON) ||
1987            osdElementIsActive(OSD_PITCH_ANGLE) ||
1988            osdElementIsActive(OSD_ROLL_ANGLE) ||
1989            osdElementIsActive(OSD_G_FORCE) ||
1990            osdElementIsActive(OSD_FLIP_ARROW) ||
1991            osdElementIsActive(OSD_UP_DOWN_REFERENCE);
1992 }
1993
1994 #endif // USE_ACC
1995
1996 #endif // USE_OSD