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