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     VTX_CHANNEL
 101         type 1: Contains Band:Channel:Power:Pit
 102         type 2: Contains only Power
 103 */
 104
 105 #include <stdbool.h>
 106 #include <stdint.h>
 107 #include <stdlib.h>
 108 #include <string.h>
 109 #include <ctype.h>
 110 #include <math.h>
 111
 112 #include "platform.h"
 113
 114 #ifdef USE_OSD
 115
 116 #include "blackbox/blackbox.h"
 117 #include "blackbox/blackbox_io.h"
 118
 119 #include "build/build_config.h"
 120 #include "build/debug.h"
 121
 122 #include "cli/settings.h"
 123
 124 #include "common/axis.h"
 125 #include "common/maths.h"
 126 #include "common/printf.h"
 127 #include "common/typeconversion.h"
 128 #include "common/utils.h"
 129 #include "common/unit.h"
 130 #include "common/filter.h"
 131
 132 #include "config/config.h"
 133 #include "config/feature.h"
 134
 135 #include "drivers/display.h"
 136 #include "drivers/dshot.h"
 137 #include "drivers/osd_symbols.h"
 138 #include "drivers/time.h"
 139 #include "drivers/vtx_common.h"
 140
 141 #include "drivers/pinio.h"
 142
 143 #include "fc/controlrate_profile.h"
 144 #include "fc/core.h"
 145 #include "fc/gps_lap_timer.h"
 146 #include "fc/rc_adjustments.h"
 147 #include "fc/rc_controls.h"
 148 #include "fc/runtime_config.h"
 149
 150 #include "flight/gps_rescue.h"
 151 #include "flight/position.h"
 152 #include "flight/imu.h"
 153 #include "flight/mixer.h"
 154 #include "flight/pid.h"
 155
 156 #include "io/gps.h"
 157 #include "io/vtx.h"
 158
 159 #include "osd/osd.h"
 160 #include "osd/osd_elements.h"
 161 #include "osd/osd_warnings.h"
 162
 163 #include "pg/motor.h"
 164 #include "pg/pilot.h"
 165 #include "pg/stats.h"
 166
 167 #include "rx/rx.h"
 168
 169 #include "sensors/adcinternal.h"
 170 #include "sensors/barometer.h"
 171 #include "sensors/battery.h"
 172 #include "sensors/sensors.h"
 173
 174 #ifdef USE_GPS_PLUS_CODES
 175 // located in lib/main/google/olc
 176 #include "olc.h"
 177 #endif
 178
 179 #define AH_SYMBOL_COUNT 9
 180 #define AH_SIDEBAR_WIDTH_POS 7
 181 #define AH_SIDEBAR_HEIGHT_POS 3
 182
 183 // Stick overlay size
 184 #define OSD_STICK_OVERLAY_WIDTH 7
 185 #define OSD_STICK_OVERLAY_HEIGHT 5
 186 #define OSD_STICK_OVERLAY_SPRITE_HEIGHT 3
 187 #define OSD_STICK_OVERLAY_VERTICAL_POSITIONS (OSD_STICK_OVERLAY_HEIGHT * OSD_STICK_OVERLAY_SPRITE_HEIGHT)
 188
 189 #define FULL_CIRCLE 360
 190 #define EFFICIENCY_MINIMUM_SPEED_CM_S 100
 191 #define EFFICIENCY_CUTOFF_HZ 0.5f
 192
 193 static pt1Filter_t batteryEfficiencyFilt;
 194
 195 #define MOTOR_STOPPED_THRESHOLD_RPM 1000
 196
 197 #define SINE_25_DEG 0.422618261740699f
 198
 199 #ifdef USE_OSD_STICK_OVERLAY
 200 typedef struct radioControls_s {
 201     uint8_t left_vertical;
 202     uint8_t left_horizontal;
 203     uint8_t right_vertical;
 204     uint8_t right_horizontal;
 205 } radioControls_t;
 206
 207 static const radioControls_t radioModes[4] = {
 208     { PITCH,    YAW,    THROTTLE,   ROLL }, // Mode 1
 209     { THROTTLE, YAW,    PITCH,      ROLL }, // Mode 2
 210     { PITCH,    ROLL,   THROTTLE,   YAW  }, // Mode 3
 211     { THROTTLE, ROLL,   PITCH,      YAW  }, // Mode 4
 212 };
 213 #endif
 214
 215 static const char compassBar[] = {
 216   SYM_HEADING_W,
 217   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 218   SYM_HEADING_N,
 219   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 220   SYM_HEADING_E,
 221   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 222   SYM_HEADING_S,
 223   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 224   SYM_HEADING_W,
 225   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 226   SYM_HEADING_N,
 227   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE
 228 };
 229
 230 static unsigned activeOsdElementCount = 0;
 231 static uint8_t activeOsdElementArray[OSD_ITEM_COUNT];
 232 static bool backgroundLayerSupported = false;
 233
 234 // Blink control
 235 #define OSD_BLINK_FREQUENCY_HZ 2
 236 static bool blinkState = true;
 237 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32];
 238 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
 239 #define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
 240 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 241 #define BLINK(item) (IS_BLINK(item) && blinkState)
 242
 243 // Current element and render status
 244 static osdElementParms_t activeElement;
 245 static bool displayPendingForeground;
 246 static bool displayPendingBackground;
 247 static char elementBuff[OSD_ELEMENT_BUFFER_LENGTH];
 248
 249 // Return whether element is a SYS element and needs special handling
 250 #define IS_SYS_OSD_ELEMENT(item) (item >= OSD_SYS_GOGGLE_VOLTAGE) && (item <= OSD_SYS_FAN_SPEED)
 251
 252 enum {UP, DOWN};
 253
 254 static int osdDisplayWrite(osdElementParms_t *element, uint8_t x, uint8_t y, uint8_t attr, const char *s)
 255 {
 256     if (IS_BLINK(element->item)) {
 257         attr |= DISPLAYPORT_BLINK;
 258     }
 259
 260     return displayWrite(element->osdDisplayPort, x, y, attr, s);
 261 }
 262
 263 static int osdDisplayWriteChar(osdElementParms_t *element, uint8_t x, uint8_t y, uint8_t attr, char c)
 264 {
 265     char buf[2];
 266
 267     buf[0] = c;
 268     buf[1] = 0;
 269
 270     return osdDisplayWrite(element, x, y, attr, buf);
 271 }
 272
 273 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
 274 typedef int (*getEscRpmOrFreqFnPtr)(int i);
 275
 276 static int getEscRpm(int i)
 277 {
 278 #ifdef USE_DSHOT_TELEMETRY
 279     if (useDshotTelemetry) {
 280         return lrintf(getDshotRpm(i));
 281     }
 282 #endif
 283 #ifdef USE_ESC_SENSOR
 284     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
 285         return lrintf(erpmToRpm(getEscSensorData(i)->rpm));
 286     }
 287 #endif
 288     return 0;
 289 }
 290
 291 static int getEscRpmFreq(int i)
 292 {
 293     return getEscRpm(i) / 60;
 294 }
 295
 296 static void renderOsdEscRpmOrFreq(getEscRpmOrFreqFnPtr escFnPtr, osdElementParms_t *element)
 297 {
 298     static uint8_t motor = 0;
 299     const int rpm = MIN((*escFnPtr)(motor),99999);
 300
 301     tfp_sprintf(element->buff, "%d", rpm);
 302     element->elemOffsetY = motor;
 303
 304     if (++motor == getMotorCount()) {
 305         motor = 0;
 306     } else {
 307         element->rendered = false;
 308     }
 309 }
 310 #endif
 311
 312 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 313 int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
 314 {
 315     switch (osdConfig()->units) {
 316     case UNIT_IMPERIAL:
 317         return lrintf(((tempInDegreesCelcius * 9.0f) / 5) + 32);
 318     default:
 319         return tempInDegreesCelcius;
 320     }
 321 }
 322 #endif
 323
 324 static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementType_e variantType)
 325 {
 326     static const struct {
 327         uint8_t decimals;
 328         bool asl;
 329     } variantMap[] = {
 330         [OSD_ELEMENT_TYPE_1] = { 1, false },
 331         [OSD_ELEMENT_TYPE_2] = { 0, false },
 332         [OSD_ELEMENT_TYPE_3] = { 1, true },
 333         [OSD_ELEMENT_TYPE_4] = { 0, true },
 334     };
 335
 336     int32_t alt = altitudeCm;
 337 #ifdef USE_GPS
 338     if (variantMap[variantType].asl) {
 339         alt = getAltitudeAsl();
 340     }
 341 #endif
 342     unsigned decimalPlaces = variantMap[variantType].decimals;
 343     const char unitSymbol = osdGetMetersToSelectedUnitSymbol();
 344
 345     osdPrintFloat(buff, SYM_ALTITUDE, osdGetMetersToSelectedUnit(alt) / 100.0f, "", decimalPlaces, true, unitSymbol);
 346 }
 347
 348 #ifdef USE_GPS
 349 static void osdFormatCoordinate(char *buff, gpsCoordinateType_e coordinateType, osdElementType_e variantType)
 350 {
 351     int32_t gpsValue = 0;
 352     const char leadingSymbol = (coordinateType == GPS_LONGITUDE) ? SYM_LON : SYM_LAT;
 353
 354     if (STATE(GPS_FIX_EVER)) {  // don't display interim coordinates until we get the first position fix
 355         gpsValue = (coordinateType == GPS_LONGITUDE) ? gpsSol.llh.lon : gpsSol.llh.lat;
 356     }
 357
 358     const int degreesPart = abs(gpsValue) / GPS_DEGREES_DIVIDER;
 359     int fractionalPart = abs(gpsValue) % GPS_DEGREES_DIVIDER;
 360
 361     switch (variantType) {
 362 #ifdef USE_GPS_PLUS_CODES
 363 #define PLUS_CODE_DIGITS 11
 364     case OSD_ELEMENT_TYPE_4: // Open Location Code
 365         {
 366             *buff++ = SYM_SAT_L;
 367             *buff++ = SYM_SAT_R;
 368             if (STATE(GPS_FIX_EVER)) {
 369                 OLC_LatLon location;
 370                 location.lat = (double)gpsSol.llh.lat / GPS_DEGREES_DIVIDER;
 371                 location.lon = (double)gpsSol.llh.lon / GPS_DEGREES_DIVIDER;
 372                 OLC_Encode(&location, PLUS_CODE_DIGITS, buff, OSD_ELEMENT_BUFFER_LENGTH - 3);
 373             } else {
 374                 memset(buff, SYM_HYPHEN, PLUS_CODE_DIGITS + 1);
 375                 buff[8] = '+';
 376                 buff[PLUS_CODE_DIGITS + 1] = '\0';
 377             }
 378             break;
 379         }
 380 #endif // USE_GPS_PLUS_CODES
 381
 382     case OSD_ELEMENT_TYPE_3: // degree, minutes, seconds style. ddd^mm'ss.00"W
 383         {
 384             char trailingSymbol;
 385             *buff++ = leadingSymbol;
 386
 387             const int minutes = fractionalPart * 60 / GPS_DEGREES_DIVIDER;
 388             const int fractionalMinutes =  fractionalPart * 60 % GPS_DEGREES_DIVIDER;
 389             const int seconds = fractionalMinutes * 60 / GPS_DEGREES_DIVIDER;
 390             const int tenthSeconds = (fractionalMinutes * 60 % GPS_DEGREES_DIVIDER) * 10 / GPS_DEGREES_DIVIDER;
 391
 392             if (coordinateType == GPS_LONGITUDE) {
 393                 trailingSymbol = (gpsValue < 0) ? 'W' : 'E';
 394             } else {
 395                 trailingSymbol = (gpsValue < 0) ? 'S' : 'N';
 396             }
 397             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);
 398             break;
 399         }
 400
 401     case OSD_ELEMENT_TYPE_2:
 402         fractionalPart /= 1000;
 403         FALLTHROUGH;
 404
 405     case OSD_ELEMENT_TYPE_1:
 406     default:
 407         *buff++ = leadingSymbol;
 408         if (gpsValue < 0) {
 409             *buff++ = SYM_HYPHEN;
 410         }
 411         tfp_sprintf(buff, (variantType == OSD_ELEMENT_TYPE_1 ? "%u.%07u" : "%u.%04u"), degreesPart, fractionalPart);
 412         break;
 413     }
 414 }
 415 #endif // USE_GPS
 416
 417 void osdFormatDistanceString(char *ptr, int distance, char leadingSymbol)
 418 {
 419     const float convertedDistance = osdGetMetersToSelectedUnit(distance);
 420     char unitSymbol;
 421     char unitSymbolExtended;
 422     int unitTransition;
 423
 424     switch (osdConfig()->units) {
 425     case UNIT_IMPERIAL:
 426         unitTransition = 5280;
 427         unitSymbol = SYM_FT;
 428         unitSymbolExtended = SYM_MILES;
 429         break;
 430     default:
 431         unitTransition = 1000;
 432         unitSymbol = SYM_M;
 433         unitSymbolExtended = SYM_KM;
 434         break;
 435     }
 436
 437     unsigned decimalPlaces;
 438     float displayDistance;
 439     char displaySymbol;
 440     if (convertedDistance < unitTransition) {
 441         decimalPlaces = 0;
 442         displayDistance = convertedDistance;
 443         displaySymbol = unitSymbol;
 444     } else {
 445         displayDistance = convertedDistance / unitTransition;
 446         displaySymbol = unitSymbolExtended;
 447         if (displayDistance >= 10) { // >= 10 miles or km - 1 decimal place
 448             decimalPlaces = 1;
 449         } else {                     // < 10 miles or km - 2 decimal places
 450             decimalPlaces = 2;
 451         }
 452     }
 453     osdPrintFloat(ptr, leadingSymbol, displayDistance, "", decimalPlaces, false, displaySymbol);
 454 }
 455
 456 static void osdFormatPID(char * buff, const char * label, uint8_t axis)
 457 {
 458     tfp_sprintf(buff, "%s %3d %3d %3d %3d %3d", label,
 459         currentPidProfile->pid[axis].P,
 460         currentPidProfile->pid[axis].I,
 461         currentPidProfile->pid[axis].D,
 462         currentPidProfile->d_max[axis],
 463         currentPidProfile->pid[axis].F);
 464 }
 465
 466 #ifdef USE_RTC_TIME
 467 bool osdFormatRtcDateTime(char *buffer)
 468 {
 469     dateTime_t dateTime;
 470     if (!rtcGetDateTime(&dateTime)) {
 471         buffer[0] = '\0';
 472
 473         return false;
 474     }
 475
 476     dateTimeFormatLocalShort(buffer, &dateTime);
 477
 478     return true;
 479 }
 480 #endif
 481
 482 void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
 483 {
 484     int seconds = time / 1000000;
 485     const int minutes = seconds / 60;
 486     seconds = seconds % 60;
 487
 488     switch (precision) {
 489     case OSD_TIMER_PREC_SECOND:
 490     default:
 491         tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
 492         break;
 493     case OSD_TIMER_PREC_HUNDREDTHS:
 494         {
 495             const int hundredths = (time / 10000) % 100;
 496             tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
 497             break;
 498         }
 499     case OSD_TIMER_PREC_TENTHS:
 500         {
 501             const int tenths = (time / 100000) % 10;
 502             tfp_sprintf(buff, "%02d:%02d.%01d", minutes, seconds, tenths);
 503             break;
 504         }
 505     }
 506 }
 507
 508 static char osdGetTimerSymbol(osd_timer_source_e src)
 509 {
 510     switch (src) {
 511     case OSD_TIMER_SRC_ON:
 512         return SYM_ON_M;
 513     case OSD_TIMER_SRC_TOTAL_ARMED:
 514     case OSD_TIMER_SRC_LAST_ARMED:
 515         return SYM_FLY_M;
 516     case OSD_TIMER_SRC_ON_OR_ARMED:
 517         return ARMING_FLAG(ARMED) ? SYM_FLY_M : SYM_ON_M;
 518     default:
 519         return ' ';
 520     }
 521 }
 522
 523 static timeUs_t osdGetTimerValue(osd_timer_source_e src)
 524 {
 525     switch (src) {
 526     case OSD_TIMER_SRC_ON:
 527         return micros();
 528     case OSD_TIMER_SRC_TOTAL_ARMED:
 529         return osdFlyTime;
 530     case OSD_TIMER_SRC_LAST_ARMED: {
 531         statistic_t *stats = osdGetStats();
 532         return stats->armed_time;
 533     }
 534     case OSD_TIMER_SRC_ON_OR_ARMED:
 535         return ARMING_FLAG(ARMED) ? osdFlyTime : micros();
 536     default:
 537         return 0;
 538     }
 539 }
 540
 541 void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
 542 {
 543     const uint16_t timer = osdConfig()->timers[timerIndex];
 544     const uint8_t src = OSD_TIMER_SRC(timer);
 545
 546     if (showSymbol) {
 547         *(buff++) = osdGetTimerSymbol(src);
 548     }
 549
 550     osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
 551 }
 552
 553 static char osdGetBatterySymbol(int cellVoltage)
 554 {
 555     if (getBatteryState() == BATTERY_CRITICAL) {
 556         return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
 557     } else {
 558         // Calculate a symbol offset using cell voltage over full cell voltage range
 559         const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatmaxcellvoltage, 0, 8);
 560         return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
 561     }
 562 }
 563
 564 static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
 565 {
 566     heading += FULL_CIRCLE;  // Ensure positive value
 567
 568     // Split input heading 0..359 into sectors 0..(directions-1), but offset
 569     // by half a sector so that sector 0 gets centered around heading 0.
 570     // We multiply heading by directions to not loose precision in divisions
 571     // In this way each segment will be a FULL_CIRCLE length
 572     int direction = (heading * directions + FULL_CIRCLE / 2) /  FULL_CIRCLE; // scale with rounding
 573     direction %= directions; // normalize
 574
 575     return direction; // return segment number
 576 }
 577
 578 static uint8_t osdGetDirectionSymbolFromHeading(int heading)
 579 {
 580     heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
 581
 582     // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
 583     // Our symbols are Down=0, Right=4, Up=8 and Left=12
 584     // There're 16 arrow symbols. Transform it.
 585     heading = 16 - heading;
 586     heading = (heading + 8) % 16;
 587
 588     return SYM_ARROW_SOUTH + heading;
 589 }
 590
 591 /**
 592  * Converts altitude based on the current unit system.
 593  * @param meters Value in meters to convert
 594  */
 595 float osdGetMetersToSelectedUnit(int32_t meters)
 596 {
 597     switch (osdConfig()->units) {
 598     case UNIT_IMPERIAL:
 599         return meters * 3.28084f;       // Convert to feet
 600     default:
 601         return meters;                  // Already in meters
 602     }
 603 }
 604
 605 /**
 606  * Gets the correct altitude symbol for the current unit system
 607  */
 608 char osdGetMetersToSelectedUnitSymbol(void)
 609 {
 610     switch (osdConfig()->units) {
 611     case UNIT_IMPERIAL:
 612         return SYM_FT;
 613     default:
 614         return SYM_M;
 615     }
 616 }
 617
 618 /**
 619  * Converts speed based on the current unit system.
 620  * @param value in cm/s to convert
 621  */
 622 int32_t osdGetSpeedToSelectedUnit(int32_t value)
 623 {
 624     switch (osdConfig()->units) {
 625     case UNIT_IMPERIAL:
 626     case UNIT_BRITISH:
 627         return CM_S_TO_MPH(value);
 628     default:
 629         return CM_S_TO_KM_H(value);
 630     }
 631 }
 632
 633 /**
 634  * Gets the correct speed symbol for the current unit system
 635  */
 636 char osdGetSpeedToSelectedUnitSymbol(void)
 637 {
 638     switch (osdConfig()->units) {
 639     case UNIT_IMPERIAL:
 640     case UNIT_BRITISH:
 641         return SYM_MPH;
 642     default:
 643         return SYM_KPH;
 644     }
 645 }
 646
 647 MAYBE_UNUSED static char osdGetVarioToSelectedUnitSymbol(void)
 648 {
 649     switch (osdConfig()->units) {
 650     case UNIT_IMPERIAL:
 651         return SYM_FTPS;
 652     default:
 653         return SYM_MPS;
 654     }
 655 }
 656
 657 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 658 char osdGetTemperatureSymbolForSelectedUnit(void)
 659 {
 660     switch (osdConfig()->units) {
 661     case UNIT_IMPERIAL:
 662         return SYM_F;
 663     default:
 664         return SYM_C;
 665     }
 666 }
 667 #endif
 668
 669 // *************************
 670 // Element drawing functions
 671 // *************************
 672
 673 #ifdef USE_OSD_ADJUSTMENTS
 674 static void osdElementAdjustmentRange(osdElementParms_t *element)
 675 {
 676     const char *name = getAdjustmentsRangeName();
 677     if (name) {
 678         tfp_sprintf(element->buff, "%s: %3d", name, getAdjustmentsRangeValue());
 679     }
 680 }
 681 #endif // USE_OSD_ADJUSTMENTS
 682
 683 static void osdElementAltitude(osdElementParms_t *element)
 684 {
 685     bool haveBaro = false;
 686     bool haveGps = false;
 687 #ifdef USE_BARO
 688     haveBaro = sensors(SENSOR_BARO);
 689 #endif // USE_BARO
 690 #ifdef USE_GPS
 691     haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
 692 #endif // USE_GPS
 693     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
 694
 695     if ((alt >= osdConfig()->alt_alarm) && ARMING_FLAG(ARMED)) {
 696         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
 697     }
 698
 699     if (haveBaro || haveGps) {
 700         osdFormatAltitudeString(element->buff, getEstimatedAltitudeCm(), element->type);
 701     } else {
 702         element->buff[0] = SYM_ALTITUDE;
 703         element->buff[1] = SYM_HYPHEN; // We use this symbol when we don't have a valid measure
 704         element->buff[2] = '\0';
 705     }
 706 }
 707
 708 #ifdef USE_ACC
 709 static void osdElementAngleRollPitch(osdElementParms_t *element)
 710 {
 711     const float angle = ((element->item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll) / 10.0f;
 712     osdPrintFloat(element->buff, (element->item == OSD_PITCH_ANGLE) ? SYM_PITCH : SYM_ROLL, fabsf(angle), ((angle < 0) ? "-%02u" : " %02u"), 1, true, SYM_NONE);
 713 }
 714 #endif
 715
 716 static void osdElementAntiGravity(osdElementParms_t *element)
 717 {
 718     if (pidOsdAntiGravityActive()) {
 719         strcpy(element->buff, "AG");
 720     }
 721 }
 722
 723 #ifdef USE_ACC
 724
 725 static void osdElementArtificialHorizon(osdElementParms_t *element)
 726 {
 727     static int x = -4;
 728     // Get pitch and roll limits in tenths of degrees
 729     const int maxPitch = osdConfig()->ahMaxPitch * 10;
 730     const int maxRoll = osdConfig()->ahMaxRoll * 10;
 731     const int ahSign = osdConfig()->ahInvert ? -1 : 1;
 732     const int rollAngle = constrain(attitude.values.roll * ahSign, -maxRoll, maxRoll);
 733     int pitchAngle = constrain(attitude.values.pitch * ahSign, -maxPitch, maxPitch);
 734     // Convert pitchAngle to y compensation value
 735     // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
 736     if (maxPitch > 0) {
 737         pitchAngle = ((pitchAngle * 25) / maxPitch);
 738     }
 739     pitchAngle -= 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
 740
 741     const int y = ((-rollAngle * x) / 64) - pitchAngle;
 742     if (y >= 0 && y <= 81) {
 743         element->elemOffsetX = x;
 744         element->elemOffsetY = y / AH_SYMBOL_COUNT;
 745
 746         tfp_sprintf(element->buff, "%c", (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
 747     } else {
 748         element->drawElement = false;  // element does not need to be rendered
 749     }
 750
 751     if (x == 4) {
 752         // Rendering is complete, so prepare to start again
 753         x = -4;
 754     } else {
 755         // Rendering not yet complete
 756         element->rendered = false;
 757         x++;
 758     }
 759 }
 760
 761 static void osdElementUpDownReference(osdElementParms_t *element)
 762 {
 763 // Up/Down reference feature displays reference points on the OSD at Zenith and Nadir
 764     const float earthUpinBodyFrame[3] = {-rMat.m[2][0], -rMat.m[2][1], -rMat.m[2][2]}; //transforum the up vector to the body frame
 765
 766     if (fabsf(earthUpinBodyFrame[2]) < SINE_25_DEG && fabsf(earthUpinBodyFrame[1]) < SINE_25_DEG) {
 767         float thetaB; // pitch from body frame to zenith/nadir
 768         float psiB; // psi from body frame to zenith/nadir
 769         char *symbol[2] = {"U", "D"}; // character buffer
 770         int direction;
 771
 772         if(attitude.values.pitch>0.0){ //nose down
 773             thetaB = -earthUpinBodyFrame[2]; // get pitch w/re to nadir (use small angle approx for sine)
 774             psiB = -earthUpinBodyFrame[1]; // calculate the yaw w/re to nadir (use small angle approx for sine)
 775             direction = DOWN;
 776         } else { // nose up
 777             thetaB = earthUpinBodyFrame[2]; // get pitch w/re to zenith (use small angle approx for sine)
 778             psiB = earthUpinBodyFrame[1]; // calculate the yaw w/re to zenith (use small angle approx for sine)
 779             direction = UP;
 780         }
 781         element->elemOffsetX = lrintf(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14));
 782         element->elemOffsetY = lrintf(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8));
 783
 784         tfp_sprintf(element->buff, "%c", symbol[direction]);
 785     }
 786 }
 787 #endif // USE_ACC
 788
 789 static void osdElementAverageCellVoltage(osdElementParms_t *element)
 790 {
 791     const int cellV = getBatteryAverageCellVoltage();
 792     const batteryState_e batteryState = getBatteryState();
 793
 794     switch (batteryState) {
 795     case BATTERY_WARNING:
 796         element->attr = DISPLAYPORT_SEVERITY_WARNING;
 797         break;
 798     case BATTERY_CRITICAL:
 799         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
 800         break;
 801     default:
 802         break;
 803     }
 804
 805     osdPrintFloat(element->buff, osdGetBatterySymbol(cellV), cellV / 100.0f, "", 2, false, SYM_VOLT);
 806 }
 807
 808 static void osdElementCompassBar(osdElementParms_t *element)
 809 {
 810     memcpy(element->buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
 811     element->buff[9] = 0;
 812 }
 813
 814 //display custom message from MSPv2
 815 static void osdElementCustomMsg(osdElementParms_t *element)
 816 {
 817     int msgIndex = element->item - OSD_CUSTOM_MSG0;
 818     if (msgIndex < 0 || msgIndex >= OSD_CUSTOM_MSG_COUNT || pilotConfig()->message[msgIndex][0] == '\0') {
 819         tfp_sprintf(element->buff, "CUSTOM_MSG%d", msgIndex + 1);
 820     } else {
 821         strncpy(element->buff, pilotConfig()->message[msgIndex], MAX_NAME_LENGTH);
 822         element->buff[MAX_NAME_LENGTH] = 0;   // terminate maximum-length string
 823     }
 824 }
 825
 826 #ifdef USE_ADC_INTERNAL
 827 static void osdElementCoreTemperature(osdElementParms_t *element)
 828 {
 829     tfp_sprintf(element->buff, "C%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius()), osdGetTemperatureSymbolForSelectedUnit());
 830 }
 831 #endif // USE_ADC_INTERNAL
 832
 833 static void osdBackgroundCameraFrame(osdElementParms_t *element)
 834 {
 835     static enum {TOP, MIDDLE, BOTTOM} renderPhase = TOP;
 836     const uint8_t xpos = element->elemPosX;
 837     const uint8_t ypos = element->elemPosY;
 838     const uint8_t width = constrain(osdConfig()->camera_frame_width, OSD_CAMERA_FRAME_MIN_WIDTH, OSD_CAMERA_FRAME_MAX_WIDTH);
 839     const uint8_t height = constrain(osdConfig()->camera_frame_height, OSD_CAMERA_FRAME_MIN_HEIGHT, OSD_CAMERA_FRAME_MAX_HEIGHT);
 840
 841     if (renderPhase != BOTTOM) {
 842         // Rendering not yet complete
 843         element->rendered = false;
 844     }
 845
 846     if (renderPhase == MIDDLE) {
 847         static uint8_t i = 1;
 848
 849         osdDisplayWriteChar(element, xpos, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
 850         osdDisplayWriteChar(element, xpos + width - 1, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
 851
 852         element->drawElement = false;  // element already drawn
 853
 854         if (++i == height) {
 855             i = 1;
 856             renderPhase = BOTTOM;
 857         }
 858     } else {
 859         element->buff[0] = SYM_STICK_OVERLAY_CENTER;
 860         for (uint8_t i = 1; i < (width - 1); i++) {
 861             element->buff[i] = SYM_STICK_OVERLAY_HORIZONTAL;
 862         }
 863         element->buff[width - 1] = SYM_STICK_OVERLAY_CENTER;
 864         element->buff[width] = 0;  // string terminator
 865
 866         if (renderPhase == TOP) {
 867             renderPhase = MIDDLE;
 868         } else {
 869             element->elemOffsetY = height - 1;
 870             renderPhase = TOP;
 871         }
 872     }
 873 }
 874
 875 static void toUpperCase(char* dest, const char* src, unsigned int maxSrcLength)
 876 {
 877     unsigned int i;
 878     for (i = 0; i < maxSrcLength && src[i]; i++) {
 879             dest[i] = toupper((unsigned char)src[i]);
 880     }
 881     dest[i] = '\0';
 882 }
 883
 884 static void osdBackgroundCraftName(osdElementParms_t *element)
 885 {
 886     if (strlen(pilotConfig()->craftName) == 0) {
 887         strcpy(element->buff, "CRAFT_NAME");
 888     } else {
 889         toUpperCase(element->buff, pilotConfig()->craftName, MAX_NAME_LENGTH);
 890     }
 891 }
 892
 893 #ifdef USE_ACC
 894 static void osdElementCrashFlipArrow(osdElementParms_t *element)
 895 {
 896     int rollAngle = attitude.values.roll / 10;
 897     const int pitchAngle = attitude.values.pitch / 10;
 898     if (abs(rollAngle) > 90) {
 899         rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
 900     }
 901
 902     if ((isCrashFlipModeActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) {
 903         element->attr = DISPLAYPORT_SEVERITY_INFO;
 904         if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
 905             if (pitchAngle > 0) {
 906                 if (rollAngle > 0) {
 907                     element->buff[0] = SYM_ARROW_WEST + 2;
 908                 } else {
 909                     element->buff[0] = SYM_ARROW_EAST - 2;
 910                 }
 911             } else {
 912                 if (rollAngle > 0) {
 913                     element->buff[0] = SYM_ARROW_WEST - 2;
 914                 } else {
 915                     element->buff[0] = SYM_ARROW_EAST + 2;
 916                 }
 917             }
 918         } else {
 919             if (abs(pitchAngle) > abs(rollAngle)) {
 920                 if (pitchAngle > 0) {
 921                     element->buff[0] = SYM_ARROW_SOUTH;
 922                 } else {
 923                     element->buff[0] = SYM_ARROW_NORTH;
 924                 }
 925             } else {
 926                 if (rollAngle > 0) {
 927                     element->buff[0] = SYM_ARROW_WEST;
 928                 } else {
 929                     element->buff[0] = SYM_ARROW_EAST;
 930                 }
 931             }
 932         }
 933         element->buff[1] = '\0';
 934     }
 935 }
 936 #endif // USE_ACC
 937
 938 static void osdElementCrosshairs(osdElementParms_t *element)
 939 {
 940     element->buff[0] = SYM_AH_CENTER_LINE;
 941     element->buff[1] = SYM_AH_CENTER;
 942     element->buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 943     element->buff[3] = 0;
 944 }
 945
 946 static void osdElementCurrentDraw(osdElementParms_t *element)
 947 {
 948     const float amperage = fabsf(getAmperage() / 100.0f);
 949     osdPrintFloat(element->buff, SYM_NONE, amperage, "%3u", 2, false, SYM_AMP);
 950 }
 951
 952 static void osdElementDebug(osdElementParms_t *element)
 953 {
 954     tfp_sprintf(element->buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
 955 }
 956
 957 static void osdElementDebug2(osdElementParms_t *element)
 958 {
 959     tfp_sprintf(element->buff, "D2  %5d %5d %5d %5d", debug[4], debug[5], debug[6], debug[7]);
 960 }
 961
 962 static void osdElementDisarmed(osdElementParms_t *element)
 963 {
 964     if (!ARMING_FLAG(ARMED)) {
 965         tfp_sprintf(element->buff, "DISARMED");
 966     }
 967 }
 968
 969 static void osdBackgroundPilotName(osdElementParms_t *element)
 970 {
 971     if (strlen(pilotConfig()->pilotName) == 0) {
 972         strcpy(element->buff, "PILOT_NAME");
 973     } else {
 974         toUpperCase(element->buff, pilotConfig()->pilotName, MAX_NAME_LENGTH);
 975     }
 976 }
 977
 978 #ifdef USE_PERSISTENT_STATS
 979 static void osdElementTotalFlights(osdElementParms_t *element)
 980 {
 981     const int32_t total_flights = statsConfig()->stats_total_flights;
 982     tfp_sprintf(element->buff, "#%d", total_flights);
 983 }
 984 #endif
 985
 986 #ifdef USE_PROFILE_NAMES
 987 static void osdElementRateProfileName(osdElementParms_t *element)
 988 {
 989     if (strlen(currentControlRateProfile->profileName) == 0) {
 990         tfp_sprintf(element->buff, "RATE_%u", getCurrentControlRateProfileIndex() + 1);
 991     } else {
 992         toUpperCase(element->buff, currentControlRateProfile->profileName, MAX_PROFILE_NAME_LENGTH);
 993     }
 994 }
 995
 996 static void osdElementPidProfileName(osdElementParms_t *element)
 997 {
 998     if (strlen(currentPidProfile->profileName) == 0) {
 999         tfp_sprintf(element->buff, "PID_%u", getCurrentPidProfileIndex() + 1);
1000     } else {
1001         toUpperCase(element->buff, currentPidProfile->profileName, MAX_PROFILE_NAME_LENGTH);
1002     }
1003 }
1004 #endif
1005
1006 #ifdef USE_OSD_PROFILES
1007 static void osdElementOsdProfileName(osdElementParms_t *element)
1008 {
1009     uint8_t profileIndex = getCurrentOsdProfileIndex();
1010
1011     if (strlen(osdConfig()->profile[profileIndex - 1]) == 0) {
1012         tfp_sprintf(element->buff, "OSD_%u", profileIndex);
1013     } else {
1014         toUpperCase(element->buff, osdConfig()->profile[profileIndex - 1], OSD_PROFILE_NAME_LENGTH);
1015     }
1016 }
1017 #endif
1018
1019 #if defined(USE_ESC_SENSOR) ||  defined(USE_DSHOT_TELEMETRY)
1020
1021 static void osdElementEscTemperature(osdElementParms_t *element)
1022 {
1023 #if defined(USE_ESC_SENSOR)
1024     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
1025         tfp_sprintf(element->buff, "E%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(osdEscDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
1026     } else
1027 #endif
1028 #if defined(USE_DSHOT_TELEMETRY)
1029     {
1030         uint32_t osdEleIx = tfp_sprintf(element->buff, "E%c", SYM_TEMPERATURE);
1031
1032         for (uint8_t k = 0; k < getMotorCount(); k++) {
1033             if ((dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) {
1034                 osdEleIx += tfp_sprintf(element->buff + osdEleIx, "%3d%c",
1035                     osdConvertTemperatureToSelectedUnit(dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]),
1036                     osdGetTemperatureSymbolForSelectedUnit());
1037             } else {
1038                 osdEleIx += tfp_sprintf(element->buff + osdEleIx, "  0%c", osdGetTemperatureSymbolForSelectedUnit());
1039             }
1040         }
1041     }
1042 #else
1043     {}
1044 #endif
1045 }
1046
1047 static void osdElementEscRpm(osdElementParms_t *element)
1048 {
1049     renderOsdEscRpmOrFreq(&getEscRpm,element);
1050 }
1051
1052 static void osdElementEscRpmFreq(osdElementParms_t *element)
1053 {
1054     renderOsdEscRpmOrFreq(&getEscRpmFreq,element);
1055 }
1056
1057 #endif
1058
1059 static void osdElementFlymode(osdElementParms_t *element)
1060 {
1061     // Note that flight mode display has precedence in what to display.
1062     //  1. FS
1063     //  2. GPS RESCUE
1064     //  3. PASSTHRU
1065     //  4. HEAD, POSHOLD, ALTHOLD, ANGLE, HORIZON, ACRO TRAINER
1066     //  5. AIR
1067     //  6. ACRO
1068
1069     if (FLIGHT_MODE(FAILSAFE_MODE)) {
1070         strcpy(element->buff, "!FS!");
1071     } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
1072         strcpy(element->buff, "RESC");
1073     } else if (FLIGHT_MODE(HEADFREE_MODE)) {
1074         strcpy(element->buff, "HEAD");
1075     } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
1076         strcpy(element->buff, "PASS");
1077     } else if (FLIGHT_MODE(POS_HOLD_MODE)) {
1078         strcpy(element->buff, "POSH");
1079     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
1080         strcpy(element->buff, "ALTH");
1081     } else if (FLIGHT_MODE(ANGLE_MODE)) {
1082         strcpy(element->buff, "ANGL");
1083     } else if (FLIGHT_MODE(HORIZON_MODE)) {
1084         strcpy(element->buff, "HOR ");
1085     } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
1086         strcpy(element->buff, "ATRN");
1087 #ifdef USE_CHIRP
1088     // the additional check for pidChirpIsFinished() is to have visual feedback for user that don't have warnings enabled in their googles
1089     } else if (FLIGHT_MODE(CHIRP_MODE) && !pidChirpIsFinished()) {
1090 #endif
1091         strcpy(element->buff, "CHIR");
1092     } else if (isAirmodeEnabled()) {
1093         strcpy(element->buff, "AIR ");
1094     } else {
1095         strcpy(element->buff, "ACRO");
1096     }
1097 }
1098
1099 static void osdElementReadyMode(osdElementParms_t *element)
1100 {
1101     if (IS_RC_MODE_ACTIVE(BOXREADY) && !ARMING_FLAG(ARMED)) {
1102         strcpy(element->buff, "READY");
1103     }
1104 }
1105
1106 #ifdef USE_ACC
1107 static void osdElementGForce(osdElementParms_t *element)
1108 {
1109     osdPrintFloat(element->buff, SYM_NONE, osdGForce, "", 1, true, 'G');
1110 }
1111 #endif // USE_ACC
1112
1113 #ifdef USE_GPS
1114 static void osdElementGpsFlightDistance(osdElementParms_t *element)
1115 {
1116     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1117         osdFormatDistanceString(element->buff, GPS_distanceFlownInCm / 100, SYM_TOTAL_DISTANCE);
1118     } else {
1119         // We use this symbol when we don't have a FIX
1120         tfp_sprintf(element->buff, "%c%c", SYM_TOTAL_DISTANCE, SYM_HYPHEN);
1121     }
1122 }
1123
1124 static void osdElementGpsHomeDirection(osdElementParms_t *element)
1125 {
1126     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1127         if (GPS_distanceToHome > 0) {
1128             int direction = GPS_directionToHome;
1129 #ifdef USE_GPS_LAP_TIMER
1130             // Override the "home" point to the start/finish location if the lap timer is running
1131             if (gpsLapTimerData.timerRunning) {
1132                 direction = lrintf(gpsLapTimerData.dirToPoint * 0.1f); // Convert from centidegree to degree and round to nearest
1133             }
1134 #endif
1135             element->buff[0] = osdGetDirectionSymbolFromHeading(DECIDEGREES_TO_DEGREES(direction - attitude.values.yaw));
1136         } else {
1137             element->buff[0] = SYM_OVER_HOME;
1138         }
1139
1140     } else {
1141         // We use this symbol when we don't have a FIX
1142         element->buff[0] = SYM_HYPHEN;
1143     }
1144
1145     element->buff[1] = 0;
1146 }
1147
1148 static void osdElementGpsHomeDistance(osdElementParms_t *element)
1149 {
1150     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1151         int distance = GPS_distanceToHome;
1152 #ifdef USE_GPS_LAP_TIMER
1153         // Change the "home" point to the start/finish location if the lap timer is running
1154         if (gpsLapTimerData.timerRunning) {
1155             distance = lrintf(gpsLapTimerData.distToPointCM * 0.01f); // Round to nearest natural number
1156         }
1157 #endif
1158         osdFormatDistanceString(element->buff, distance, SYM_HOMEFLAG);
1159     } else {
1160         element->buff[0] = SYM_HOMEFLAG;
1161         // We use this symbol when we don't have a FIX
1162         element->buff[1] = SYM_HYPHEN;
1163         element->buff[2] = '\0';
1164     }
1165 }
1166
1167 static void osdElementGpsCoordinate(osdElementParms_t *element)
1168 {
1169     const gpsCoordinateType_e coordinateType = (element->item == OSD_GPS_LON) ? GPS_LONGITUDE : GPS_LATITUDE;
1170     osdFormatCoordinate(element->buff, coordinateType, element->type);
1171     if (STATE(GPS_FIX_EVER) && !STATE(GPS_FIX)) {
1172         SET_BLINK(element->item); // blink if we had a fix but have since lost it
1173     } else {
1174         CLR_BLINK(element->item);
1175     }
1176 }
1177
1178 static void osdElementGpsSats(osdElementParms_t *element)
1179 {
1180     if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < GPS_MIN_SAT_COUNT) ) {
1181         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1182     }
1183 #ifdef USE_GPS_RESCUE
1184     else if ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured()) {
1185         element->attr = DISPLAYPORT_SEVERITY_WARNING;
1186     }
1187 #endif
1188     else {
1189         element->attr = DISPLAYPORT_SEVERITY_NORMAL;
1190     }
1191
1192     if (!gpsIsHealthy()) {
1193         tfp_sprintf(element->buff, "%c%cNC", SYM_SAT_L, SYM_SAT_R);
1194     } else {
1195         int pos = tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
1196         if (osdConfig()->gps_sats_show_pdop) { // add on the GPS module PDOP estimate
1197             element->buff[pos++] = ' ';
1198             osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.dop.pdop / 100.0f, "", 1, true, SYM_NONE);
1199         }
1200     }
1201 }
1202
1203 static void osdElementGpsSpeed(osdElementParms_t *element)
1204 {
1205     if (STATE(GPS_FIX)) {
1206         tfp_sprintf(element->buff, "%c%3d%c", SYM_SPEED, osdGetSpeedToSelectedUnit(gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed), osdGetSpeedToSelectedUnitSymbol());
1207     } else {
1208         tfp_sprintf(element->buff, "%c%c%c", SYM_SPEED, SYM_HYPHEN, osdGetSpeedToSelectedUnitSymbol());
1209     }
1210 }
1211
1212 static void osdElementEfficiency(osdElementParms_t *element)
1213 {
1214     int efficiency = 0;
1215     if (sensors(SENSOR_GPS) && ARMING_FLAG(ARMED) && STATE(GPS_FIX) && gpsSol.groundSpeed >= EFFICIENCY_MINIMUM_SPEED_CM_S) {
1216         const float speed = (float)osdGetSpeedToSelectedUnit(gpsSol.groundSpeed);
1217         const float mAmperage = (float)getAmperage() * 10.f; // Current in mA
1218         efficiency = lrintf(pt1FilterApply(&batteryEfficiencyFilt, (mAmperage / speed)));
1219     }
1220
1221     const char unitSymbol = osdConfig()->units == UNIT_IMPERIAL ? SYM_MILES : SYM_KM;
1222     if (efficiency > 0 && efficiency <= 9999) {
1223         tfp_sprintf(element->buff, "%4d%c/%c", efficiency, SYM_MAH, unitSymbol);
1224     } else {
1225         tfp_sprintf(element->buff, "----%c/%c", SYM_MAH, unitSymbol);
1226     }
1227 }
1228 #endif // USE_GPS
1229
1230 #ifdef USE_GPS_LAP_TIMER
1231 static void osdFormatLapTime(osdElementParms_t *element, uint32_t timeMs, uint8_t symbol)
1232 {
1233     timeMs += 5;  // round to nearest centisecond (+/- 5ms)
1234     uint32_t seconds = timeMs / 1000;
1235     uint32_t decimals = (timeMs % 1000) / 10;
1236     tfp_sprintf(element->buff, "%c%3u.%02u", symbol, seconds, decimals);
1237 }
1238
1239 static void osdElementGpsLapTimeCurrent(osdElementParms_t *element)
1240 {
1241     if (gpsLapTimerData.timerRunning) {
1242         osdFormatLapTime(element, gpsSol.time - gpsLapTimerData.timeOfLastLap, SYM_TOTAL_DISTANCE);
1243     } else {
1244         osdFormatLapTime(element, 0, SYM_TOTAL_DISTANCE);
1245     }
1246 }
1247
1248 static void osdElementGpsLapTimePrevious(osdElementParms_t *element)
1249 {
1250     osdFormatLapTime(element, gpsLapTimerData.previousLaps[0], SYM_PREV_LAP_TIME);
1251 }
1252
1253 static void osdElementGpsLapTimeBest3(osdElementParms_t *element)
1254 {
1255     osdFormatLapTime(element, gpsLapTimerData.best3Consec, SYM_CHECKERED_FLAG);
1256 }
1257 #endif // GPS_LAP_TIMER
1258
1259 static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
1260 {
1261     static bool renderLevel = false;
1262     static int8_t y = -AH_SIDEBAR_HEIGHT_POS;
1263     // Draw AH sides
1264     const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
1265     const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
1266
1267     if (renderLevel) {
1268         // AH level indicators
1269         osdDisplayWriteChar(element, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_LEFT);
1270         osdDisplayWriteChar(element, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_RIGHT);
1271         renderLevel = false;
1272     } else {
1273         osdDisplayWriteChar(element, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_DECORATION);
1274         osdDisplayWriteChar(element, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_DECORATION);
1275
1276         if (y == hudheight) {
1277             // Rendering is complete, so prepare to start again
1278             y = -hudheight;
1279             // On next pass render the level markers
1280             renderLevel = true;
1281         } else {
1282             y++;
1283         }
1284         // Rendering not yet complete
1285         element->rendered = false;
1286     }
1287
1288     element->drawElement = false;  // element already drawn
1289 }
1290
1291 #ifdef USE_RX_LINK_QUALITY_INFO
1292 static void osdElementLinkQuality(osdElementParms_t *element)
1293 {
1294     uint16_t osdLinkQuality = 0;
1295
1296     if (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm) {
1297         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1298     }
1299
1300     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-99
1301         osdLinkQuality = rxGetLinkQuality();
1302         const uint8_t osdRfMode = rxGetRfMode();
1303         tfp_sprintf(element->buff, "%c%1d:%2d", SYM_LINK_QUALITY, osdRfMode, osdLinkQuality);
1304     } else if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_GHST) { // 0-100
1305         osdLinkQuality = rxGetLinkQuality();
1306         tfp_sprintf(element->buff, "%c%2d", SYM_LINK_QUALITY, osdLinkQuality);
1307     } else { // 0-9
1308         osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
1309         if (osdLinkQuality >= 10) {
1310             osdLinkQuality = 9;
1311         }
1312         tfp_sprintf(element->buff, "%c%1d", SYM_LINK_QUALITY, osdLinkQuality);
1313     }
1314 }
1315 #endif // USE_RX_LINK_QUALITY_INFO
1316
1317 #ifdef USE_RX_LINK_UPLINK_POWER
1318 static void osdElementTxUplinkPower(osdElementParms_t *element)
1319 {
1320     const uint16_t osdUplinkTxPowerMw = rxGetUplinkTxPwrMw();
1321     if (osdUplinkTxPowerMw < 1000) {
1322         tfp_sprintf(element->buff, "%c%3dMW", SYM_RSSI, osdUplinkTxPowerMw);
1323     } else {
1324         osdPrintFloat(element->buff, SYM_RSSI, osdUplinkTxPowerMw / 1000.0f, "", 1, false, 'W');
1325     }
1326 }
1327 #endif // USE_RX_LINK_UPLINK_POWER
1328
1329 #ifdef USE_BLACKBOX
1330 static void osdElementLogStatus(osdElementParms_t *element)
1331 {
1332     if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
1333         if (!isBlackboxDeviceWorking()) {
1334             tfp_sprintf(element->buff, "%c!", SYM_BBLOG);
1335         } else if (isBlackboxDeviceFull()) {
1336             tfp_sprintf(element->buff, "%c>", SYM_BBLOG);
1337         } else {
1338             int32_t logNumber = blackboxGetLogNumber();
1339             if (logNumber >= 0) {
1340                 tfp_sprintf(element->buff, "%c%d", SYM_BBLOG, logNumber);
1341             } else {
1342                 tfp_sprintf(element->buff, "%c", SYM_BBLOG);
1343             }
1344         }
1345     }
1346 }
1347 #endif // USE_BLACKBOX
1348
1349 static void osdElementMahDrawn(osdElementParms_t *element)
1350 {
1351     const int mAhDrawn = getMAhDrawn();
1352
1353     if (mAhDrawn >= osdConfig()->cap_alarm) {
1354         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1355     }
1356
1357     tfp_sprintf(element->buff, "%4d%c", mAhDrawn, SYM_MAH);
1358 }
1359
1360 static void osdElementWattHoursDrawn(osdElementParms_t *element)
1361 {
1362     const int mAhDrawn = getMAhDrawn();
1363     const float wattHoursDrawn = getWhDrawn();
1364
1365     if (mAhDrawn >= osdConfig()->cap_alarm) {
1366         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1367     }
1368
1369     if (wattHoursDrawn < 1.0f) {
1370         tfp_sprintf(element->buff, "%3dMWH", lrintf(wattHoursDrawn * 1000));
1371     } else {
1372         int wattHourWholeNumber = (int)wattHoursDrawn;
1373         int wattHourDecimalValue = (int)((wattHoursDrawn - wattHourWholeNumber) * 100);
1374
1375         tfp_sprintf(element->buff, wattHourDecimalValue >= 10 ? "%3d.%2dWH" : "%3d.0%1dWH", wattHourWholeNumber, wattHourDecimalValue);
1376     }
1377 }
1378
1379 static void osdElementMainBatteryUsage(osdElementParms_t *element)
1380 {
1381     // Set length of indicator bar
1382     #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
1383     const int mAhDrawn = getMAhDrawn();
1384     const int usedCapacity = getMAhDrawn();
1385     int displayBasis = usedCapacity;
1386
1387     if (mAhDrawn >= osdConfig()->cap_alarm) {
1388         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1389     }
1390
1391     switch (element->type) {
1392     case OSD_ELEMENT_TYPE_3:  // mAh remaining percentage (counts down as battery is used)
1393         displayBasis = constrain(batteryConfig()->batteryCapacity - usedCapacity, 0, batteryConfig()->batteryCapacity);
1394         FALLTHROUGH;
1395
1396     case OSD_ELEMENT_TYPE_4:  // mAh used percentage (counts up as battery is used)
1397         {
1398             int displayPercent = 0;
1399             if (batteryConfig()->batteryCapacity) {
1400                 displayPercent = constrain(lrintf(100.0f * displayBasis / batteryConfig()->batteryCapacity), 0, 100);
1401             }
1402             tfp_sprintf(element->buff, "%c%d%%", SYM_MAH, displayPercent);
1403             break;
1404         }
1405
1406     case OSD_ELEMENT_TYPE_2:  // mAh used graphical progress bar (grows as battery is used)
1407         displayBasis = constrain(batteryConfig()->batteryCapacity - usedCapacity, 0, batteryConfig()->batteryCapacity);
1408         FALLTHROUGH;
1409
1410     case OSD_ELEMENT_TYPE_1:  // mAh remaining graphical progress bar (shrinks as battery is used)
1411     default:
1412         {
1413             uint8_t remainingCapacityBars = 0;
1414
1415             if (batteryConfig()->batteryCapacity) {
1416                 const float batteryRemaining = constrain(batteryConfig()->batteryCapacity - displayBasis, 0, batteryConfig()->batteryCapacity);
1417                 remainingCapacityBars = ceilf((batteryRemaining / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
1418             }
1419
1420             // Create empty battery indicator bar
1421             element->buff[0] = SYM_PB_START;
1422             for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
1423                 element->buff[i] = i <= remainingCapacityBars ? SYM_PB_FULL : SYM_PB_EMPTY;
1424             }
1425             element->buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
1426             if (remainingCapacityBars > 0 && remainingCapacityBars < MAIN_BATT_USAGE_STEPS) {
1427                 element->buff[1 + remainingCapacityBars] = SYM_PB_END;
1428             }
1429             element->buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
1430             break;
1431         }
1432     }
1433 }
1434
1435 static void osdElementMainBatteryVoltage(osdElementParms_t *element)
1436 {
1437     unsigned decimalPlaces;
1438     const float batteryVoltage = getBatteryVoltage() / 100.0f;
1439     batteryState_e batteryState = getBatteryState();
1440
1441     switch (batteryState) {
1442     case BATTERY_WARNING:
1443         element->attr = DISPLAYPORT_SEVERITY_WARNING;
1444         break;
1445     case BATTERY_CRITICAL:
1446         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1447         break;
1448     default:
1449         break;
1450     }
1451
1452     if (batteryVoltage >= 10) { // if voltage is 10v or more then display only 1 decimal place
1453         decimalPlaces = 1;
1454     } else {
1455         decimalPlaces = 2;
1456     }
1457     osdPrintFloat(element->buff, osdGetBatterySymbol(getBatteryAverageCellVoltage()), batteryVoltage, "", decimalPlaces, true, SYM_VOLT);
1458 }
1459
1460 static void osdElementMotorDiagnostics(osdElementParms_t *element)
1461 {
1462     int i = 0;
1463     const bool motorsRunning = areMotorsRunning();
1464     for (; i < getMotorCount(); i++) {
1465         if (motorsRunning) {
1466             element->buff[i] =  0x88 - scaleRange(motor[i], getMotorOutputLow(), getMotorOutputHigh(), 0, 8);
1467 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
1468             if (getEscRpm(i) < MOTOR_STOPPED_THRESHOLD_RPM) {
1469                 // Motor is not spinning properly. Mark as Stopped
1470                 element->buff[i] = 'S';
1471             }
1472 #endif
1473         } else {
1474             element->buff[i] =  0x88;
1475         }
1476     }
1477     element->buff[i] = '\0';
1478 }
1479
1480 static void osdElementNumericalHeading(osdElementParms_t *element)
1481 {
1482     const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
1483     tfp_sprintf(element->buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
1484 }
1485
1486 #ifdef USE_VARIO
1487 static void osdElementNumericalVario(osdElementParms_t *element)
1488 {
1489     bool haveBaro = false;
1490     bool haveGps = false;
1491 #ifdef USE_BARO
1492     haveBaro = sensors(SENSOR_BARO);
1493 #endif // USE_BARO
1494 #ifdef USE_GPS
1495     haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
1496 #endif // USE_GPS
1497     if (haveBaro || haveGps) {
1498         const float verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()) / 100.0f;
1499         const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SMALL_DOWN : SYM_ARROW_SMALL_UP;
1500         osdPrintFloat(element->buff, directionSymbol, fabsf(verticalSpeed), "", 1, true, osdGetVarioToSelectedUnitSymbol());
1501     } else {
1502         // We use this symbol when we don't have a valid measure
1503         element->buff[0] = SYM_HYPHEN;
1504         element->buff[1] = '\0';
1505     }
1506 }
1507 #endif // USE_VARIO
1508
1509 static void osdElementPidRateProfile(osdElementParms_t *element)
1510 {
1511     tfp_sprintf(element->buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
1512 }
1513
1514 static void osdElementPidsPitch(osdElementParms_t *element)
1515 {
1516     osdFormatPID(element->buff, "PIT", PID_PITCH);
1517 }
1518
1519 static void osdElementPidsRoll(osdElementParms_t *element)
1520 {
1521     osdFormatPID(element->buff, "ROL", PID_ROLL);
1522 }
1523
1524 static void osdElementPidsYaw(osdElementParms_t *element)
1525 {
1526     osdFormatPID(element->buff, "YAW", PID_YAW);
1527 }
1528
1529 static void osdElementPower(osdElementParms_t *element)
1530 {
1531     tfp_sprintf(element->buff, "%4dW", getAmperage() * getBatteryVoltage() / 10000);
1532 }
1533
1534 static void osdElementRcChannels(osdElementParms_t *element)
1535 {
1536     static uint8_t channel = 0;
1537
1538     if (osdConfig()->rcChannels[channel] >= 0) {
1539         // Translate (1000, 2000) to (-1000, 1000)
1540         int data = scaleRange(rcData[osdConfig()->rcChannels[channel]], PWM_RANGE_MIN, PWM_RANGE_MAX, -1000, 1000);
1541         // Opt for the simplest formatting for now.
1542         // Decimal notation can be added when tfp_sprintf supports float among fancy options.
1543         tfp_sprintf(element->buff, "%5d", data);
1544         element->elemOffsetY = channel;
1545     }
1546
1547     if (++channel == OSD_RCCHANNELS_COUNT) {
1548         channel = 0;
1549     } else {
1550         element->rendered = false;
1551     }
1552 }
1553
1554 static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
1555 {
1556     const int mAhDrawn = getMAhDrawn();
1557
1558     if (mAhDrawn >= osdConfig()->cap_alarm) {
1559         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1560     }
1561
1562     if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) {  // also handles the mAhDrawn == 0 condition
1563         tfp_sprintf(element->buff, "--:--");
1564     } else if (mAhDrawn > osdConfig()->cap_alarm) {
1565         tfp_sprintf(element->buff, "00:00");
1566     } else {
1567         const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)osdFlyTime) / mAhDrawn);
1568         osdFormatTime(element->buff, OSD_TIMER_PREC_SECOND, remaining_time);
1569     }
1570 }
1571
1572 static void osdElementRssi(osdElementParms_t *element)
1573 {
1574     uint16_t osdRssi = getRssi() * 100 / 1024; // change range
1575     if (osdRssi >= 100) {
1576         osdRssi = 99;
1577     }
1578
1579     if (getRssiPercent() < osdConfig()->rssi_alarm) {
1580         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1581     }
1582
1583     tfp_sprintf(element->buff, "%c%2d", SYM_RSSI, osdRssi);
1584 }
1585
1586 #ifdef USE_RTC_TIME
1587 static void osdElementRtcTime(osdElementParms_t *element)
1588 {
1589     osdFormatRtcDateTime(&element->buff[0]);
1590 }
1591 #endif // USE_RTC_TIME
1592
1593 #ifdef USE_RX_RSSI_DBM
1594 static void osdElementRssiDbm(osdElementParms_t *element)
1595 {
1596     const int8_t antenna = getActiveAntenna();
1597     const int16_t osdRssiDbm = getRssiDbm();
1598     static bool diversity = false;
1599
1600     if (osdRssiDbm < osdConfig()->rssi_dbm_alarm) {
1601         element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1602     }
1603
1604     if (antenna || diversity) {
1605         diversity = true;
1606         tfp_sprintf(element->buff, "%c%3d:%d", SYM_RSSI, osdRssiDbm, antenna + 1);
1607     } else {
1608         tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, osdRssiDbm);
1609     }
1610 }
1611 #endif // USE_RX_RSSI_DBM
1612
1613 #ifdef USE_RX_RSNR
1614 static void osdElementRsnr(osdElementParms_t *element)
1615 {
1616     tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, getRsnr());
1617 }
1618 #endif // USE_RX_RSNR
1619
1620 #ifdef USE_OSD_STICK_OVERLAY
1621 static void osdBackgroundStickOverlay(osdElementParms_t *element)
1622 {
1623     static enum {VERT, HORZ} renderPhase = VERT;
1624
1625     if (renderPhase == VERT) {
1626         static uint8_t y = 0;
1627         tfp_sprintf(element->buff, "%c", SYM_STICK_OVERLAY_VERTICAL);
1628         element->elemOffsetX = ((OSD_STICK_OVERLAY_WIDTH - 1) / 2);
1629         element->elemOffsetY = y;
1630
1631         y++;
1632
1633         if (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2) {
1634             // Skip over horizontal
1635             y++;
1636         }
1637
1638         if (y == OSD_STICK_OVERLAY_HEIGHT) {
1639             y = 0;
1640             renderPhase = HORZ;
1641         }
1642
1643         element->rendered = false;
1644     } else {
1645         for (uint8_t i = 0; i < OSD_STICK_OVERLAY_WIDTH; i++) {
1646             element->buff[i] = SYM_STICK_OVERLAY_HORIZONTAL;
1647         }
1648         element->buff[((OSD_STICK_OVERLAY_WIDTH - 1) / 2)] = SYM_STICK_OVERLAY_CENTER;
1649         element->buff[OSD_STICK_OVERLAY_WIDTH] = 0;  // string terminator
1650
1651         element->elemOffsetY = ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2);
1652
1653         renderPhase = VERT;
1654     }
1655 }
1656
1657 static void osdElementStickOverlay(osdElementParms_t *element)
1658 {
1659     // Now draw the cursor
1660     rc_alias_e vertical_channel, horizontal_channel;
1661
1662     if (element->item == OSD_STICK_OVERLAY_LEFT) {
1663         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_vertical;
1664         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_horizontal;
1665     } else {
1666         vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_vertical;
1667         horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_horizontal;
1668     }
1669
1670     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);
1671     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);
1672     const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (cursorY % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
1673
1674     tfp_sprintf(element->buff, "%c", cursor);
1675     element->elemOffsetX = cursorX;
1676     element->elemOffsetY = cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT;
1677 }
1678 #endif // USE_OSD_STICK_OVERLAY
1679
1680 static void osdElementThrottlePosition(osdElementParms_t *element)
1681 {
1682     tfp_sprintf(element->buff, "%c%3d", SYM_THR, calculateThrottlePercent());
1683 }
1684
1685 static void osdElementTimer(osdElementParms_t *element)
1686 {
1687     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1688         const uint16_t timer = osdConfig()->timers[i];
1689         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
1690         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
1691         if (alarmTime != 0 && time >= alarmTime) {
1692             element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
1693         }
1694     }
1695
1696     osdFormatTimer(element->buff, true, true, element->item - OSD_ITEM_TIMER_1);
1697 }
1698
1699 #ifdef USE_VTX_COMMON
1700 static void osdElementVtxChannel(osdElementParms_t *element)
1701 {
1702     const vtxDevice_t *vtxDevice = vtxCommonDevice();
1703     uint8_t band = vtxSettingsConfigMutable()->band;
1704     uint8_t channel = vtxSettingsConfig()->channel;
1705     if (band == 0) {
1706         /* Direct frequency set is used */
1707         vtxCommonLookupBandChan(vtxDevice, vtxSettingsConfig()->freq, &band, &channel);
1708     }
1709     const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, band);
1710     const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, channel);
1711     unsigned vtxStatus = 0;
1712     uint8_t vtxPower = vtxSettingsConfig()->power;
1713     if (vtxDevice) {
1714         vtxCommonGetStatus(vtxDevice, &vtxStatus);
1715
1716         if (vtxSettingsConfig()->lowPowerDisarm) {
1717             vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
1718         }
1719     }
1720     const char *vtxPowerLabel = vtxCommonLookupPowerName(vtxDevice, vtxPower);
1721
1722     char vtxStatusIndicator = '\0';
1723     if (IS_RC_MODE_ACTIVE(BOXVTXCONTROLDISABLE)) {
1724         vtxStatusIndicator = 'D';
1725     } else if (vtxStatus & VTX_STATUS_PIT_MODE) {
1726         vtxStatusIndicator = 'P';
1727     }
1728
1729 switch (element->type) {
1730     case OSD_ELEMENT_TYPE_2:
1731             tfp_sprintf(element->buff, "%s", vtxPowerLabel);
1732         break;
1733
1734     default:
1735         if (vtxStatus & VTX_STATUS_LOCKED) {
1736             tfp_sprintf(element->buff, "-:-:-:L");
1737         } else if (vtxStatusIndicator) {
1738             tfp_sprintf(element->buff, "%c:%s:%s:%c", vtxBandLetter, vtxChannelName, vtxPowerLabel, vtxStatusIndicator);
1739         } else {
1740             tfp_sprintf(element->buff, "%c:%s:%s", vtxBandLetter, vtxChannelName, vtxPowerLabel);
1741         }
1742         break;
1743     }
1744 }
1745 #endif // USE_VTX_COMMON
1746
1747 static void osdElementAuxValue(osdElementParms_t *element)
1748 {
1749     tfp_sprintf(element->buff, "%c%d", osdConfig()->aux_symbol, osdAuxValue);
1750 }
1751
1752 static void osdElementWarnings(osdElementParms_t *element)
1753 {
1754     bool elementBlinking = false;
1755     renderOsdWarning(element->buff, &elementBlinking, &element->attr);
1756     if (elementBlinking) {
1757         SET_BLINK(OSD_WARNINGS);
1758     } else {
1759         CLR_BLINK(OSD_WARNINGS);
1760     }
1761
1762 #ifdef USE_CRAFTNAME_MSGS
1763     // Injects data into the CraftName variable for systems which limit
1764     // the available MSP data field in their OSD.
1765     if (osdConfig()->osd_craftname_msgs == true) {
1766         // if warning is not set, or blink is off, then display LQ & RSSI
1767         if (blinkState || (strlen(element->buff) == 0)) {
1768 #ifdef USE_RX_LINK_QUALITY_INFO
1769             // replicate the LQ functionality without the special font symbols
1770             uint16_t osdLinkQuality = 0;
1771             if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-99
1772                 osdLinkQuality = rxGetLinkQuality();
1773 #ifdef USE_RX_RSSI_DBM
1774                 const uint8_t osdRfMode = rxGetRfMode();
1775                 tfp_sprintf(element->buff, "LQ %2d:%03d %3d", osdRfMode, osdLinkQuality, getRssiDbm());
1776             } else if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_GHST) { // 0-100
1777                 osdLinkQuality = rxGetLinkQuality();
1778                 tfp_sprintf(element->buff, "LQ %03d %3d", osdLinkQuality, getRssiDbm());
1779 #endif
1780             } else { // 0-9
1781                 osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
1782                 if (osdLinkQuality >= 10) {
1783                     osdLinkQuality = 9;
1784                 }
1785                 tfp_sprintf(element->buff, "LQ %1d", osdLinkQuality);
1786             }
1787 #endif // USE_RX_LINK_QUALITY_INFO
1788         }
1789         strncpy(pilotConfigMutable()->craftName, element->buff, MAX_NAME_LENGTH - 1);
1790     }
1791 #endif // USE_CRAFTNAME_MSGS
1792 }
1793
1794 #ifdef USE_MSP_DISPLAYPORT
1795 static void osdElementSys(osdElementParms_t *element)
1796 {
1797     UNUSED(element);
1798
1799     // Nothing to render for a system element
1800 }
1801 #endif
1802
1803 // Define the order in which the elements are drawn.
1804 // Elements positioned later in the list will overlay the earlier
1805 // ones if their character positions overlap
1806 // Elements that need special runtime conditional processing should be added
1807 // to osdAddActiveElements()
1808
1809 static const uint8_t osdElementDisplayOrder[] = {
1810     OSD_MAIN_BATT_VOLTAGE,
1811     OSD_RSSI_VALUE,
1812     OSD_CROSSHAIRS,
1813     OSD_HORIZON_SIDEBARS,
1814     OSD_UP_DOWN_REFERENCE,
1815     OSD_ITEM_TIMER_1,
1816     OSD_ITEM_TIMER_2,
1817     OSD_REMAINING_TIME_ESTIMATE,
1818     OSD_FLYMODE,
1819     OSD_THROTTLE_POS,
1820     OSD_VTX_CHANNEL,
1821     OSD_CURRENT_DRAW,
1822     OSD_MAH_DRAWN,
1823     OSD_WATT_HOURS_DRAWN,
1824     OSD_CRAFT_NAME,
1825     OSD_CUSTOM_MSG0,
1826     OSD_CUSTOM_MSG1,
1827     OSD_CUSTOM_MSG2,
1828     OSD_CUSTOM_MSG3,
1829     OSD_ALTITUDE,
1830     OSD_ROLL_PIDS,
1831     OSD_PITCH_PIDS,
1832     OSD_YAW_PIDS,
1833     OSD_POWER,
1834     OSD_PIDRATE_PROFILE,
1835     OSD_WARNINGS,
1836     OSD_AVG_CELL_VOLTAGE,
1837     OSD_DEBUG,
1838     OSD_DEBUG2,
1839     OSD_PITCH_ANGLE,
1840     OSD_ROLL_ANGLE,
1841     OSD_MAIN_BATT_USAGE,
1842     OSD_DISARMED,
1843     OSD_NUMERICAL_HEADING,
1844     OSD_READY_MODE,
1845 #ifdef USE_VARIO
1846     OSD_NUMERICAL_VARIO,
1847 #endif
1848     OSD_COMPASS_BAR,
1849     OSD_ANTI_GRAVITY,
1850 #ifdef USE_BLACKBOX
1851     OSD_LOG_STATUS,
1852 #endif
1853     OSD_MOTOR_DIAG,
1854 #ifdef USE_ACC
1855     OSD_FLIP_ARROW,
1856 #endif
1857     OSD_PILOT_NAME,
1858 #ifdef USE_RTC_TIME
1859     OSD_RTC_DATETIME,
1860 #endif
1861 #ifdef USE_OSD_ADJUSTMENTS
1862     OSD_ADJUSTMENT_RANGE,
1863 #endif
1864 #ifdef USE_ADC_INTERNAL
1865     OSD_CORE_TEMPERATURE,
1866 #endif
1867 #ifdef USE_RX_LINK_QUALITY_INFO
1868     OSD_LINK_QUALITY,
1869 #endif
1870 #ifdef USE_RX_LINK_UPLINK_POWER
1871     OSD_TX_UPLINK_POWER,
1872 #endif
1873 #ifdef USE_RX_RSSI_DBM
1874     OSD_RSSI_DBM_VALUE,
1875 #endif
1876 #ifdef USE_RX_RSNR
1877     OSD_RSNR_VALUE,
1878 #endif
1879 #ifdef USE_OSD_STICK_OVERLAY
1880     OSD_STICK_OVERLAY_LEFT,
1881     OSD_STICK_OVERLAY_RIGHT,
1882 #endif
1883 #ifdef USE_PROFILE_NAMES
1884     OSD_RATE_PROFILE_NAME,
1885     OSD_PID_PROFILE_NAME,
1886 #endif
1887 #ifdef USE_OSD_PROFILES
1888     OSD_PROFILE_NAME,
1889 #endif
1890     OSD_RC_CHANNELS,
1891     OSD_CAMERA_FRAME,
1892 #ifdef USE_PERSISTENT_STATS
1893     OSD_TOTAL_FLIGHTS,
1894 #endif
1895     OSD_AUX_VALUE,
1896 #ifdef USE_OSD_HD
1897     OSD_SYS_GOGGLE_VOLTAGE,
1898     OSD_SYS_VTX_VOLTAGE,
1899     OSD_SYS_BITRATE,
1900     OSD_SYS_DELAY,
1901     OSD_SYS_DISTANCE,
1902     OSD_SYS_LQ,
1903     OSD_SYS_GOGGLE_DVR,
1904     OSD_SYS_VTX_DVR,
1905     OSD_SYS_WARNINGS,
1906     OSD_SYS_VTX_TEMP,
1907     OSD_SYS_FAN_SPEED,
1908 #endif
1909 };
1910
1911 // Define the mapping between the OSD element id and the function to draw it
1912
1913 const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
1914     [OSD_CAMERA_FRAME]            = NULL,  // only has background. Added first so it's the lowest "layer" and doesn't cover other elements
1915     [OSD_RSSI_VALUE]              = osdElementRssi,
1916     [OSD_MAIN_BATT_VOLTAGE]       = osdElementMainBatteryVoltage,
1917     [OSD_CROSSHAIRS]              = osdElementCrosshairs,  // only has background, but needs to be over other elements (like artificial horizon)
1918 #ifdef USE_ACC
1919     [OSD_ARTIFICIAL_HORIZON]      = osdElementArtificialHorizon,
1920     [OSD_UP_DOWN_REFERENCE]       = osdElementUpDownReference,
1921 #endif
1922     [OSD_HORIZON_SIDEBARS]        = NULL,  // only has background
1923     [OSD_ITEM_TIMER_1]            = osdElementTimer,
1924     [OSD_ITEM_TIMER_2]            = osdElementTimer,
1925     [OSD_FLYMODE]                 = osdElementFlymode,
1926     [OSD_CRAFT_NAME]              = NULL,  // only has background
1927     [OSD_CUSTOM_MSG0]             = osdElementCustomMsg,
1928     [OSD_CUSTOM_MSG1]             = osdElementCustomMsg,
1929     [OSD_CUSTOM_MSG2]             = osdElementCustomMsg,
1930     [OSD_CUSTOM_MSG3]             = osdElementCustomMsg,
1931     [OSD_THROTTLE_POS]            = osdElementThrottlePosition,
1932 #ifdef USE_VTX_COMMON
1933     [OSD_VTX_CHANNEL]             = osdElementVtxChannel,
1934 #endif
1935     [OSD_CURRENT_DRAW]            = osdElementCurrentDraw,
1936     [OSD_MAH_DRAWN]               = osdElementMahDrawn,
1937     [OSD_WATT_HOURS_DRAWN]        = osdElementWattHoursDrawn,
1938 #ifdef USE_GPS
1939     [OSD_GPS_SPEED]               = osdElementGpsSpeed,
1940     [OSD_GPS_SATS]                = osdElementGpsSats,
1941 #endif
1942     [OSD_ALTITUDE]                = osdElementAltitude,
1943     [OSD_ROLL_PIDS]               = osdElementPidsRoll,
1944     [OSD_PITCH_PIDS]              = osdElementPidsPitch,
1945     [OSD_YAW_PIDS]                = osdElementPidsYaw,
1946     [OSD_POWER]                   = osdElementPower,
1947     [OSD_PIDRATE_PROFILE]         = osdElementPidRateProfile,
1948     [OSD_WARNINGS]                = osdElementWarnings,
1949     [OSD_AVG_CELL_VOLTAGE]        = osdElementAverageCellVoltage,
1950     [OSD_READY_MODE]              = osdElementReadyMode,
1951 #ifdef USE_GPS
1952     [OSD_GPS_LON]                 = osdElementGpsCoordinate,
1953     [OSD_GPS_LAT]                 = osdElementGpsCoordinate,
1954 #endif
1955     [OSD_DEBUG]                   = osdElementDebug,
1956     [OSD_DEBUG2]                  = osdElementDebug2,
1957 #ifdef USE_ACC
1958     [OSD_PITCH_ANGLE]             = osdElementAngleRollPitch,
1959     [OSD_ROLL_ANGLE]              = osdElementAngleRollPitch,
1960 #endif
1961     [OSD_MAIN_BATT_USAGE]         = osdElementMainBatteryUsage,
1962     [OSD_DISARMED]                = osdElementDisarmed,
1963 #ifdef USE_GPS
1964     [OSD_HOME_DIR]                = osdElementGpsHomeDirection,
1965     [OSD_HOME_DIST]               = osdElementGpsHomeDistance,
1966 #endif
1967     [OSD_NUMERICAL_HEADING]       = osdElementNumericalHeading,
1968 #ifdef USE_VARIO
1969     [OSD_NUMERICAL_VARIO]         = osdElementNumericalVario,
1970 #endif
1971     [OSD_COMPASS_BAR]             = osdElementCompassBar,
1972 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
1973     [OSD_ESC_TMP]                 = osdElementEscTemperature,
1974     [OSD_ESC_RPM]                 = osdElementEscRpm,
1975 #endif
1976     [OSD_REMAINING_TIME_ESTIMATE] = osdElementRemainingTimeEstimate,
1977 #ifdef USE_RTC_TIME
1978     [OSD_RTC_DATETIME]            = osdElementRtcTime,
1979 #endif
1980 #ifdef USE_OSD_ADJUSTMENTS
1981     [OSD_ADJUSTMENT_RANGE]        = osdElementAdjustmentRange,
1982 #endif
1983 #ifdef USE_ADC_INTERNAL
1984     [OSD_CORE_TEMPERATURE]        = osdElementCoreTemperature,
1985 #endif
1986     [OSD_ANTI_GRAVITY]            = osdElementAntiGravity,
1987 #ifdef USE_ACC
1988     [OSD_G_FORCE]                 = osdElementGForce,
1989 #endif
1990     [OSD_MOTOR_DIAG]              = osdElementMotorDiagnostics,
1991 #ifdef USE_BLACKBOX
1992     [OSD_LOG_STATUS]              = osdElementLogStatus,
1993 #endif
1994 #ifdef USE_ACC
1995     [OSD_FLIP_ARROW]              = osdElementCrashFlipArrow,
1996 #endif
1997 #ifdef USE_RX_LINK_QUALITY_INFO
1998     [OSD_LINK_QUALITY]            = osdElementLinkQuality,
1999 #endif
2000 #ifdef USE_RX_LINK_UPLINK_POWER
2001     [OSD_TX_UPLINK_POWER]         = osdElementTxUplinkPower,
2002 #endif
2003 #ifdef USE_GPS
2004     [OSD_FLIGHT_DIST]             = osdElementGpsFlightDistance,
2005 #endif
2006 #ifdef USE_OSD_STICK_OVERLAY
2007     [OSD_STICK_OVERLAY_LEFT]      = osdElementStickOverlay,
2008     [OSD_STICK_OVERLAY_RIGHT]     = osdElementStickOverlay,
2009 #endif
2010     [OSD_PILOT_NAME]              = NULL,  // only has background
2011 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
2012     [OSD_ESC_RPM_FREQ]            = osdElementEscRpmFreq,
2013 #endif
2014 #ifdef USE_PROFILE_NAMES
2015     [OSD_RATE_PROFILE_NAME]       = osdElementRateProfileName,
2016     [OSD_PID_PROFILE_NAME]        = osdElementPidProfileName,
2017 #endif
2018 #ifdef USE_OSD_PROFILES
2019     [OSD_PROFILE_NAME]            = osdElementOsdProfileName,
2020 #endif
2021 #ifdef USE_RX_RSSI_DBM
2022     [OSD_RSSI_DBM_VALUE]          = osdElementRssiDbm,
2023 #endif
2024 #ifdef USE_RX_RSNR
2025     [OSD_RSNR_VALUE]              = osdElementRsnr,
2026 #endif
2027     [OSD_RC_CHANNELS]             = osdElementRcChannels,
2028 #ifdef USE_GPS
2029     [OSD_EFFICIENCY]              = osdElementEfficiency,
2030 #endif
2031 #ifdef USE_GPS_LAP_TIMER
2032     [OSD_GPS_LAP_TIME_CURRENT]    = osdElementGpsLapTimeCurrent,
2033     [OSD_GPS_LAP_TIME_PREVIOUS]   = osdElementGpsLapTimePrevious,
2034     [OSD_GPS_LAP_TIME_BEST3]      = osdElementGpsLapTimeBest3,
2035 #endif // GPS_LAP_TIMER
2036 #ifdef USE_PERSISTENT_STATS
2037     [OSD_TOTAL_FLIGHTS]           = osdElementTotalFlights,
2038 #endif
2039     [OSD_AUX_VALUE]               = osdElementAuxValue,
2040 #ifdef USE_MSP_DISPLAYPORT
2041     [OSD_SYS_GOGGLE_VOLTAGE]      = osdElementSys,
2042     [OSD_SYS_VTX_VOLTAGE]         = osdElementSys,
2043     [OSD_SYS_BITRATE]             = osdElementSys,
2044     [OSD_SYS_DELAY]               = osdElementSys,
2045     [OSD_SYS_DISTANCE]            = osdElementSys,
2046     [OSD_SYS_LQ]                  = osdElementSys,
2047     [OSD_SYS_GOGGLE_DVR]          = osdElementSys,
2048     [OSD_SYS_VTX_DVR]             = osdElementSys,
2049     [OSD_SYS_WARNINGS]            = osdElementSys,
2050     [OSD_SYS_VTX_TEMP]            = osdElementSys,
2051     [OSD_SYS_FAN_SPEED]           = osdElementSys,
2052 #endif
2053 };
2054
2055 // Define the mapping between the OSD element id and the function to draw its background (static part)
2056 // Only necessary to define the entries that actually have a background function
2057
2058 const osdElementDrawFn osdElementBackgroundFunction[OSD_ITEM_COUNT] = {
2059     [OSD_CAMERA_FRAME]            = osdBackgroundCameraFrame,
2060     [OSD_HORIZON_SIDEBARS]        = osdBackgroundHorizonSidebars,
2061     [OSD_CRAFT_NAME]              = osdBackgroundCraftName,
2062 #ifdef USE_OSD_STICK_OVERLAY
2063     [OSD_STICK_OVERLAY_LEFT]      = osdBackgroundStickOverlay,
2064     [OSD_STICK_OVERLAY_RIGHT]     = osdBackgroundStickOverlay,
2065 #endif
2066     [OSD_PILOT_NAME]              = osdBackgroundPilotName,
2067 };
2068
2069 static void osdAddActiveElement(osd_items_e element)
2070 {
2071     if (VISIBLE(osdElementConfig()->item_pos[element])) {
2072         activeOsdElementArray[activeOsdElementCount++] = element;
2073     }
2074 }
2075
2076 // Examine the elements and build a list of only the active (enabled)
2077 // ones to speed up rendering.
2078
2079 void osdAddActiveElements(void)
2080 {
2081     activeOsdElementCount = 0;
2082
2083 #ifdef USE_ACC
2084     if (sensors(SENSOR_ACC)) {
2085         osdAddActiveElement(OSD_ARTIFICIAL_HORIZON);
2086         osdAddActiveElement(OSD_G_FORCE);
2087         osdAddActiveElement(OSD_UP_DOWN_REFERENCE);
2088     }
2089 #endif
2090
2091     for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
2092         osdAddActiveElement(osdElementDisplayOrder[i]);
2093     }
2094
2095 #ifdef USE_GPS
2096     if (sensors(SENSOR_GPS)) {
2097         osdAddActiveElement(OSD_GPS_SATS);
2098         osdAddActiveElement(OSD_GPS_SPEED);
2099         osdAddActiveElement(OSD_GPS_LAT);
2100         osdAddActiveElement(OSD_GPS_LON);
2101         osdAddActiveElement(OSD_HOME_DIST);
2102         osdAddActiveElement(OSD_HOME_DIR);
2103         osdAddActiveElement(OSD_FLIGHT_DIST);
2104         osdAddActiveElement(OSD_EFFICIENCY);
2105     }
2106 #endif // GPS
2107
2108 #if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
2109     if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || useDshotTelemetry) {
2110         osdAddActiveElement(OSD_ESC_TMP);
2111         osdAddActiveElement(OSD_ESC_RPM);
2112         osdAddActiveElement(OSD_ESC_RPM_FREQ);
2113     }
2114 #endif
2115
2116 #ifdef USE_GPS_LAP_TIMER
2117     if (sensors(SENSOR_GPS)) {
2118         osdAddActiveElement(OSD_GPS_LAP_TIME_CURRENT);
2119         osdAddActiveElement(OSD_GPS_LAP_TIME_PREVIOUS);
2120         osdAddActiveElement(OSD_GPS_LAP_TIME_BEST3);
2121     }
2122 #endif // GPS_LAP_TIMER
2123
2124 #ifdef USE_PERSISTENT_STATS
2125     osdAddActiveElement(OSD_TOTAL_FLIGHTS);
2126 #endif
2127 }
2128
2129 static bool osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
2130 {
2131     // By default mark the element as rendered in case it's in the off blink state
2132     activeElement.rendered = true;
2133
2134     if (!osdElementDrawFunction[item]) {
2135         // Element has no drawing function
2136         return true;
2137     }
2138     if (!osdDisplayPort->useDeviceBlink && BLINK(item)) {
2139         return true;
2140     }
2141
2142     uint8_t elemPosX = OSD_X(osdElementConfig()->item_pos[item]);
2143     uint8_t elemPosY = OSD_Y(osdElementConfig()->item_pos[item]);
2144
2145     activeElement.item = item;
2146     activeElement.elemPosX = elemPosX;
2147     activeElement.elemPosY = elemPosY;
2148     activeElement.elemOffsetX = 0;
2149     activeElement.elemOffsetY = 0;
2150     activeElement.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
2151     activeElement.buff = elementBuff;
2152     activeElement.osdDisplayPort = osdDisplayPort;
2153     activeElement.drawElement = true;
2154     activeElement.attr = DISPLAYPORT_SEVERITY_NORMAL;
2155
2156     // Call the element drawing function
2157     if (IS_SYS_OSD_ELEMENT(item)) {
2158         displaySys(osdDisplayPort, elemPosX, elemPosY, (displayPortSystemElement_e)(item - OSD_SYS_GOGGLE_VOLTAGE + DISPLAYPORT_SYS_GOGGLE_VOLTAGE));
2159     } else {
2160         osdElementDrawFunction[item](&activeElement);
2161         if (activeElement.drawElement) {
2162             displayPendingForeground = true;
2163         }
2164     }
2165
2166     return activeElement.rendered;
2167 }
2168
2169 static bool osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_t item)
2170 {
2171     if (!osdElementBackgroundFunction[item]) {
2172         // Element has no background drawing function
2173         return true;
2174     }
2175
2176     uint8_t elemPosX = OSD_X(osdElementConfig()->item_pos[item]);
2177     uint8_t elemPosY = OSD_Y(osdElementConfig()->item_pos[item]);
2178
2179     activeElement.item = item;
2180     activeElement.elemPosX = elemPosX;
2181     activeElement.elemPosY = elemPosY;
2182     activeElement.elemOffsetX = 0;
2183     activeElement.elemOffsetY = 0;
2184     activeElement.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
2185     activeElement.buff = elementBuff;
2186     activeElement.osdDisplayPort = osdDisplayPort;
2187     activeElement.drawElement = true;
2188     activeElement.rendered = true;
2189     activeElement.attr = DISPLAYPORT_SEVERITY_NORMAL;
2190
2191     // Call the element background drawing function
2192     osdElementBackgroundFunction[item](&activeElement);
2193     if (activeElement.drawElement) {
2194         displayPendingBackground = true;
2195     }
2196
2197     return activeElement.rendered;
2198 }
2199
2200 static uint8_t activeElementNumber = 0;
2201
2202 bool osdIsRenderPending(void)
2203 {
2204     return displayPendingForeground | displayPendingBackground;
2205 }
2206
2207 uint8_t osdGetActiveElement(void)
2208 {
2209     return activeElementNumber;
2210 }
2211
2212 uint8_t osdGetActiveElementCount(void)
2213 {
2214     return activeOsdElementCount;
2215 }
2216
2217 // Return true if there is more to display
2218 bool osdDisplayActiveElement(void)
2219 {
2220     if (activeElementNumber >= activeOsdElementCount) {
2221         return false;
2222     }
2223
2224     // If there's a previously drawn background string to be displayed, do that
2225     if (displayPendingBackground) {
2226         osdDisplayWrite(&activeElement,
2227                         activeElement.elemPosX + activeElement.elemOffsetX,
2228                         activeElement.elemPosY + activeElement.elemOffsetY,
2229                         activeElement.attr, activeElement.buff);
2230
2231         activeElement.buff[0] = '\0';
2232
2233         displayPendingBackground = false;
2234
2235         return displayPendingForeground;
2236     }
2237
2238     // If there's a previously drawn foreground string to be displayed, do that
2239     if (displayPendingForeground) {
2240         osdDisplayWrite(&activeElement,
2241                         activeElement.elemPosX + activeElement.elemOffsetX,
2242                         activeElement.elemPosY + activeElement.elemOffsetY,
2243                         activeElement.attr, activeElement.buff);
2244
2245         activeElement.buff[0] = '\0';
2246
2247         displayPendingForeground = false;
2248     }
2249
2250     return false;
2251 }
2252
2253 // Return true if there are more elements to draw
2254 bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort)
2255 {
2256     static bool backgroundRendered = false;
2257
2258     if (activeElementNumber >= activeOsdElementCount) {
2259         activeElementNumber = 0;
2260         return false;
2261     }
2262
2263     uint8_t item = activeOsdElementArray[activeElementNumber];
2264
2265     if (!backgroundLayerSupported && osdElementBackgroundFunction[item] && !backgroundRendered) {
2266         // If the background layer isn't supported then we
2267         // have to draw the element's static layer as well.
2268         backgroundRendered = osdDrawSingleElementBackground(osdDisplayPort, item);
2269
2270         // After the background always come back to check for foreground
2271         return true;
2272     }
2273
2274     // Only advance to the next element if rendering is complete
2275     if (osdDrawSingleElement(osdDisplayPort, item)) {
2276         // If rendering is complete then advance to the next element
2277
2278         // Prepare to render the background of the next element
2279         backgroundRendered = false;
2280
2281         if (++activeElementNumber >= activeOsdElementCount) {
2282             activeElementNumber = 0;
2283             return false;
2284         }
2285     }
2286
2287     return true;
2288 }
2289
2290 #ifdef USE_SPEC_PREARM_SCREEN
2291 bool osdDrawSpec(displayPort_t *osdDisplayPort)
2292 {
2293     static enum {RPM, POLES, MIXER, THR, MOTOR, BAT, VER} specState = RPM;
2294     static int currentRow;
2295
2296     const uint8_t midRow = osdDisplayPort->rows / 2;
2297     const uint8_t midCol = osdDisplayPort->cols / 2;
2298
2299     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
2300
2301     int len = 0;
2302
2303     switch (specState) {
2304     default:
2305     case RPM:
2306         currentRow = midRow - 3;
2307 #ifdef USE_RPM_LIMIT
2308         {
2309             const bool rpmLimitActive = mixerConfig()->rpm_limit > 0 && isMotorProtocolBidirDshot();
2310             if (rpmLimitActive) {
2311                 len = tfp_sprintf(buff, "RPM LIMIT ON  %d", mixerConfig()->rpm_limit_value);
2312             } else {
2313                 len = tfp_sprintf(buff, "%s", "RPM LIMIT OFF");
2314             }
2315             displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, buff);
2316
2317             if (rpmLimitActive) {
2318                 specState = POLES;
2319             } else {
2320                 specState = THR;
2321             }
2322         }
2323         break;
2324
2325     case POLES:
2326         len = tfp_sprintf(buff, "KV %d   POLES %d", motorConfig()->kv, motorConfig()->motorPoleCount);
2327         displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, buff);
2328
2329         specState = MIXER;
2330         break;
2331
2332     case MIXER:
2333         len = tfp_sprintf(buff, "%d  %d  %d", mixerConfig()->rpm_limit_p, mixerConfig()->rpm_limit_i, mixerConfig()->rpm_limit_d);
2334         displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, buff);
2335
2336         specState = THR;
2337         break;
2338
2339     case THR:
2340 #endif // #USE_RPM_LIMIT
2341         len = tfp_sprintf(buff, "THR LIMIT %s", lookupTableThrottleLimitType[currentControlRateProfile->throttle_limit_type]);
2342         if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) {
2343             len = tfp_sprintf(buff, "%s %d", buff, currentControlRateProfile->throttle_limit_percent);
2344         }
2345         displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, buff);
2346
2347         specState = MOTOR;
2348         break;
2349
2350     case MOTOR:
2351         len = tfp_sprintf(buff, "MOTOR LIMIT %d", currentPidProfile->motor_output_limit);
2352         displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, buff);
2353
2354         specState = BAT;
2355         break;
2356
2357     case BAT:
2358         {
2359             const float batteryVoltage = getBatteryVoltage() / 100.0f;
2360             len = osdPrintFloat(buff, osdGetBatterySymbol(getBatteryAverageCellVoltage()), batteryVoltage, "", 2, true, SYM_VOLT);
2361             displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, buff);
2362         }
2363
2364         specState = VER;
2365         break;
2366
2367     case VER:
2368         len = strlen(FC_VERSION_STRING);
2369         displayWrite(osdDisplayPort, midCol - (len / 2), currentRow++, DISPLAYPORT_SEVERITY_NORMAL, FC_VERSION_STRING);
2370
2371         specState = RPM;
2372
2373         return true;
2374     }
2375
2376     return false;
2377 }
2378 #endif // USE_SPEC_PREARM_SCREEN
2379
2380 void osdDrawActiveElementsBackground(displayPort_t *osdDisplayPort)
2381 {
2382     if (backgroundLayerSupported) {
2383         displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_BACKGROUND);
2384         displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_WAIT);
2385         for (unsigned i = 0; i < activeOsdElementCount; i++) {
2386             while (!osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[i]));
2387         }
2388         displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND);
2389     }
2390 }
2391
2392 void osdElementsInit(bool backgroundLayerFlag)
2393 {
2394     backgroundLayerSupported = backgroundLayerFlag;
2395     activeOsdElementCount = 0;
2396     pt1FilterInit(&batteryEfficiencyFilt, pt1FilterGain(EFFICIENCY_CUTOFF_HZ, 1.0f / osdConfig()->framerate_hz));
2397 }
2398
2399 void osdSyncBlink(timeUs_t currentTimeUs)
2400 {
2401     const int period = 1000000/OSD_BLINK_FREQUENCY_HZ;
2402
2403     blinkState = ((currentTimeUs % period) < (period >> 1));
2404 }
2405
2406 void osdResetAlarms(void)
2407 {
2408     memset(blinkBits, 0, sizeof(blinkBits));
2409 }
2410
2411 void osdUpdateAlarms(void)
2412 {
2413     // This is overdone?
2414
2415     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
2416
2417     if (getRssiPercent() < osdConfig()->rssi_alarm) {
2418         SET_BLINK(OSD_RSSI_VALUE);
2419     } else {
2420         CLR_BLINK(OSD_RSSI_VALUE);
2421     }
2422
2423 #ifdef USE_RX_RSSI_DBM
2424     if (getRssiDbm() < osdConfig()->rssi_dbm_alarm) {
2425         SET_BLINK(OSD_RSSI_DBM_VALUE);
2426     } else {
2427         CLR_BLINK(OSD_RSSI_DBM_VALUE);
2428     }
2429 #endif
2430
2431 #ifdef USE_RX_LINK_QUALITY_INFO
2432     if (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm) {
2433         SET_BLINK(OSD_LINK_QUALITY);
2434     } else {
2435         CLR_BLINK(OSD_LINK_QUALITY);
2436     }
2437 #endif // USE_RX_LINK_QUALITY_INFO
2438
2439     if (getBatteryState() == BATTERY_OK) {
2440         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
2441         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
2442     } else {
2443         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
2444         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
2445     }
2446
2447 #ifdef USE_GPS
2448     if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < GPS_MIN_SAT_COUNT)
2449 #ifdef USE_GPS_RESCUE
2450             || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
2451 #endif
2452             ) {
2453         SET_BLINK(OSD_GPS_SATS);
2454     } else {
2455         CLR_BLINK(OSD_GPS_SATS);
2456     }
2457 #endif //USE_GPS
2458
2459     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
2460         const uint16_t timer = osdConfig()->timers[i];
2461         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
2462         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
2463         if (alarmTime != 0 && time >= alarmTime) {
2464             SET_BLINK(OSD_ITEM_TIMER_1 + i);
2465         } else {
2466             CLR_BLINK(OSD_ITEM_TIMER_1 + i);
2467         }
2468     }
2469
2470     if (getMAhDrawn() >= osdConfig()->cap_alarm) {
2471         SET_BLINK(OSD_MAH_DRAWN);
2472         SET_BLINK(OSD_MAIN_BATT_USAGE);
2473         SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
2474     } else {
2475         CLR_BLINK(OSD_MAH_DRAWN);
2476         CLR_BLINK(OSD_MAIN_BATT_USAGE);
2477         CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
2478     }
2479
2480     if ((alt >= osdConfig()->alt_alarm) && ARMING_FLAG(ARMED)) {
2481         SET_BLINK(OSD_ALTITUDE);
2482     } else {
2483         CLR_BLINK(OSD_ALTITUDE);
2484     }
2485
2486 #ifdef USE_GPS
2487     if (sensors(SENSOR_GPS) && ARMING_FLAG(ARMED) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
2488         if (osdConfig()->distance_alarm && GPS_distanceToHome >= osdConfig()->distance_alarm) {
2489             SET_BLINK(OSD_HOME_DIST);
2490         } else {
2491             CLR_BLINK(OSD_HOME_DIST);
2492         }
2493     } else {
2494         CLR_BLINK(OSD_HOME_DIST);
2495     }
2496 #endif
2497
2498 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
2499     bool blink;
2500
2501 #if defined(USE_ESC_SENSOR)
2502     if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
2503         // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
2504         blink = osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm;
2505     } else
2506 #endif
2507 #if defined(USE_DSHOT_TELEMETRY)
2508     {
2509         blink = false;
2510         if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF) {
2511             for (uint32_t k = 0; !blink && (k < getMotorCount()); k++) {
2512                 blink = (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0 &&
2513                     dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] >= osdConfig()->esc_temp_alarm;
2514             }
2515         }
2516     }
2517 #else
2518     {}
2519 #endif
2520
2521     if (blink) {
2522         SET_BLINK(OSD_ESC_TMP);
2523     } else {
2524         CLR_BLINK(OSD_ESC_TMP);
2525     }
2526 #endif
2527 }
2528
2529 #ifdef USE_ACC
2530 static bool osdElementIsActive(osd_items_e element)
2531 {
2532     for (unsigned i = 0; i < activeOsdElementCount; i++) {
2533         if (activeOsdElementArray[i] == element) {
2534             return true;
2535         }
2536     }
2537     return false;
2538 }
2539
2540 // Determine if any active elements need the ACC
2541 bool osdElementsNeedAccelerometer(void)
2542 {
2543     return osdElementIsActive(OSD_ARTIFICIAL_HORIZON) ||
2544            osdElementIsActive(OSD_PITCH_ANGLE) ||
2545            osdElementIsActive(OSD_ROLL_ANGLE) ||
2546            osdElementIsActive(OSD_G_FORCE) ||
2547            osdElementIsActive(OSD_FLIP_ARROW) ||
2548            osdElementIsActive(OSD_UP_DOWN_REFERENCE);
2549 }
2550
2551 #endif // USE_ACC
2552
2553 #endif // USE_OSD