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