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