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