src/main/osd/osd_elements.c

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