src/main/io/osd.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /*
  19  Created by Marcin Baliniak
  20  some functions based on MinimOSD
  21
  22  OSD-CMS separation by jflyper
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <ctype.h>
  30 #include <math.h>
  31
  32 #include "platform.h"
  33
  34 FILE_COMPILE_FOR_SPEED
  35
  36 #ifdef USE_OSD
  37
  38 #include "build/debug.h"
  39 #include "build/version.h"
  40
  41 #include "cms/cms.h"
  42 #include "cms/cms_types.h"
  43 #include "cms/cms_menu_osd.h"
  44
  45 #include "common/axis.h"
  46 #include "common/constants.h"
  47 #include "common/filter.h"
  48 #include "common/log.h"
  49 #include "common/olc.h"
  50 #include "common/printf.h"
  51 #include "common/string_light.h"
  52 #include "common/time.h"
  53 #include "common/typeconversion.h"
  54 #include "common/utils.h"
  55
  56 #include "config/feature.h"
  57 #include "config/parameter_group.h"
  58 #include "config/parameter_group_ids.h"
  59
  60 #include "drivers/display.h"
  61 #include "drivers/display_canvas.h"
  62 #include "drivers/display_font_metadata.h"
  63 #include "drivers/osd_symbols.h"
  64 #include "drivers/time.h"
  65 #include "drivers/vtx_common.h"
  66
  67 #include "io/flashfs.h"
  68 #include "io/gps.h"
  69 #include "io/osd.h"
  70 #include "io/osd_common.h"
  71 #include "io/osd_hud.h"
  72 #include "io/vtx.h"
  73 #include "io/vtx_string.h"
  74
  75 #include "fc/config.h"
  76 #include "fc/controlrate_profile.h"
  77 #include "fc/fc_core.h"
  78 #include "fc/fc_tasks.h"
  79 #include "fc/rc_adjustments.h"
  80 #include "fc/rc_controls.h"
  81 #include "fc/rc_modes.h"
  82 #include "fc/runtime_config.h"
  83 #include "fc/settings.h"
  84
  85 #include "flight/imu.h"
  86 #include "flight/mixer.h"
  87 #include "flight/pid.h"
  88 #include "flight/rth_estimator.h"
  89 #include "flight/wind_estimator.h"
  90
  91 #include "navigation/navigation.h"
  92 #include "navigation/navigation_private.h"
  93
  94 #include "rx/rx.h"
  95 #include "rx/msp_override.h"
  96
  97 #include "sensors/acceleration.h"
  98 #include "sensors/battery.h"
  99 #include "sensors/boardalignment.h"
 100 #include "sensors/diagnostics.h"
 101 #include "sensors/sensors.h"
 102 #include "sensors/pitotmeter.h"
 103 #include "sensors/temperature.h"
 104 #include "sensors/esc_sensor.h"
 105
 106 #include "programming/logic_condition.h"
 107
 108 #ifdef USE_HARDWARE_REVISION_DETECTION
 109 #include "hardware_revision.h"
 110 #endif
 111
 112 #define VIDEO_BUFFER_CHARS_PAL    480
 113
 114 #define GFORCE_FILTER_TC 0.2
 115
 116 #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
 117
 118 #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms
 119 #define ARMED_SCREEN_DISPLAY_TIME 1500 // ms
 120 #define STATS_SCREEN_DISPLAY_TIME 60000 // ms
 121
 122 #define EFFICIENCY_UPDATE_INTERVAL (5 * 1000)
 123
 124 // Adjust OSD_MESSAGE's default position when
 125 // changing OSD_MESSAGE_LENGTH
 126 #define OSD_MESSAGE_LENGTH 28
 127 #define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices)
 128 #define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0'
 129 // Wrap all string constants intenteded for display as messages with
 130 // this macro to ensure compile time length validation.
 131 #define OSD_MESSAGE_STR(x) ({ \
 132     STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \
 133     x; \
 134 })
 135
 136 #define OSD_CHR_IS_NUM(c) (c >= '0' && c <= '9')
 137
 138 #define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2)
 139 #define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s))
 140
 141 #define OSD_MIN_FONT_VERSION 1
 142
 143 static unsigned currentLayout = 0;
 144 static int layoutOverride = -1;
 145 static bool hasExtendedFont = false; // Wether the font supports characters > 256
 146 static timeMs_t layoutOverrideUntil = 0;
 147 static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT];
 148 static float GForce, GForceAxis[XYZ_AXIS_COUNT];
 149
 150 typedef struct statistic_s {
 151     uint16_t max_speed;
 152     uint16_t min_voltage; // /100
 153     int16_t max_current; // /100
 154     int16_t max_power; // /100
 155     int16_t min_rssi;
 156     int32_t max_altitude;
 157     uint32_t max_distance;
 158 } statistic_t;
 159
 160 static statistic_t stats;
 161
 162 static timeUs_t resumeRefreshAt = 0;
 163 static bool refreshWaitForResumeCmdRelease;
 164
 165 static bool fullRedraw = false;
 166
 167 static uint8_t armState;
 168
 169 typedef struct osdMapData_s {
 170     uint32_t scale;
 171     char referenceSymbol;
 172 } osdMapData_t;
 173
 174 static osdMapData_t osdMapData;
 175
 176 static displayPort_t *osdDisplayPort;
 177 static bool osdDisplayIsReady = false;
 178 #if defined(USE_CANVAS)
 179 static displayCanvas_t osdCanvas;
 180 static bool osdDisplayHasCanvas;
 181 #else
 182 #define osdDisplayHasCanvas false
 183 #endif
 184
 185 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 186
 187 PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13);
 188 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 189
 190 static int digitCount(int32_t value)
 191 {
 192     int digits = 1;
 193     while(1) {
 194         value = value / 10;
 195         if (value == 0) {
 196             break;
 197         }
 198         digits++;
 199     }
 200     return digits;
 201 }
 202
 203 bool osdDisplayIsPAL(void)
 204 {
 205     return displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL;
 206 }
 207
 208 /**
 209  * Formats a number given in cents, to support non integer values
 210  * without using floating point math. Value is always right aligned
 211  * and spaces are inserted before the number to always yield a string
 212  * of the same length. If the value doesn't fit into the provided length
 213  * it will be divided by scale and true will be returned.
 214  */
 215 bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length)
 216 {
 217     char *ptr = buff;
 218     char *dec;
 219     int decimals = maxDecimals;
 220     bool negative = false;
 221     bool scaled = false;
 222
 223     buff[length] = '\0';
 224
 225     if (centivalue < 0) {
 226         negative = true;
 227         centivalue = -centivalue;
 228         length--;
 229     }
 230
 231     int32_t integerPart = centivalue / 100;
 232     // 3 decimal digits
 233     int32_t millis = (centivalue % 100) * 10;
 234
 235     int digits = digitCount(integerPart);
 236     int remaining = length - digits;
 237
 238     if (remaining < 0 && scale > 0) {
 239         // Reduce by scale
 240         scaled = true;
 241         decimals = maxScaledDecimals;
 242         integerPart = integerPart / scale;
 243         // Multiply by 10 to get 3 decimal digits
 244         millis = ((centivalue % (100 * scale)) * 10) / scale;
 245         digits = digitCount(integerPart);
 246         remaining = length - digits;
 247     }
 248
 249     // 3 decimals at most
 250     decimals = MIN(remaining, MIN(decimals, 3));
 251     remaining -= decimals;
 252
 253     // Done counting. Time to write the characters.
 254
 255     // Write spaces at the start
 256     while (remaining > 0) {
 257         *ptr = SYM_BLANK;
 258         ptr++;
 259         remaining--;
 260     }
 261
 262     // Write the minus sign if required
 263     if (negative) {
 264         *ptr = '-';
 265         ptr++;
 266     }
 267     // Now write the digits.
 268     ui2a(integerPart, 10, 0, ptr);
 269     ptr += digits;
 270     if (decimals > 0) {
 271         *(ptr-1) += SYM_ZERO_HALF_TRAILING_DOT - '0';
 272         dec = ptr;
 273         int factor = 3; // we're getting the decimal part in millis first
 274         while (decimals < factor) {
 275             factor--;
 276             millis /= 10;
 277         }
 278         int decimalDigits = digitCount(millis);
 279         while (decimalDigits < decimals) {
 280             decimalDigits++;
 281             *ptr = '0';
 282             ptr++;
 283         }
 284         ui2a(millis, 10, 0, ptr);
 285         *dec += SYM_ZERO_HALF_LEADING_DOT - '0';
 286     }
 287     return scaled;
 288 }
 289
 290 /*
 291  * Aligns text to the left side. Adds spaces at the end to keep string length unchanged.
 292  */
 293 static void osdLeftAlignString(char *buff)
 294 {
 295     uint8_t sp = 0, ch = 0;
 296     uint8_t len = strlen(buff);
 297     while (buff[sp] == ' ') sp++;
 298     for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp];
 299     for (sp = ch; sp < len; sp++) buff[sp] = ' ';
 300 }
 301
 302 /**
 303  * Converts distance into a string based on the current unit system
 304  * prefixed by a a symbol to indicate the unit used.
 305  * @param dist Distance in centimeters
 306  */
 307 static void osdFormatDistanceSymbol(char *buff, int32_t dist)
 308 {
 309     switch ((osd_unit_e)osdConfig()->units) {
 310     case OSD_UNIT_IMPERIAL:
 311         if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) {
 312             buff[3] = SYM_DIST_MI;
 313         } else {
 314             buff[3] = SYM_DIST_FT;
 315         }
 316         buff[4] = '\0';
 317         break;
 318     case OSD_UNIT_UK:
 319         FALLTHROUGH;
 320     case OSD_UNIT_METRIC:
 321         if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, 0, 3, 3)) {
 322             buff[3] = SYM_DIST_KM;
 323         } else {
 324             buff[3] = SYM_DIST_M;
 325         }
 326         buff[4] = '\0';
 327         break;
 328     }
 329 }
 330
 331 /**
 332  * Converts distance into a string based on the current unit system.
 333  * @param dist Distance in centimeters
 334  */
 335 static void osdFormatDistanceStr(char *buff, int32_t dist)
 336 {
 337  int32_t centifeet;
 338  switch ((osd_unit_e)osdConfig()->units) {
 339  case OSD_UNIT_IMPERIAL:
 340     centifeet = CENTIMETERS_TO_CENTIFEET(dist);
 341     if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
 342         // Show feet when dist < 0.5mi
 343         tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT);
 344     } else {
 345         // Show miles when dist >= 0.5mi
 346         tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)),
 347         (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI);
 348     }
 349     break;
 350  case OSD_UNIT_UK:
 351      FALLTHROUGH;
 352  case OSD_UNIT_METRIC:
 353     if (abs(dist) < METERS_PER_KILOMETER * 100) {
 354         // Show meters when dist < 1km
 355         tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M);
 356     } else {
 357         // Show kilometers when dist >= 1km
 358         tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)),
 359             (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM);
 360      }
 361      break;
 362  }
 363 }
 364
 365 /**
 366  * Converts velocity based on the current unit system (kmh or mph).
 367  * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second)
 368  */
 369 static int32_t osdConvertVelocityToUnit(int32_t vel)
 370 {
 371     switch ((osd_unit_e)osdConfig()->units) {
 372     case OSD_UNIT_UK:
 373         FALLTHROUGH;
 374     case OSD_UNIT_IMPERIAL:
 375         return (vel * 224) / 10000; // Convert to mph
 376     case OSD_UNIT_METRIC:
 377         return (vel * 36) / 1000;   // Convert to kmh
 378     }
 379     // Unreachable
 380     return -1;
 381 }
 382
 383 /**
 384  * Converts velocity into a string based on the current unit system.
 385  * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
 386  */
 387 void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D)
 388 {
 389     switch ((osd_unit_e)osdConfig()->units) {
 390     case OSD_UNIT_UK:
 391         FALLTHROUGH;
 392     case OSD_UNIT_IMPERIAL:
 393         tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH));
 394         break;
 395     case OSD_UNIT_METRIC:
 396         tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH));
 397         break;
 398     }
 399 }
 400
 401 /**
 402  * Converts wind speed into a string based on the current unit system, using
 403  * always 3 digits and an additional character for the unit at the right. buff
 404  * is null terminated.
 405  * @param ws Raw wind speed in cm/s
 406  */
 407 #ifdef USE_WIND_ESTIMATOR
 408 static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
 409 {
 410     int32_t centivalue;
 411     char suffix;
 412     switch (osdConfig()->units) {
 413         case OSD_UNIT_UK:
 414             FALLTHROUGH;
 415         case OSD_UNIT_IMPERIAL:
 416             centivalue = (ws * 224) / 100;
 417             suffix = SYM_MPH;
 418             break;
 419         case OSD_UNIT_METRIC:
 420             centivalue = (ws * 36) / 10;
 421             suffix = SYM_KMH;
 422             break;
 423     }
 424     if (isValid) {
 425         osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
 426     } else {
 427         buff[0] = buff[1] = buff[2] = '-';
 428     }
 429     buff[3] = suffix;
 430     buff[4] = '\0';
 431 }
 432 #endif
 433
 434 /**
 435 * Converts altitude into a string based on the current unit system
 436 * prefixed by a a symbol to indicate the unit used.
 437 * @param alt Raw altitude/distance (i.e. as taken from baro.BaroAlt in centimeters)
 438 */
 439 void osdFormatAltitudeSymbol(char *buff, int32_t alt)
 440 {
 441     switch ((osd_unit_e)osdConfig()->units) {
 442         case OSD_UNIT_UK:
 443             FALLTHROUGH;
 444         case OSD_UNIT_IMPERIAL:
 445             if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) {
 446                 // Scaled to kft
 447                 buff[3] = SYM_ALT_KFT;
 448             } else {
 449                 // Formatted in feet
 450                 buff[3] = SYM_ALT_FT;
 451             }
 452             buff[4] = '\0';
 453             break;
 454         case OSD_UNIT_METRIC:
 455             // alt is alredy in cm
 456             if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) {
 457                 // Scaled to km
 458                 buff[3] = SYM_ALT_KM;
 459             } else {
 460                 // Formatted in m
 461                 buff[3] = SYM_ALT_M;
 462             }
 463             buff[4] = '\0';
 464             break;
 465     }
 466 }
 467
 468 /**
 469 * Converts altitude into a string based on the current unit system.
 470 * @param alt Raw altitude/distance (i.e. as taken from baro.BaroAlt in centimeters)
 471 */
 472 static void osdFormatAltitudeStr(char *buff, int32_t alt)
 473 {
 474     int32_t value;
 475     switch ((osd_unit_e)osdConfig()->units) {
 476         case OSD_UNIT_IMPERIAL:
 477             value = CENTIMETERS_TO_FEET(alt);
 478             tfp_sprintf(buff, "%d%c", (int)value, SYM_FT);
 479             break;
 480         case OSD_UNIT_UK:
 481             FALLTHROUGH;
 482         case OSD_UNIT_METRIC:
 483             value = CENTIMETERS_TO_METERS(alt);
 484             tfp_sprintf(buff, "%d%c", (int)value, SYM_M);
 485             break;
 486     }
 487 }
 488
 489 static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h)
 490 {
 491     uint32_t value = seconds;
 492     char sym = sym_m;
 493     // Maximum value we can show in minutes is 99 minutes and 59 seconds
 494     if (seconds > (99 * 60) + 59) {
 495         sym = sym_h;
 496         value = seconds / 60;
 497     }
 498     buff[0] = sym;
 499     tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60));
 500 }
 501
 502 static inline void osdFormatOnTime(char *buff)
 503 {
 504     osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H);
 505 }
 506
 507 static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
 508 {
 509     uint32_t seconds = getFlightTime();
 510     osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H);
 511     if (attr && osdConfig()->time_alarm > 0) {
 512        if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) {
 513             TEXT_ATTRIBUTES_ADD_BLINK(*attr);
 514         }
 515     }
 516 }
 517
 518 /**
 519  * Converts RSSI into a % value used by the OSD.
 520  */
 521 static uint16_t osdConvertRSSI(void)
 522 {
 523     // change range to [0, 99]
 524     return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
 525 }
 526
 527 /**
 528 * Displays a temperature postfixed with a symbol depending on the current unit system
 529 * @param label to display
 530 * @param valid true if measurement is valid
 531 * @param temperature in deciDegrees Celcius
 532 */
 533 static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t symbol, const char *label, bool valid, int16_t temperature, int16_t alarm_min, int16_t alarm_max)
 534 {
 535     char buff[TEMPERATURE_LABEL_LEN + 2 < 6 ? 6 : TEMPERATURE_LABEL_LEN + 2];
 536     textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT;
 537     uint8_t valueXOffset = 0;
 538
 539     if (symbol) {
 540         buff[0] = symbol;
 541         buff[1] = '\0';
 542         displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
 543         valueXOffset = 1;
 544     }
 545 #ifdef USE_TEMPERATURE_SENSOR
 546     else if (label[0] != '\0') {
 547         uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN);
 548         memcpy(buff, label, label_len);
 549         memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len);
 550         buff[5] = '\0';
 551         displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
 552         valueXOffset = osdConfig()->temp_label_align == OSD_ALIGN_LEFT ? 5 : label_len + 1;
 553     }
 554 #else
 555     UNUSED(label);
 556 #endif
 557
 558     if (valid) {
 559
 560         if ((temperature <= alarm_min) || (temperature >= alarm_max)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
 561         if (osdConfig()->units == OSD_UNIT_IMPERIAL) temperature = temperature * 9 / 5.0f + 320;
 562         tfp_sprintf(buff, "%3d", temperature / 10);
 563
 564     } else
 565         strcpy(buff, "---");
 566
 567     buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C;
 568     buff[4] = '\0';
 569
 570     displayWriteWithAttr(osdDisplayPort, elemPosX + valueXOffset, elemPosY, buff, elemAttr);
 571 }
 572
 573 #ifdef USE_TEMPERATURE_SENSOR
 574 static void osdDisplayTemperatureSensor(uint8_t elemPosX, uint8_t elemPosY, uint8_t sensorIndex)
 575 {
 576     int16_t temperature;
 577     const bool valid = getSensorTemperature(sensorIndex, &temperature);
 578     const tempSensorConfig_t *sensorConfig = tempSensorConfig(sensorIndex);
 579     uint16_t symbol = sensorConfig->osdSymbol ? SYM_TEMP_SENSOR_FIRST + sensorConfig->osdSymbol - 1 : 0;
 580     osdDisplayTemperature(elemPosX, elemPosY, symbol, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
 581 }
 582 #endif
 583
 584 static void osdFormatCoordinate(char *buff, char sym, int32_t val)
 585 {
 586     // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals
 587     const int coordinateLength = osdConfig()->coordinate_digits + 1;
 588
 589     buff[0] = sym;
 590     int32_t integerPart = val / GPS_DEGREES_DIVIDER;
 591     // Latitude maximum integer width is 3 (-90) while
 592     // longitude maximum integer width is 4 (-180).
 593     int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart);
 594     // We can show up to 7 digits in decimalPart.
 595     int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER);
 596     STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits);
 597     int decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart);
 598     // Embbed the decimal separator
 599     buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0';
 600     buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0';
 601     // Fill up to coordinateLength with zeros
 602     int total = 1 + integerDigits + decimalDigits;
 603     while(total < coordinateLength) {
 604         buff[total] = '0';
 605         total++;
 606     }
 607     buff[coordinateLength] = '\0';
 608 }
 609
 610 static void osdFormatCraftName(char *buff)
 611 {
 612     if (strlen(systemConfig()->name) == 0)
 613             strcpy(buff, "CRAFT_NAME");
 614     else {
 615         for (int i = 0; i < MAX_NAME_LENGTH; i++) {
 616             buff[i] = sl_toupper((unsigned char)systemConfig()->name[i]);
 617             if (systemConfig()->name[i] == 0)
 618                 break;
 619         }
 620     }
 621 }
 622
 623 // Used twice, make sure it's exactly the same string
 624 // to save some memory
 625 #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
 626
 627 static const char * osdArmingDisabledReasonMessage(void)
 628 {
 629     switch (isArmingDisabledReason()) {
 630         case ARMING_DISABLED_FAILSAFE_SYSTEM:
 631             // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
 632             if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
 633                 if (failsafeIsReceivingRxData()) {
 634                     // If we're not using sticks, it means the ARM switch
 635                     // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
 636                     // yet
 637                     return OSD_MESSAGE_STR("TURN ARM SWITCH OFF");
 638                 }
 639                 // Not receiving RX data
 640                 return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
 641             }
 642             return OSD_MESSAGE_STR("DISABLED BY FAILSAFE");
 643         case ARMING_DISABLED_NOT_LEVEL:
 644             return OSD_MESSAGE_STR("AIRCRAFT IS NOT LEVEL");
 645         case ARMING_DISABLED_SENSORS_CALIBRATING:
 646             return OSD_MESSAGE_STR("SENSORS CALIBRATING");
 647         case ARMING_DISABLED_SYSTEM_OVERLOADED:
 648             return OSD_MESSAGE_STR("SYSTEM OVERLOADED");
 649         case ARMING_DISABLED_NAVIGATION_UNSAFE:
 650 #if defined(USE_NAV)
 651             // Check the exact reason
 652             switch (navigationIsBlockingArming(NULL)) {
 653                 case NAV_ARMING_BLOCKER_NONE:
 654                     break;
 655                 case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
 656                     return OSD_MESSAGE_STR("WAITING FOR GPS FIX");
 657                 case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
 658                     return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST");
 659                 case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
 660                     return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR");
 661                 case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
 662                     return OSD_MESSAGE_STR("JUMP WAYPOINT MISCONFIGURED");
 663             }
 664 #endif
 665             break;
 666         case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
 667             return OSD_MESSAGE_STR("COMPASS NOT CALIBRATED");
 668         case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:
 669             return OSD_MESSAGE_STR("ACCELEROMETER NOT CALIBRATED");
 670         case ARMING_DISABLED_ARM_SWITCH:
 671             return OSD_MESSAGE_STR("DISABLE ARM SWITCH FIRST");
 672         case ARMING_DISABLED_HARDWARE_FAILURE:
 673             {
 674                 if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) {
 675                     return OSD_MESSAGE_STR("GYRO FAILURE");
 676                 }
 677                 if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) {
 678                     return OSD_MESSAGE_STR("ACCELEROMETER FAILURE");
 679                 }
 680                 if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) {
 681                     return OSD_MESSAGE_STR("COMPASS FAILURE");
 682                 }
 683                 if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) {
 684                     return OSD_MESSAGE_STR("BAROMETER FAILURE");
 685                 }
 686                 if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) {
 687                     return OSD_MESSAGE_STR("GPS FAILURE");
 688                 }
 689                 if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) {
 690                     return OSD_MESSAGE_STR("RANGE FINDER FAILURE");
 691                 }
 692                 if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) {
 693                     return OSD_MESSAGE_STR("PITOT METER FAILURE");
 694                 }
 695             }
 696             return OSD_MESSAGE_STR("HARDWARE FAILURE");
 697         case ARMING_DISABLED_BOXFAILSAFE:
 698             return OSD_MESSAGE_STR("FAILSAFE MODE ENABLED");
 699         case ARMING_DISABLED_BOXKILLSWITCH:
 700             return OSD_MESSAGE_STR("KILLSWITCH MODE ENABLED");
 701         case ARMING_DISABLED_RC_LINK:
 702             return OSD_MESSAGE_STR("NO RC LINK");
 703         case ARMING_DISABLED_THROTTLE:
 704             return OSD_MESSAGE_STR("THROTTLE IS NOT LOW");
 705         case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
 706             return OSD_MESSAGE_STR("ROLLPITCH NOT CENTERED");
 707         case ARMING_DISABLED_SERVO_AUTOTRIM:
 708             return OSD_MESSAGE_STR("AUTOTRIM IS ACTIVE");
 709         case ARMING_DISABLED_OOM:
 710             return OSD_MESSAGE_STR("NOT ENOUGH MEMORY");
 711         case ARMING_DISABLED_INVALID_SETTING:
 712             return OSD_MESSAGE_STR("INVALID SETTING");
 713         case ARMING_DISABLED_CLI:
 714             return OSD_MESSAGE_STR("CLI IS ACTIVE");
 715         case ARMING_DISABLED_PWM_OUTPUT_ERROR:
 716             return OSD_MESSAGE_STR("PWM INIT ERROR");
 717             // Cases without message
 718         case ARMING_DISABLED_CMS_MENU:
 719             FALLTHROUGH;
 720         case ARMING_DISABLED_OSD_MENU:
 721             FALLTHROUGH;
 722         case ARMING_DISABLED_ALL_FLAGS:
 723             FALLTHROUGH;
 724         case ARMED:
 725             FALLTHROUGH;
 726         case WAS_EVER_ARMED:
 727             break;
 728     }
 729     return NULL;
 730 }
 731
 732 static const char * osdFailsafePhaseMessage(void)
 733 {
 734     // See failsafe.h for each phase explanation
 735     switch (failsafePhase()) {
 736 #ifdef USE_NAV
 737         case FAILSAFE_RETURN_TO_HOME:
 738             // XXX: Keep this in sync with OSD_FLYMODE.
 739             return OSD_MESSAGE_STR("(RTH)");
 740 #endif
 741         case FAILSAFE_LANDING:
 742             // This should be considered an emergengy landing
 743             return OSD_MESSAGE_STR("(EMERGENCY LANDING)");
 744         case FAILSAFE_RX_LOSS_MONITORING:
 745             // Only reachable from FAILSAFE_LANDED, which performs
 746             // a disarm. Since aircraft has been disarmed, we no
 747             // longer show failsafe details.
 748             FALLTHROUGH;
 749         case FAILSAFE_LANDED:
 750             // Very brief, disarms and transitions into
 751             // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents
 752             // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM,
 753             // so we'll show the user how to re-arm in when
 754             // that flag is the reason to prevent arming.
 755             FALLTHROUGH;
 756         case FAILSAFE_RX_LOSS_IDLE:
 757             // This only happens when user has chosen NONE as FS
 758             // procedure. The recovery messages should be enough.
 759             FALLTHROUGH;
 760         case FAILSAFE_IDLE:
 761             // Failsafe not active
 762             FALLTHROUGH;
 763         case FAILSAFE_RX_LOSS_DETECTED:
 764             // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED
 765             // or the FS procedure immediately.
 766             FALLTHROUGH;
 767         case FAILSAFE_RX_LOSS_RECOVERED:
 768             // Exiting failsafe
 769             break;
 770     }
 771     return NULL;
 772 }
 773
 774 static const char * osdFailsafeInfoMessage(void)
 775 {
 776     if (failsafeIsReceivingRxData()) {
 777         // User must move sticks to exit FS mode
 778         return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
 779     }
 780     return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
 781 }
 782
 783 static const char * navigationStateMessage(void)
 784 {
 785     switch (NAV_Status.state) {
 786         case MW_NAV_STATE_NONE:
 787             break;
 788         case MW_NAV_STATE_RTH_START:
 789             return OSD_MESSAGE_STR("STARTING RTH");
 790         case MW_NAV_STATE_RTH_ENROUTE:
 791             // TODO: Break this up between climb and head home
 792             return OSD_MESSAGE_STR("EN ROUTE TO HOME");
 793         case MW_NAV_STATE_HOLD_INFINIT:
 794             // Used by HOLD flight modes. No information to add.
 795             break;
 796         case MW_NAV_STATE_HOLD_TIMED:
 797             // TODO: Maybe we can display a count down
 798             return OSD_MESSAGE_STR("HOLDING WAYPOINT");
 799             break;
 800         case MW_NAV_STATE_WP_ENROUTE:
 801             // TODO: Show WP number
 802             return OSD_MESSAGE_STR("TO WP");
 803         case MW_NAV_STATE_PROCESS_NEXT:
 804             return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
 805         case MW_NAV_STATE_DO_JUMP:
 806             // Not used
 807             break;
 808         case MW_NAV_STATE_LAND_START:
 809             // Not used
 810             break;
 811         case MW_NAV_STATE_EMERGENCY_LANDING:
 812             return OSD_MESSAGE_STR("EMERGENCY LANDING");
 813         case MW_NAV_STATE_LAND_IN_PROGRESS:
 814             return OSD_MESSAGE_STR("LANDING");
 815         case MW_NAV_STATE_HOVER_ABOVE_HOME:
 816             if (STATE(FIXED_WING_LEGACY)) {
 817                 return OSD_MESSAGE_STR("LOITERING AROUND HOME");
 818             }
 819             return OSD_MESSAGE_STR("HOVERING");
 820         case MW_NAV_STATE_LANDED:
 821             return OSD_MESSAGE_STR("LANDED");
 822         case MW_NAV_STATE_LAND_SETTLE:
 823             return OSD_MESSAGE_STR("PREPARING TO LAND");
 824         case MW_NAV_STATE_LAND_START_DESCENT:
 825             // Not used
 826             break;
 827     }
 828     return NULL;
 829 }
 830
 831 static void osdFormatMessage(char *buff, size_t size, const char *message)
 832 {
 833     memset(buff, SYM_BLANK, size);
 834     if (message) {
 835         int messageLength = strlen(message);
 836         int rem = MAX(0, OSD_MESSAGE_LENGTH - (int)messageLength);
 837         // Don't finish the string at the end of the message,
 838         // write the rest of the blanks.
 839         strncpy(buff + rem / 2, message, MIN(OSD_MESSAGE_LENGTH - rem / 2, messageLength));
 840     }
 841     // Ensure buff is zero terminated
 842     buff[size - 1] = '\0';
 843 }
 844
 845 /**
 846  * Draws the battery symbol filled in accordingly to the
 847  * battery voltage to buff[0].
 848  **/
 849 static void osdFormatBatteryChargeSymbol(char *buff)
 850 {
 851     uint8_t p = calculateBatteryPercentage();
 852     p = (100 - p) / 16.6;
 853     buff[0] = SYM_BATT_FULL + p;
 854 }
 855
 856 static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *attr)
 857 {
 858     if ((getBatteryState() != BATTERY_NOT_PRESENT) && ((batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) || ((!batteryUsesCapacityThresholds()) && (getBatteryVoltage() <= getBatteryWarningVoltage()))))
 859         TEXT_ATTRIBUTES_ADD_BLINK(*attr);
 860 }
 861
 862 void osdCrosshairPosition(uint8_t *x, uint8_t *y)
 863 {
 864     *x = osdDisplayPort->cols / 2;
 865     *y = osdDisplayPort->rows / 2;
 866     *y += osdConfig()->horizon_offset;
 867 }
 868
 869 /**
 870  * Formats throttle position prefixed by its symbol. If autoThr
 871  * is true and the navigation system is controlling THR, it
 872  * uses the THR value applied by the system rather than the
 873  * input value received by the sticks.
 874  **/
 875 static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr)
 876 {
 877     buff[0] = SYM_BLANK;
 878     buff[1] = SYM_THR;
 879     int16_t thr = rxGetChannelValue(THROTTLE);
 880     if (autoThr && navigationIsControllingThrottle()) {
 881         buff[0] = SYM_AUTO_THR0;
 882         buff[1] = SYM_AUTO_THR1;
 883         thr = rcCommand[THROTTLE];
 884         if (isFixedWingAutoThrottleManuallyIncreased())
 885             TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
 886     }
 887     tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
 888 }
 889
 890 #if defined(USE_ESC_SENSOR)
 891 static void osdFormatRpm(char *buff, uint32_t rpm)
 892 {
 893     buff[0] = SYM_RPM;
 894     if (rpm) {
 895         if (rpm >= 1000) {
 896             osdFormatCentiNumber(buff + 1, rpm / 10, 0, 1, 1, 2);
 897             buff[3] = 'K';
 898             buff[4] = '\0';
 899         }
 900         else {
 901             tfp_sprintf(buff + 1, "%3lu", rpm);
 902         }
 903     }
 904     else {
 905         strcpy(buff + 1, "---");
 906     }
 907 }
 908 #endif
 909
 910 int32_t osdGetAltitude(void)
 911 {
 912 #if defined(USE_NAV)
 913     return getEstimatedActualPosition(Z);
 914 #elif defined(USE_BARO)
 915     return baro.alt;
 916 #else
 917     return 0;
 918 #endif
 919 }
 920
 921 static inline int32_t osdGetAltitudeMsl(void)
 922 {
 923 #if defined(USE_NAV)
 924     return getEstimatedActualPosition(Z)+GPS_home.alt;
 925 #elif defined(USE_BARO)
 926     return baro.alt+GPS_home.alt;
 927 #else
 928     return 0;
 929 #endif
 930 }
 931
 932 static bool osdIsHeadingValid(void)
 933 {
 934     return isImuHeadingValid();
 935 }
 936
 937 int16_t osdGetHeading(void)
 938 {
 939     return attitude.values.yaw;
 940 }
 941
 942 // Returns a heading angle in degrees normalized to [0, 360).
 943 int osdGetHeadingAngle(int angle)
 944 {
 945     while (angle < 0) {
 946         angle += 360;
 947     }
 948     while (angle >= 360) {
 949         angle -= 360;
 950     }
 951     return angle;
 952 }
 953
 954 #if defined(USE_GPS)
 955
 956 /* Draws a map with the given symbol in the center and given point of interest
 957  * defined by its distance in meters and direction in degrees.
 958  * referenceHeading indicates the up direction in the map, in degrees, while
 959  * referenceSym (if non-zero) is drawn at the upper right corner below a small
 960  * arrow to indicate the map reference to the user. The drawn argument is an
 961  * in-out used to store the last position where the craft was drawn to avoid
 962  * erasing all screen on each redraw.
 963  */
 964 static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t centerSym,
 965                        uint32_t poiDistance, int16_t poiDirection, uint8_t poiSymbol,
 966                        uint16_t *drawn, uint32_t *usedScale)
 967 {
 968     // TODO: These need to be tested with several setups. We might
 969     // need to make them configurable.
 970     const int hMargin = 5;
 971     const int vMargin = 3;
 972
 973     // TODO: Get this from the display driver?
 974     const int charWidth = 12;
 975     const int charHeight = 18;
 976
 977     uint8_t minX = hMargin;
 978     uint8_t maxX = osdDisplayPort->cols - 1 - hMargin;
 979     uint8_t minY = vMargin;
 980     uint8_t maxY = osdDisplayPort->rows - 1 - vMargin;
 981     uint8_t midX = osdDisplayPort->cols / 2;
 982     uint8_t midY = osdDisplayPort->rows / 2;
 983
 984     // Fixed marks
 985     displayWriteChar(osdDisplayPort, midX, midY, centerSym);
 986
 987     // First, erase the previous drawing.
 988     if (OSD_VISIBLE(*drawn)) {
 989         displayWriteChar(osdDisplayPort, OSD_X(*drawn), OSD_Y(*drawn), SYM_BLANK);
 990         *drawn = 0;
 991     }
 992
 993     uint32_t initialScale;
 994     const unsigned scaleMultiplier = 2;
 995     // We try to reduce the scale when the POI will be around half the distance
 996     // between the center and the closers map edge, to avoid too much jumping
 997     const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2;
 998
 999     switch (osdConfig()->units) {
1000         case OSD_UNIT_IMPERIAL:
1001             initialScale = 16; // 16m ~= 0.01miles
1002             break;
1003         case OSD_UNIT_UK:
1004             FALLTHROUGH;
1005         case OSD_UNIT_METRIC:
1006             initialScale = 10; // 10m as initial scale
1007             break;
1008     }
1009
1010     // Try to keep the same scale when getting closer until we draw over the center point
1011     uint32_t scale = initialScale;
1012     if (*usedScale) {
1013         scale = *usedScale;
1014         if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) {
1015             scale /= scaleMultiplier;
1016         }
1017     }
1018
1019     if (STATE(GPS_FIX)) {
1020
1021         int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
1022         float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
1023         float poiSin = sin_approx(poiAngle);
1024         float poiCos = cos_approx(poiAngle);
1025
1026         // Now start looking for a valid scale that lets us draw everything
1027         int ii;
1028         for (ii = 0; ii < 50; ii++) {
1029             // Calculate location of the aircraft in map
1030             int points = poiDistance / ((float)scale / charHeight);
1031
1032             float pointsX = points * poiSin;
1033             int poiX = midX - roundf(pointsX / charWidth);
1034             if (poiX < minX || poiX > maxX) {
1035                 scale *= scaleMultiplier;
1036                 continue;
1037             }
1038
1039             float pointsY = points * poiCos;
1040             int poiY = midY + roundf(pointsY / charHeight);
1041             if (poiY < minY || poiY > maxY) {
1042                 scale *= scaleMultiplier;
1043                 continue;
1044             }
1045
1046             if (poiX == midX && poiY == midY) {
1047                 // We're over the map center symbol, so we would be drawing
1048                 // over it even if we increased the scale. Alternate between
1049                 // drawing the center symbol or drawing the POI.
1050                 if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
1051                     break;
1052                 }
1053             } else {
1054
1055                 uint16_t c;
1056                 if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) {
1057                     // Something else written here, increase scale. If the display doesn't support reading
1058                     // back characters, we assume there's nothing.
1059                     //
1060                     // If we're close to the center, decrease scale. Otherwise increase it.
1061                     uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2);
1062                     uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2);
1063                     if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX &&
1064                         poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY &&
1065                         scale > scaleMultiplier) {
1066
1067                         scale /= scaleMultiplier;
1068                     } else {
1069                         scale *= scaleMultiplier;
1070                     }
1071                     continue;
1072                 }
1073             }
1074
1075             // Draw the point on the map
1076             if (poiSymbol == SYM_ARROW_UP) {
1077                 // Drawing aircraft, rotate
1078                 int mapHeading = osdGetHeadingAngle(DECIDEGREES_TO_DEGREES(osdGetHeading()) - referenceHeading);
1079                 poiSymbol += mapHeading * 2 / 45;
1080             }
1081             displayWriteChar(osdDisplayPort, poiX, poiY, poiSymbol);
1082
1083             // Update saved location
1084             *drawn = OSD_POS(poiX, poiY) | OSD_VISIBLE_FLAG;
1085             break;
1086         }
1087     }
1088
1089     *usedScale = scale;
1090
1091     // Update global map data for scale and reference
1092     osdMapData.scale = scale;
1093     osdMapData.referenceSymbol = referenceSym;
1094 }
1095
1096 /* Draws a map with the home in the center and the craft moving around.
1097  * See osdDrawMap() for reference.
1098  */
1099 static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn, uint32_t *usedScale)
1100 {
1101     osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale);
1102 }
1103
1104 /* Draws a map with the aircraft in the center and the home moving around.
1105  * See osdDrawMap() for reference.
1106  */
1107 static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
1108 {
1109     int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading());
1110     int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180);
1111     osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
1112 }
1113
1114 static int16_t osdGet3DSpeed(void)
1115 {
1116     int16_t vert_speed = getEstimatedActualVelocity(Z);
1117     int16_t hor_speed = gpsSol.groundSpeed;
1118     return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
1119 }
1120
1121 #endif
1122
1123 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
1124     strcpy(buff, label);
1125     for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' ';
1126     uint8_t decimals = showDecimal ? 1 : 0;
1127     osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4);
1128     buff[9] = ' ';
1129     osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4);
1130     buff[14] = ' ';
1131     osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4);
1132     buff[19] = ' ';
1133     osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4);
1134     buff[24] = '\0';
1135 }
1136
1137 static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals)
1138 {
1139     char buff[6];
1140     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
1141
1142     osdFormatBatteryChargeSymbol(buff);
1143     buff[1] = '\0';
1144     osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1145     displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
1146
1147     elemAttr = TEXT_ATTRIBUTES_NONE;
1148     digits = MIN(digits, 4);
1149     osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits);
1150     buff[digits] = SYM_VOLT;
1151     buff[digits+1] = '\0';
1152     if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage()))
1153         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1154     displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
1155 }
1156
1157 static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
1158 {
1159     textAttributes_t elemAttr;
1160     char buff[4];
1161
1162     const pid8_t *pid = &pidBank()->pid[pidIndex];
1163     pidType_e pidType = pidIndexGetType(pidIndex);
1164
1165     displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
1166
1167     if (pidType == PID_TYPE_NONE) {
1168         // PID is not used in this configuration. Draw dashes.
1169         // XXX: Keep this in sync with the %3d format and spacing used below
1170         displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "-   -   -");
1171         return;
1172     }
1173
1174     elemAttr = TEXT_ATTRIBUTES_NONE;
1175     tfp_sprintf(buff, "%3d", pid->P);
1176     if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
1177         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1178     displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1179
1180     elemAttr = TEXT_ATTRIBUTES_NONE;
1181     tfp_sprintf(buff, "%3d", pid->I);
1182     if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
1183         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1184     displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
1185
1186     elemAttr = TEXT_ATTRIBUTES_NONE;
1187     tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
1188     if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
1189         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1190     displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
1191 }
1192
1193 static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, const char *str, const uint8_t valueOffset, const float value, const uint8_t valueLength, const uint8_t maxDecimals, adjustmentFunction_e adjFunc) {
1194     char buff[8];
1195     textAttributes_t elemAttr;
1196     displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
1197
1198     elemAttr = TEXT_ATTRIBUTES_NONE;
1199     osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8));
1200     if (isAdjustmentFunctionSelected(adjFunc))
1201         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1202     displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr);
1203 }
1204
1205 static bool osdDrawSingleElement(uint8_t item)
1206 {
1207     uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
1208     if (!OSD_VISIBLE(pos)) {
1209         return false;
1210     }
1211     uint8_t elemPosX = OSD_X(pos);
1212     uint8_t elemPosY = OSD_Y(pos);
1213     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
1214     char buff[32];
1215
1216     switch (item) {
1217     case OSD_RSSI_VALUE:
1218         {
1219             uint16_t osdRssi = osdConvertRSSI();
1220             buff[0] = SYM_RSSI;
1221             tfp_sprintf(buff + 1, "%2d", osdRssi);
1222             if (osdRssi < osdConfig()->rssi_alarm) {
1223                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1224             }
1225             break;
1226         }
1227
1228     case OSD_MAIN_BATT_VOLTAGE:
1229         osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
1230         return true;
1231
1232     case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE:
1233         osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
1234         return true;
1235
1236     case OSD_CURRENT_DRAW:
1237         osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3);
1238         buff[3] = SYM_AMP;
1239         buff[4] = '\0';
1240
1241         uint8_t current_alarm = osdConfig()->current_alarm;
1242         if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) {
1243             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1244         }
1245         break;
1246
1247     case OSD_MAH_DRAWN:
1248         tfp_sprintf(buff, "%4d", (int)getMAhDrawn());
1249         buff[4] = SYM_MAH;
1250         buff[5] = '\0';
1251         osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1252         break;
1253
1254     case OSD_WH_DRAWN:
1255         osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
1256         osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1257         buff[3] = SYM_WH;
1258         buff[4] = '\0';
1259         break;
1260
1261     case OSD_BATTERY_REMAINING_CAPACITY:
1262
1263         if (currentBatteryProfile->capacity.value == 0)
1264             tfp_sprintf(buff, "  NA");
1265         else if (!batteryWasFullWhenPluggedIn())
1266             tfp_sprintf(buff, "  NF");
1267         else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH)
1268             tfp_sprintf(buff, "%4lu", getBatteryRemainingCapacity());
1269         else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
1270             osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3);
1271
1272         buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
1273         buff[5] = '\0';
1274
1275         if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical))
1276             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1277
1278         break;
1279
1280     case OSD_BATTERY_REMAINING_PERCENT:
1281         tfp_sprintf(buff, "%3d%%", calculateBatteryPercentage());
1282         osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1283         break;
1284
1285     case OSD_POWER_SUPPLY_IMPEDANCE:
1286         if (isPowerSupplyImpedanceValid())
1287             tfp_sprintf(buff, "%3d", getPowerSupplyImpedance());
1288         else
1289             strcpy(buff, "---");
1290         buff[3] = SYM_MILLIOHM;
1291         buff[4] = '\0';
1292         break;
1293
1294 #ifdef USE_GPS
1295     case OSD_GPS_SATS:
1296         buff[0] = SYM_SAT_L;
1297         buff[1] = SYM_SAT_R;
1298         tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
1299         if (!STATE(GPS_FIX)) {
1300             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1301         }
1302         break;
1303
1304     case OSD_GPS_SPEED:
1305         osdFormatVelocityStr(buff, gpsSol.groundSpeed, false);
1306         break;
1307
1308     case OSD_3D_SPEED:
1309         {
1310             osdFormatVelocityStr(buff, osdGet3DSpeed(), true);
1311             break;
1312         }
1313
1314     case OSD_GPS_LAT:
1315         osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat);
1316         break;
1317
1318     case OSD_GPS_LON:
1319         osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon);
1320         break;
1321
1322     case OSD_HOME_DIR:
1323         {
1324             if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
1325                 if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
1326                     displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
1327                 }
1328                 else
1329                 {
1330                     int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
1331                     osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
1332                 }
1333             } else {
1334                 // No home or no fix or unknown heading, blink.
1335                 // If we're unarmed, show the arrow pointing up so users can see the arrow
1336                 // while configuring the OSD. If we're armed, show a '-' indicating that
1337                 // we don't know the direction to home.
1338                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1339                 displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP, elemAttr);
1340             }
1341             return true;
1342         }
1343
1344     case OSD_HOME_HEADING_ERROR:
1345         {
1346             buff[0] = SYM_HOME;
1347             buff[1] = SYM_HEADING;
1348
1349             if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) {
1350                 int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading()))));
1351                 tfp_sprintf(buff + 2, "%4d", h);
1352             } else {
1353                 strcpy(buff + 2, "----");
1354             }
1355
1356             buff[6] = SYM_DEGREES;
1357             buff[7] = '\0';
1358             break;
1359         }
1360
1361     case OSD_HOME_DIST:
1362         {
1363             buff[0] = SYM_HOME;
1364             osdFormatDistanceSymbol(&buff[1], GPS_distanceToHome * 100);
1365             uint16_t dist_alarm = osdConfig()->dist_alarm;
1366             if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) {
1367                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1368             }
1369         }
1370         break;
1371
1372     case OSD_TRIP_DIST:
1373         buff[0] = SYM_TOTAL;
1374         osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance());
1375         break;
1376
1377     case OSD_HEADING:
1378         {
1379             buff[0] = SYM_HEADING;
1380             if (osdIsHeadingValid()) {
1381                 int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading());
1382                 if (h < 0) {
1383                     h += 360;
1384                 }
1385                 tfp_sprintf(&buff[1], "%3d", h);
1386             } else {
1387                 buff[1] = buff[2] = buff[3] = '-';
1388             }
1389             buff[4] = '\0';
1390             break;
1391         }
1392
1393     case OSD_CRUISE_HEADING_ERROR:
1394         {
1395             if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) {
1396                 displayWrite(osdDisplayPort, elemPosX, elemPosY, "     ");
1397                 return true;
1398             }
1399
1400             buff[0] = SYM_HEADING;
1401
1402             if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) {
1403                 buff[1] = buff[2] = buff[3] = '-';
1404             } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
1405                 int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
1406                 if (ABS(herr) > 99)
1407                     strcpy(buff + 1, ">99");
1408                 else
1409                     tfp_sprintf(buff + 1, "%3d", herr);
1410             }
1411
1412             buff[4] = SYM_DEGREES;
1413             buff[5] = '\0';
1414             break;
1415         }
1416
1417     case OSD_CRUISE_HEADING_ADJUSTMENT:
1418         {
1419             int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
1420
1421             if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
1422                 displayWrite(osdDisplayPort, elemPosX, elemPosY, "      ");
1423                 return true;
1424             }
1425
1426             buff[0] = SYM_HEADING;
1427
1428             if (!ARMING_FLAG(ARMED)) {
1429                 buff[1] = buff[2] = buff[3] = buff[4] = '-';
1430             } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
1431                 tfp_sprintf(buff + 1, "%4d", heading_adjust);
1432             }
1433
1434             buff[5] = SYM_DEGREES;
1435             buff[6] = '\0';
1436             break;
1437         }
1438
1439     case OSD_GPS_HDOP:
1440         {
1441             buff[0] = SYM_HDP_L;
1442             buff[1] = SYM_HDP_R;
1443             int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE;
1444             osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, 2);
1445             break;
1446         }
1447
1448     case OSD_MAP_NORTH:
1449         {
1450             static uint16_t drawn = 0;
1451             static uint32_t scale = 0;
1452             osdDrawHomeMap(0, 'N', &drawn, &scale);
1453             return true;
1454         }
1455     case OSD_MAP_TAKEOFF:
1456         {
1457             static uint16_t drawn = 0;
1458             static uint32_t scale = 0;
1459             osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale);
1460             return true;
1461         }
1462     case OSD_RADAR:
1463         {
1464             static uint16_t drawn = 0;
1465             static uint32_t scale = 0;
1466             osdDrawRadar(&drawn, &scale);
1467             return true;
1468         }
1469 #endif // GPS
1470
1471     case OSD_ALTITUDE:
1472         {
1473             int32_t alt = osdGetAltitude();
1474             osdFormatAltitudeSymbol(buff, alt);
1475             uint16_t alt_alarm = osdConfig()->alt_alarm;
1476             uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm;
1477             if ((alt_alarm > 0 && CENTIMETERS_TO_METERS(alt) > alt_alarm) ||
1478                 (neg_alt_alarm > 0 && alt < 0 && -CENTIMETERS_TO_METERS(alt) > neg_alt_alarm)) {
1479
1480                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1481             }
1482             break;
1483         }
1484
1485     case OSD_ALTITUDE_MSL:
1486         {
1487             int32_t alt = osdGetAltitudeMsl();
1488             osdFormatAltitudeSymbol(buff, alt);
1489             break;
1490         }
1491
1492     case OSD_ONTIME:
1493         {
1494             osdFormatOnTime(buff);
1495             break;
1496         }
1497
1498     case OSD_FLYTIME:
1499         {
1500             osdFormatFlyTime(buff, &elemAttr);
1501             break;
1502         }
1503
1504     case OSD_ONTIME_FLYTIME:
1505         {
1506             if (ARMING_FLAG(ARMED)) {
1507                 osdFormatFlyTime(buff, &elemAttr);
1508             } else {
1509                 osdFormatOnTime(buff);
1510             }
1511             break;
1512         }
1513
1514     case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH:
1515         {
1516             static timeUs_t updatedTimestamp = 0;
1517             /*static int32_t updatedTimeSeconds = 0;*/
1518             timeUs_t currentTimeUs = micros();
1519             static int32_t timeSeconds = -1;
1520             if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
1521                 timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
1522                 updatedTimestamp = currentTimeUs;
1523             }
1524             if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
1525                 buff[0] = SYM_FLY_M;
1526                 strcpy(buff + 1, "--:--");
1527                 updatedTimestamp = 0;
1528             } else if (timeSeconds == -2) {
1529                 // Wind is too strong to come back with cruise throttle
1530                 buff[0] = SYM_FLY_M;
1531                 buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL;
1532                 buff[3] = ':';
1533                 buff[6] = '\0';
1534                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1535             } else {
1536                 osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H);
1537                 if (timeSeconds == 0)
1538                     TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1539             }
1540         }
1541         break;
1542
1543     case OSD_REMAINING_DISTANCE_BEFORE_RTH:;
1544         static timeUs_t updatedTimestamp = 0;
1545         timeUs_t currentTimeUs = micros();
1546         static int32_t distanceMeters = -1;
1547         if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
1548             distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
1549             updatedTimestamp = currentTimeUs;
1550         }
1551         buff[0] = SYM_TRIP_DIST;
1552         if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
1553             buff[4] = SYM_DIST_M;
1554             buff[5] = '\0';
1555             strcpy(buff + 1, "---");
1556         } else if (distanceMeters == -2) {
1557             // Wind is too strong to come back with cruise throttle
1558             buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL;
1559             buff[4] = SYM_DIST_M;
1560             buff[5] = '\0';
1561             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1562         } else {
1563             osdFormatDistanceSymbol(buff + 1, distanceMeters * 100);
1564             if (distanceMeters == 0)
1565                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1566         }
1567         break;
1568
1569     case OSD_FLYMODE:
1570         {
1571             char *p = "ACRO";
1572
1573             if (FLIGHT_MODE(FAILSAFE_MODE))
1574                 p = "!FS!";
1575             else if (FLIGHT_MODE(MANUAL_MODE))
1576                 p = "MANU";
1577             else if (FLIGHT_MODE(NAV_RTH_MODE))
1578                 p = "RTH ";
1579             else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
1580                 p = "HOLD";
1581             else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
1582                 p = "3CRS";
1583             else if (FLIGHT_MODE(NAV_CRUISE_MODE))
1584                 p = "CRS ";
1585             else if (FLIGHT_MODE(NAV_WP_MODE))
1586                 p = " WP ";
1587             else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
1588                 // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
1589                 // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
1590                 // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
1591                 p = " AH ";
1592             }
1593             else if (FLIGHT_MODE(ANGLE_MODE))
1594                 p = "ANGL";
1595             else if (FLIGHT_MODE(HORIZON_MODE))
1596                 p = "HOR ";
1597
1598             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
1599             return true;
1600         }
1601
1602     case OSD_CRAFT_NAME:
1603         osdFormatCraftName(buff);
1604         break;
1605
1606     case OSD_THROTTLE_POS:
1607     {
1608         osdFormatThrottlePosition(buff, false, NULL);
1609         break;
1610     }
1611
1612     case OSD_VTX_CHANNEL:
1613         {
1614             vtxDeviceOsdInfo_t osdInfo;
1615             vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
1616
1617             tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName);
1618             displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
1619
1620             tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
1621             if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1622             displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr);
1623             return true;
1624         }
1625         break;
1626
1627     case OSD_VTX_POWER:
1628         {
1629             vtxDeviceOsdInfo_t osdInfo;
1630             vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
1631
1632             tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
1633             if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1634             displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
1635             return true;
1636         }
1637
1638     case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair
1639
1640         osdCrosshairPosition(&elemPosX, &elemPosY);
1641         osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
1642
1643         if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
1644             osdHudDrawHoming(elemPosX, elemPosY);
1645         }
1646
1647         if (STATE(GPS_FIX) && isImuHeadingValid()) {
1648
1649             if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
1650                     osdHudClear();
1651             }
1652
1653             // -------- POI : Home point
1654
1655             if (osdConfig()->hud_homepoint) { // Display the home point (H)
1656                 osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0);
1657             }
1658
1659             // -------- POI : Nearby aircrafts from ESP32 radar
1660
1661             if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar
1662                 for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) {
1663                     if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped
1664                         fpVector3_t poi;
1665                         geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE);
1666                         radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters
1667
1668                         if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) {
1669                             radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In °
1670                             radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100;
1671                             osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq);
1672                         }
1673                     }
1674                 }
1675
1676                 if (osdConfig()->hud_radar_nearest > 0) { // Display extra datas for 1 POI closer than a set distance
1677                     int poi_id = radarGetNearestPOI();
1678                     if (poi_id >= 0 && radar_pois[poi_id].distance <= osdConfig()->hud_radar_nearest) {
1679                         osdHudDrawExtras(poi_id);
1680                     }
1681                 }
1682             }
1683
1684             // -------- POI : Next waypoints from navigation
1685
1686             if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints
1687                 gpsLocation_t wp2;
1688                 int j;
1689
1690                 tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
1691                 displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
1692
1693                 for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
1694                     j = posControl.activeWaypointIndex + i;
1695                     if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {
1696                         wp2.lat = posControl.waypointList[j].lat;
1697                         wp2.lon = posControl.waypointList[j].lon;
1698                         wp2.alt = posControl.waypointList[j].alt;
1699                         fpVector3_t poi;
1700                         geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
1701                         while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
1702                         osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
1703                     }
1704                 }
1705             }
1706         }
1707
1708         return true;
1709         break;
1710
1711     case OSD_ATTITUDE_ROLL:
1712         buff[0] = SYM_ROLL_LEVEL;
1713         if (ABS(attitude.values.roll) >= 1)
1714             buff[0] += (attitude.values.roll < 0 ? -1 : 1);
1715         osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3);
1716         break;
1717
1718     case OSD_ATTITUDE_PITCH:
1719         if (ABS(attitude.values.pitch) < 1)
1720             buff[0] = 'P';
1721         else if (attitude.values.pitch > 0)
1722             buff[0] = SYM_PITCH_DOWN;
1723         else if (attitude.values.pitch < 0)
1724             buff[0] = SYM_PITCH_UP;
1725         osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3);
1726         break;
1727
1728     case OSD_ARTIFICIAL_HORIZON:
1729         {
1730             float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
1731             float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
1732
1733             if (osdConfig()->ahi_reverse_roll) {
1734                 rollAngle = -rollAngle;
1735             }
1736             osdDrawArtificialHorizon(osdDisplayPort, osdGetDisplayPortCanvas(),
1737                  OSD_DRAW_POINT_GRID(elemPosX, elemPosY), rollAngle, pitchAngle);
1738             osdDrawSingleElement(OSD_HORIZON_SIDEBARS);
1739             osdDrawSingleElement(OSD_CROSSHAIRS);
1740
1741             return true;
1742         }
1743
1744     case OSD_HORIZON_SIDEBARS:
1745         {
1746             osdDrawSidebars(osdDisplayPort, osdGetDisplayPortCanvas());
1747             return true;
1748         }
1749
1750 #if defined(USE_BARO) || defined(USE_GPS)
1751     case OSD_VARIO:
1752         {
1753             float zvel = getEstimatedActualVelocity(Z);
1754             osdDrawVario(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), zvel);
1755             return true;
1756         }
1757
1758     case OSD_VARIO_NUM:
1759         {
1760             int16_t value = getEstimatedActualVelocity(Z);
1761             char sym;
1762             switch ((osd_unit_e)osdConfig()->units) {
1763                 case OSD_UNIT_UK:
1764                     FALLTHROUGH;
1765                 case OSD_UNIT_IMPERIAL:
1766                     // Convert to centifeet/s
1767                     value = CENTIMETERS_TO_CENTIFEET(value);
1768                     sym = SYM_FTS;
1769                     break;
1770                 case OSD_UNIT_METRIC:
1771                     // Already in cm/s
1772                     sym = SYM_MS;
1773                     break;
1774             }
1775
1776             osdFormatCentiNumber(buff, value, 0, 1, 0, 3);
1777             buff[3] = sym;
1778             buff[4] = '\0';
1779             break;
1780         }
1781 #endif
1782
1783     case OSD_ROLL_PIDS:
1784         osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D);
1785         return true;
1786
1787     case OSD_PITCH_PIDS:
1788         osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D);
1789         return true;
1790
1791     case OSD_YAW_PIDS:
1792         osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D);
1793         return true;
1794
1795     case OSD_LEVEL_PIDS:
1796         osdDisplayPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
1797         return true;
1798
1799     case OSD_POS_XY_PIDS:
1800         osdDisplayPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
1801         return true;
1802
1803     case OSD_POS_Z_PIDS:
1804         osdDisplayPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
1805         return true;
1806
1807     case OSD_VEL_XY_PIDS:
1808         osdDisplayPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
1809         return true;
1810
1811     case OSD_VEL_Z_PIDS:
1812         osdDisplayPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
1813         return true;
1814
1815     case OSD_HEADING_P:
1816         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P);
1817         return true;
1818
1819     case OSD_BOARD_ALIGN_ROLL:
1820         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT);
1821         return true;
1822
1823     case OSD_BOARD_ALIGN_PITCH:
1824         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT);
1825         return true;
1826
1827     case OSD_RC_EXPO:
1828         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO);
1829         return true;
1830
1831     case OSD_RC_YAW_EXPO:
1832         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO);
1833         return true;
1834
1835     case OSD_THROTTLE_EXPO:
1836         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO);
1837         return true;
1838
1839     case OSD_PITCH_RATE:
1840         displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR");
1841
1842         elemAttr = TEXT_ATTRIBUTES_NONE;
1843         tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
1844         if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
1845             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1846         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1847         return true;
1848
1849     case OSD_ROLL_RATE:
1850         displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR");
1851
1852         elemAttr = TEXT_ATTRIBUTES_NONE;
1853         tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
1854         if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
1855             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1856         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1857         return true;
1858
1859     case OSD_YAW_RATE:
1860         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE);
1861         return true;
1862
1863     case OSD_MANUAL_RC_EXPO:
1864         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO);
1865         return true;
1866
1867     case OSD_MANUAL_RC_YAW_EXPO:
1868         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO);
1869         return true;
1870
1871     case OSD_MANUAL_PITCH_RATE:
1872         displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR");
1873
1874         elemAttr = TEXT_ATTRIBUTES_NONE;
1875         tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]);
1876         if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
1877             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1878         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1879         return true;
1880
1881     case OSD_MANUAL_ROLL_RATE:
1882         displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR");
1883
1884         elemAttr = TEXT_ATTRIBUTES_NONE;
1885         tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]);
1886         if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
1887             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1888         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1889         return true;
1890
1891     case OSD_MANUAL_YAW_RATE:
1892         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE);
1893         return true;
1894
1895     case OSD_NAV_FW_CRUISE_THR:
1896         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
1897         return true;
1898
1899     case OSD_NAV_FW_PITCH2THR:
1900         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
1901         return true;
1902
1903     case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
1904         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
1905         return true;
1906
1907     case OSD_FW_ALT_PID_OUTPUTS:
1908         {
1909             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1910             osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees
1911             break;
1912         }
1913
1914     case OSD_FW_POS_PID_OUTPUTS:
1915         {
1916             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees
1917             osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true);
1918             break;
1919         }
1920
1921     case OSD_MC_VEL_Z_PID_OUTPUTS:
1922         {
1923             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1924             osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs
1925             break;
1926         }
1927
1928     case OSD_MC_VEL_X_PID_OUTPUTS:
1929         {
1930             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1931             osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2
1932             break;
1933         }
1934
1935     case OSD_MC_VEL_Y_PID_OUTPUTS:
1936         {
1937             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1938             osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2
1939             break;
1940         }
1941
1942     case OSD_MC_POS_XYZ_P_OUTPUTS:
1943         {
1944             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1945             strcpy(buff, "POSO ");
1946             // display requested velocity cm/s
1947             tfp_sprintf(buff + 5, "%4d", (int)lrintf(nav_pids->pos[X].output_constrained * 100));
1948             buff[9] = ' ';
1949             tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100));
1950             buff[14] = ' ';
1951             tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100));
1952             buff[19] = '\0';
1953             break;
1954         }
1955
1956     case OSD_POWER:
1957         {
1958             osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3);
1959             buff[3] = SYM_WATT;
1960             buff[4] = '\0';
1961             break;
1962         }
1963
1964     case OSD_AIR_SPEED:
1965         {
1966         #ifdef USE_PITOT
1967             buff[0] = SYM_AIR;
1968             osdFormatVelocityStr(buff + 1, pitot.airSpeed, false);
1969         #else
1970             return false;
1971         #endif
1972             break;
1973         }
1974
1975     case OSD_RTC_TIME:
1976         {
1977             // RTC not configured will show 00:00
1978             dateTime_t dateTime;
1979             rtcGetDateTimeLocal(&dateTime);
1980             buff[0] = SYM_CLOCK;
1981             tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes);
1982             break;
1983         }
1984
1985     case OSD_MESSAGES:
1986         {
1987             const char *message = NULL;
1988             char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
1989             if (ARMING_FLAG(ARMED)) {
1990                 // Aircraft is armed. We might have up to 5
1991                 // messages to show.
1992                 const char *messages[5];
1993                 unsigned messageCount = 0;
1994                 if (FLIGHT_MODE(FAILSAFE_MODE)) {
1995                     // In FS mode while being armed too
1996                     const char *failsafePhaseMessage = osdFailsafePhaseMessage();
1997                     const char *failsafeInfoMessage = osdFailsafeInfoMessage();
1998                     const char *navStateFSMessage = navigationStateMessage();
1999                     if (failsafePhaseMessage) {
2000                         messages[messageCount++] = failsafePhaseMessage;
2001                     }
2002                     if (failsafeInfoMessage) {
2003                         messages[messageCount++] = failsafeInfoMessage;
2004                     }
2005                     if (navStateFSMessage) {
2006                         messages[messageCount++] = navStateFSMessage;
2007                     }
2008                     if (messageCount > 0) {
2009                         message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
2010                         if (message == failsafeInfoMessage) {
2011                             // failsafeInfoMessage is not useful for recovering
2012                             // a lost model, but might help avoiding a crash.
2013                             // Blink to grab user attention.
2014                             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2015                         }
2016                         // We're shoing either failsafePhaseMessage or
2017                         // navStateFSMessage. Don't BLINK here since
2018                         // having this text available might be crucial
2019                         // during a lost aircraft recovery and blinking
2020                         // will cause it to be missing from some frames.
2021                     }
2022                 } else {
2023                     if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
2024                         const char *navStateMessage = navigationStateMessage();
2025                         if (navStateMessage) {
2026                             messages[messageCount++] = navStateMessage;
2027                         }
2028                     } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
2029                             messages[messageCount++] = "AUTOLAUNCH";
2030                     } else {
2031                         if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
2032                             // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
2033                             // when it doesn't require ANGLE mode (required only in FW
2034                             // right now). If if requires ANGLE, its display is handled
2035                             // by OSD_FLYMODE.
2036                             messages[messageCount++] = "(ALTITUDE HOLD)";
2037                         }
2038                         if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
2039                             messages[messageCount++] = "(AUTOTRIM)";
2040                         }
2041                         if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
2042                             messages[messageCount++] = "(AUTOTUNE)";
2043                         }
2044                         if (FLIGHT_MODE(HEADFREE_MODE)) {
2045                             messages[messageCount++] = "(HEADFREE)";
2046                         }
2047                     }
2048                     // Pick one of the available messages. Each message lasts
2049                     // a second.
2050                     if (messageCount > 0) {
2051                         message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
2052                     }
2053                 }
2054             } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
2055                 unsigned invalidIndex;
2056                 // Check if we're unable to arm for some reason
2057                 if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
2058                     if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
2059                         const setting_t *setting = settingGet(invalidIndex);
2060                         settingGetName(setting, messageBuf);
2061                         for (int ii = 0; messageBuf[ii]; ii++) {
2062                             messageBuf[ii] = sl_toupper(messageBuf[ii]);
2063                         }
2064                         message = messageBuf;
2065                     } else {
2066                         message = "INVALID SETTING";
2067                         TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
2068                     }
2069                 } else {
2070                     if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
2071                         message = "UNABLE TO ARM";
2072                         TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
2073                     } else {
2074                         // Show the reason for not arming
2075                         message = osdArmingDisabledReasonMessage();
2076                     }
2077                 }
2078             }
2079             osdFormatMessage(buff, sizeof(buff), message);
2080             break;
2081         }
2082
2083     case OSD_MAIN_BATT_CELL_VOLTAGE:
2084         {
2085             osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2);
2086             return true;
2087         }
2088
2089     case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE:
2090         {
2091             osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), 3, 2);
2092             return true;
2093         }
2094
2095     case OSD_THROTTLE_POS_AUTO_THR:
2096         {
2097             osdFormatThrottlePosition(buff, true, &elemAttr);
2098             break;
2099         }
2100
2101     case OSD_HEADING_GRAPH:
2102         {
2103             if (osdIsHeadingValid()) {
2104                 osdDrawHeadingGraph(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), osdGetHeading());
2105                 return true;
2106             } else {
2107                 buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE;
2108                 buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE;
2109                 buff[OSD_HEADING_GRAPH_WIDTH] = '\0';
2110             }
2111             break;
2112         }
2113
2114     case OSD_EFFICIENCY_MAH_PER_KM:
2115         {
2116             // amperage is in centi amps, speed is in cms/s. We want
2117             // mah/km. Values over 999 are considered useless and
2118             // displayed as "---""
2119             static pt1Filter_t eFilterState;
2120             static timeUs_t efficiencyUpdated = 0;
2121             int32_t value = 0;
2122             timeUs_t currentTimeUs = micros();
2123             timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
2124             if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
2125                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
2126                     value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
2127                         1, efficiencyTimeDelta * 1e-6f);
2128
2129                     efficiencyUpdated = currentTimeUs;
2130                 } else {
2131                     value = eFilterState.state;
2132                 }
2133             }
2134             if (value > 0 && value <= 999) {
2135                 tfp_sprintf(buff, "%3d", (int)value);
2136             } else {
2137                 buff[0] = buff[1] = buff[2] = '-';
2138             }
2139             buff[3] = SYM_MAH_KM_0;
2140             buff[4] = SYM_MAH_KM_1;
2141             buff[5] = '\0';
2142             break;
2143         }
2144
2145     case OSD_EFFICIENCY_WH_PER_KM:
2146         {
2147             // amperage is in centi amps, speed is in cms/s. We want
2148             // mWh/km. Values over 999Wh/km are considered useless and
2149             // displayed as "---""
2150             static pt1Filter_t eFilterState;
2151             static timeUs_t efficiencyUpdated = 0;
2152             int32_t value = 0;
2153             timeUs_t currentTimeUs = micros();
2154             timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
2155             if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
2156                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
2157                     value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
2158                         1, efficiencyTimeDelta * 1e-6f);
2159
2160                     efficiencyUpdated = currentTimeUs;
2161                 } else {
2162                     value = eFilterState.state;
2163                 }
2164             }
2165             if (value > 0 && value <= 999999) {
2166                 osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3);
2167             } else {
2168                 buff[0] = buff[1] = buff[2] = '-';
2169             }
2170             buff[3] = SYM_WH_KM_0;
2171             buff[4] = SYM_WH_KM_1;
2172             buff[5] = '\0';
2173             break;
2174         }
2175
2176     case OSD_GFORCE:
2177         {
2178             buff[0] = SYM_GFORCE;
2179             osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3);
2180             if (GForce > osdConfig()->gforce_alarm * 100) {
2181                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2182             }
2183             break;
2184         }
2185
2186     case OSD_GFORCE_X:
2187     case OSD_GFORCE_Y:
2188     case OSD_GFORCE_Z:
2189         {
2190             float GForceValue = GForceAxis[item - OSD_GFORCE_X];
2191             buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X;
2192             osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4);
2193             if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) {
2194                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2195             }
2196             break;
2197         }
2198
2199     case OSD_DEBUG:
2200         {
2201             // Longest representable string is -2147483648, hence 11 characters
2202             for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) {
2203                 tfp_sprintf(buff, "[%u]=%11ld [%u]=%11ld", bufferIndex, debug[bufferIndex], bufferIndex+1, debug[bufferIndex+1]);
2204                 displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
2205             }
2206             break;
2207         }
2208
2209     case OSD_IMU_TEMPERATURE:
2210         {
2211             int16_t temperature;
2212             const bool valid = getIMUTemperature(&temperature);
2213             osdDisplayTemperature(elemPosX, elemPosY, SYM_IMU_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max);
2214             return true;
2215         }
2216
2217     case OSD_BARO_TEMPERATURE:
2218         {
2219             int16_t temperature;
2220             const bool valid = getBaroTemperature(&temperature);
2221             osdDisplayTemperature(elemPosX, elemPosY, SYM_BARO_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max);
2222             return true;
2223         }
2224
2225 #ifdef USE_TEMPERATURE_SENSOR
2226     case OSD_TEMP_SENSOR_0_TEMPERATURE:
2227     case OSD_TEMP_SENSOR_1_TEMPERATURE:
2228     case OSD_TEMP_SENSOR_2_TEMPERATURE:
2229     case OSD_TEMP_SENSOR_3_TEMPERATURE:
2230     case OSD_TEMP_SENSOR_4_TEMPERATURE:
2231     case OSD_TEMP_SENSOR_5_TEMPERATURE:
2232     case OSD_TEMP_SENSOR_6_TEMPERATURE:
2233     case OSD_TEMP_SENSOR_7_TEMPERATURE:
2234         {
2235             osdDisplayTemperatureSensor(elemPosX, elemPosY, item - OSD_TEMP_SENSOR_0_TEMPERATURE);
2236             return true;
2237         }
2238 #endif /* ifdef USE_TEMPERATURE_SENSOR */
2239
2240     case OSD_WIND_SPEED_HORIZONTAL:
2241 #ifdef USE_WIND_ESTIMATOR
2242         {
2243             bool valid = isEstimatedWindSpeedValid();
2244             float horizontalWindSpeed;
2245             if (valid) {
2246                 uint16_t angle;
2247                 horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
2248                 int16_t windDirection = osdGetHeadingAngle((int)angle - DECIDEGREES_TO_DEGREES(attitude.values.yaw));
2249                 buff[1] = SYM_DIRECTION + (windDirection * 2 / 90);
2250             } else {
2251                 horizontalWindSpeed = 0;
2252                 buff[1] = SYM_BLANK;
2253             }
2254             buff[0] = SYM_WIND_HORIZONTAL;
2255             osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid);
2256             break;
2257         }
2258 #else
2259         return false;
2260 #endif
2261
2262     case OSD_WIND_SPEED_VERTICAL:
2263 #ifdef USE_WIND_ESTIMATOR
2264         {
2265             buff[0] = SYM_WIND_VERTICAL;
2266             buff[1] = SYM_BLANK;
2267             bool valid = isEstimatedWindSpeedValid();
2268             float verticalWindSpeed;
2269             if (valid) {
2270                 verticalWindSpeed = getEstimatedWindSpeed(Z);
2271                 if (verticalWindSpeed < 0) {
2272                     buff[1] = SYM_AH_DECORATION_DOWN;
2273                     verticalWindSpeed = -verticalWindSpeed;
2274                 } else if (verticalWindSpeed > 0) {
2275                     buff[1] = SYM_AH_DECORATION_UP;
2276                 }
2277             } else {
2278                 verticalWindSpeed = 0;
2279             }
2280             osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid);
2281             break;
2282         }
2283 #else
2284         return false;
2285 #endif
2286
2287     case OSD_PLUS_CODE:
2288         {
2289             STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
2290             int digits = osdConfig()->plus_code_digits;
2291             if (STATE(GPS_FIX)) {
2292                 olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff));
2293             } else {
2294                 // +codes with > 8 digits have a + at the 9th digit
2295                 // and we only support 10 and up.
2296                 memset(buff, '-', digits + 1);
2297                 buff[8] = '+';
2298                 buff[digits + 1] = '\0';
2299             }
2300             break;
2301         }
2302
2303     case OSD_AZIMUTH:
2304         {
2305
2306             buff[0] = SYM_AZIMUTH;
2307             if (osdIsHeadingValid()) {
2308                 int16_t h = GPS_directionToHome;
2309                 if (h < 0) {
2310                     h += 360;
2311                 }
2312                 if(h >= 180)
2313                     h = h - 180;
2314                 else
2315                     h = h + 180;
2316
2317                 tfp_sprintf(&buff[1], "%3d", h);
2318             } else {
2319                 buff[1] = buff[2] = buff[3] = '-';
2320             }
2321             buff[4] = '\0';
2322             break;
2323         }
2324
2325     case OSD_MAP_SCALE:
2326         {
2327             float scaleToUnit;
2328             int scaleUnitDivisor;
2329             char symUnscaled;
2330             char symScaled;
2331             int maxDecimals;
2332
2333             switch (osdConfig()->units) {
2334             case OSD_UNIT_IMPERIAL:
2335                 scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber()
2336                 scaleUnitDivisor = 0;
2337                 symUnscaled = SYM_MI;
2338                 symScaled = SYM_MI;
2339                 maxDecimals = 2;
2340                 break;
2341             case OSD_UNIT_UK:
2342                 FALLTHROUGH;
2343             case OSD_UNIT_METRIC:
2344                 scaleToUnit = 100; // scale to cm for osdFormatCentiNumber()
2345                 scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m
2346                 symUnscaled = SYM_M;
2347                 symScaled = SYM_KM;
2348                 maxDecimals = 0;
2349                 break;
2350             }
2351             buff[0] = SYM_SCALE;
2352             if (osdMapData.scale > 0) {
2353                 bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3);
2354                 buff[4] = scaled ? symScaled : symUnscaled;
2355                 // Make sure this is cleared if the map stops being drawn
2356                 osdMapData.scale = 0;
2357             } else {
2358                 memset(&buff[1], '-', 4);
2359             }
2360             buff[5] = '\0';
2361             break;
2362         }
2363     case OSD_MAP_REFERENCE:
2364         {
2365             char referenceSymbol;
2366             if (osdMapData.referenceSymbol) {
2367                 referenceSymbol = osdMapData.referenceSymbol;
2368                 // Make sure this is cleared if the map stops being drawn
2369                 osdMapData.referenceSymbol = 0;
2370             } else {
2371                 referenceSymbol = '-';
2372             }
2373             displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION);
2374             displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol);
2375             return true;
2376         }
2377
2378 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
2379     case OSD_RC_SOURCE:
2380         {
2381             const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD";
2382             if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2383             displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr);
2384             return true;
2385         }
2386 #endif
2387
2388 #if defined(USE_ESC_SENSOR)
2389     case OSD_ESC_RPM:
2390         {
2391             escSensorData_t * escSensor = escSensorGetData();
2392             if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
2393                 osdFormatRpm(buff, escSensor->rpm);
2394             }
2395             else {
2396                 osdFormatRpm(buff, 0);
2397             }
2398             break;
2399         }
2400     case OSD_ESC_TEMPERATURE:
2401         {
2402             escSensorData_t * escSensor = escSensorGetData();
2403             bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE;
2404             osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max);
2405             return true;
2406         }
2407 #endif
2408
2409     default:
2410         return false;
2411     }
2412
2413     displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
2414     return true;
2415 }
2416
2417 static uint8_t osdIncElementIndex(uint8_t elementIndex)
2418 {
2419     ++elementIndex;
2420
2421     if (elementIndex == OSD_ARTIFICIAL_HORIZON)
2422         ++elementIndex;
2423
2424 #ifndef USE_TEMPERATURE_SENSOR
2425     if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE)
2426         elementIndex = OSD_ALTITUDE_MSL;
2427 #endif
2428
2429     if (!sensors(SENSOR_ACC)) {
2430         if (elementIndex == OSD_CROSSHAIRS) {
2431             elementIndex = OSD_ONTIME;
2432         }
2433     }
2434
2435     if (!feature(FEATURE_VBAT)) {
2436         if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
2437             elementIndex = OSD_LEVEL_PIDS;
2438         }
2439     }
2440
2441     if (!feature(FEATURE_CURRENT_METER)) {
2442         if (elementIndex == OSD_CURRENT_DRAW) {
2443             elementIndex = OSD_GPS_SPEED;
2444         }
2445         if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
2446             elementIndex = OSD_TRIP_DIST;
2447         }
2448         if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) {
2449             elementIndex = OSD_HOME_HEADING_ERROR;
2450         }
2451         if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
2452             elementIndex = OSD_LEVEL_PIDS;
2453         }
2454     }
2455
2456     if (!feature(FEATURE_GPS)) {
2457         if (elementIndex == OSD_GPS_SPEED) {
2458             elementIndex = OSD_ALTITUDE;
2459         }
2460         if (elementIndex == OSD_GPS_LON) {
2461             elementIndex = OSD_VARIO;
2462         }
2463         if (elementIndex == OSD_GPS_HDOP) {
2464             elementIndex = OSD_MAIN_BATT_CELL_VOLTAGE;
2465         }
2466         if (elementIndex == OSD_TRIP_DIST) {
2467             elementIndex = OSD_ATTITUDE_PITCH;
2468         }
2469         if (elementIndex == OSD_WIND_SPEED_HORIZONTAL) {
2470             elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE;
2471         }
2472         if (elementIndex == OSD_3D_SPEED) {
2473             elementIndex++;
2474         }
2475     }
2476
2477     if (!STATE(ESC_SENSOR_ENABLED)) {
2478         if (elementIndex == OSD_ESC_RPM) {
2479             elementIndex++;
2480         }
2481     }
2482
2483     if (elementIndex == OSD_ITEM_COUNT) {
2484         elementIndex = 0;
2485     }
2486     return elementIndex;
2487 }
2488
2489 void osdDrawNextElement(void)
2490 {
2491     static uint8_t elementIndex = 0;
2492     // Prevent infinite loop when no elements are enabled
2493     uint8_t index = elementIndex;
2494     do {
2495         elementIndex = osdIncElementIndex(elementIndex);
2496     } while(!osdDrawSingleElement(elementIndex) && index != elementIndex);
2497
2498     // Draw artificial horizon last
2499     osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
2500 }
2501
2502 PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
2503     .rssi_alarm = 20,
2504     .time_alarm = 10,
2505     .alt_alarm = 100,
2506     .dist_alarm = 1000,
2507     .neg_alt_alarm = 5,
2508     .current_alarm = 0,
2509     .imu_temp_alarm_min = -200,
2510     .imu_temp_alarm_max = 600,
2511     .esc_temp_alarm_min = -200,
2512     .esc_temp_alarm_max = 900,
2513     .gforce_alarm = 5,
2514     .gforce_axis_alarm_min = -5,
2515     .gforce_axis_alarm_max = 5,
2516 #ifdef USE_BARO
2517     .baro_temp_alarm_min = -200,
2518     .baro_temp_alarm_max = 600,
2519 #endif
2520
2521 #ifdef USE_TEMPERATURE_SENSOR
2522     .temp_label_align = OSD_ALIGN_LEFT,
2523 #endif
2524
2525     .video_system = VIDEO_SYSTEM_AUTO,
2526
2527     .ahi_reverse_roll = 0,
2528     .ahi_max_pitch = AH_MAX_PITCH_DEFAULT,
2529     .crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT,
2530     .horizon_offset = 0,
2531     .camera_uptilt = 0,
2532     .camera_fov_h = 135,
2533     .camera_fov_v = 85,
2534     .hud_margin_h = 3,
2535     .hud_margin_v = 3,
2536     .hud_homing = 0,
2537     .hud_homepoint = 0,
2538     .hud_radar_disp = 0,
2539     .hud_radar_range_min = 3,
2540     .hud_radar_range_max = 4000,
2541     .hud_radar_nearest = 0,
2542     .hud_wp_disp = 0,
2543     .left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
2544     .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
2545     .sidebar_scroll_arrows = 0,
2546
2547     .units = OSD_UNIT_METRIC,
2548     .main_voltage_decimals = 1,
2549
2550     .estimations_wind_compensation = true,
2551     .coordinate_digits = 9,
2552
2553     .osd_failsafe_switch_layout = false,
2554
2555     .plus_code_digits = 11,
2556
2557     .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH,
2558     .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT,
2559     .ahi_vertical_offset = -OSD_CHAR_HEIGHT,
2560     .sidebar_horizontal_offset = OSD_AH_SIDEBAR_WIDTH_POS * OSD_CHAR_WIDTH,
2561 );
2562
2563 void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
2564 {
2565     osdLayoutsConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG;
2566     osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG;
2567     osdLayoutsConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1);
2568
2569     osdLayoutsConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG;
2570     //line 2
2571     osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);
2572     osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2);
2573     osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
2574     osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
2575     osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
2576     osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
2577
2578     osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
2579     osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
2580     osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
2581     osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
2582     osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
2583     osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
2584     osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
2585     osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
2586     osdLayoutsConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5);
2587     osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6);
2588     osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7);
2589     osdLayoutsConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8);
2590
2591     osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5);
2592     osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5);
2593
2594     osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7);
2595     osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8);
2596
2597     // avoid OSD_VARIO under OSD_CROSSHAIRS
2598     osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5);
2599     // OSD_VARIO_NUM at the right of OSD_VARIO
2600     osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7);
2601     osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11);
2602     osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
2603     osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
2604
2605     osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
2606     osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
2607
2608     osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
2609     osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
2610     osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
2611     osdLayoutsConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
2612     osdLayoutsConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
2613     osdLayoutsConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);
2614
2615     osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
2616     osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
2617
2618     osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);
2619     // Put this on top of the latitude, since it's very unlikely
2620     // that users will want to use both at the same time.
2621     osdLayoutsConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12);
2622     osdLayoutsConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG;
2623     osdLayoutsConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12);
2624
2625     osdLayoutsConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12);
2626
2627     osdLayoutsConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10);
2628     osdLayoutsConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11);
2629     osdLayoutsConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12);
2630     osdLayoutsConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12);
2631     osdLayoutsConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12);
2632     osdLayoutsConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12);
2633     osdLayoutsConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12);
2634     osdLayoutsConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12);
2635     osdLayoutsConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12);
2636     osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10);
2637     osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11);
2638     osdLayoutsConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12);
2639     osdLayoutsConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12);
2640     osdLayoutsConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12);
2641     osdLayoutsConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12);
2642     osdLayoutsConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12);
2643     osdLayoutsConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12);
2644     osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12);
2645     osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12);
2646     osdLayoutsConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12);
2647     osdLayoutsConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12);
2648     osdLayoutsConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12);
2649     osdLayoutsConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12);
2650     osdLayoutsConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12);
2651     osdLayoutsConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12);
2652     osdLayoutsConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12);
2653     osdLayoutsConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12);
2654     osdLayoutsConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12);
2655     osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12);
2656     osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12);
2657     osdLayoutsConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
2658
2659     osdLayoutsConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);
2660
2661     osdLayoutsConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2);
2662     osdLayoutsConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3);
2663     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4);
2664     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5);
2665     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6);
2666     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7);
2667     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8);
2668     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9);
2669     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10);
2670     osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11);
2671
2672     osdLayoutsConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);
2673     osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6);
2674     osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7);
2675
2676     osdLayoutsConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4);
2677     osdLayoutsConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5);
2678     osdLayoutsConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6);
2679     osdLayoutsConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7);
2680
2681     osdLayoutsConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5);
2682
2683 #if defined(USE_ESC_SENSOR)
2684     osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2);
2685     osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3);
2686 #endif
2687
2688 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
2689     osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
2690 #endif
2691
2692     // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
2693     osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
2694
2695     for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) {
2696         for (unsigned jj = 0; jj < ARRAYLEN(osdLayoutsConfig->item_pos[0]); jj++) {
2697              osdLayoutsConfig->item_pos[ii][jj] = osdLayoutsConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG;
2698         }
2699     }
2700 }
2701
2702 static void osdSetNextRefreshIn(uint32_t timeMs) {
2703     resumeRefreshAt = micros() + timeMs * 1000;
2704     refreshWaitForResumeCmdRelease = true;
2705 }
2706
2707 static void osdCompleteAsyncInitialization(void)
2708 {
2709     if (!displayIsReady(osdDisplayPort)) {
2710         // Update the display.
2711         // XXX: Rename displayDrawScreen() and associated functions
2712         // to displayUpdate()
2713         displayDrawScreen(osdDisplayPort);
2714         return;
2715     }
2716
2717     osdDisplayIsReady = true;
2718
2719 #if defined(USE_CANVAS)
2720     if (osdConfig()->force_grid) {
2721         osdDisplayHasCanvas = false;
2722     } else {
2723         osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort);
2724     }
2725 #endif
2726
2727     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
2728     displayClearScreen(osdDisplayPort);
2729
2730     uint8_t y = 1;
2731     displayFontMetadata_t metadata;
2732     bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort);
2733     LOG_D(OSD, "Font metadata version %s: %u (%u chars)",
2734         fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount);
2735
2736     if (fontHasMetadata && metadata.charCount > 256) {
2737         hasExtendedFont = true;
2738         unsigned logo_c = SYM_LOGO_START;
2739         unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH);
2740         for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) {
2741             for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) {
2742                 displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++);
2743             }
2744             y++;
2745         }
2746         y++;
2747     } else {
2748         if (!fontHasMetadata || metadata.version < OSD_MIN_FONT_VERSION) {
2749             const char *m = "INVALID FONT";
2750             displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m);
2751         }
2752         y = 4;
2753     }
2754
2755     char string_buffer[30];
2756     tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING);
2757     displayWrite(osdDisplayPort, 5, y++, string_buffer);
2758 #ifdef USE_CMS
2759     displayWrite(osdDisplayPort, 7, y++,  CMS_STARTUP_HELP_TEXT1);
2760     displayWrite(osdDisplayPort, 11, y++, CMS_STARTUP_HELP_TEXT2);
2761     displayWrite(osdDisplayPort, 11, y++, CMS_STARTUP_HELP_TEXT3);
2762 #endif
2763
2764 #ifdef USE_STATS
2765 #define STATS_LABEL_X_POS 4
2766 #define STATS_VALUE_X_POS 24
2767     if (statsConfig()->stats_enabled) {
2768         displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:");
2769         if (osdConfig()->units == OSD_UNIT_IMPERIAL) {
2770             tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE));
2771             string_buffer[5] = SYM_MI;
2772         } else {
2773             tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER));
2774             string_buffer[5] = SYM_KM;
2775         }
2776         string_buffer[6] = '\0';
2777         displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y,  string_buffer);
2778
2779         displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "TOTAL TIME:");
2780         uint32_t tot_mins = statsConfig()->stats_total_time / 60;
2781         tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60));
2782         displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y,  string_buffer);
2783
2784 #ifdef USE_ADC
2785         if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
2786             displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "TOTAL ENERGY:");
2787             osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4);
2788             strcat(string_buffer, "\xAB"); // SYM_WH
2789             displayWrite(osdDisplayPort, STATS_VALUE_X_POS-4, y,  string_buffer);
2790
2791             displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:");
2792             if (statsConfig()->stats_total_dist) {
2793                 uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km
2794                 osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3);
2795             } else
2796                 strcpy(string_buffer, "---");
2797             string_buffer[3] = SYM_WH_KM_0;
2798             string_buffer[4] = SYM_WH_KM_1;
2799             string_buffer[5] = '\0';
2800             displayWrite(osdDisplayPort, STATS_VALUE_X_POS-3, y,  string_buffer);
2801         }
2802 #endif // USE_ADC
2803     }
2804 #endif
2805
2806     displayCommitTransaction(osdDisplayPort);
2807     displayResync(osdDisplayPort);
2808     osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME);
2809 }
2810
2811 void osdInit(displayPort_t *osdDisplayPortToUse)
2812 {
2813     if (!osdDisplayPortToUse)
2814         return;
2815
2816     BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
2817
2818     osdDisplayPort = osdDisplayPortToUse;
2819
2820 #ifdef USE_CMS
2821     cmsDisplayPortRegister(osdDisplayPort);
2822 #endif
2823
2824     armState = ARMING_FLAG(ARMED);
2825     osdCompleteAsyncInitialization();
2826 }
2827
2828 static void osdResetStats(void)
2829 {
2830     stats.max_current = 0;
2831     stats.max_power = 0;
2832     stats.max_speed = 0;
2833     stats.min_voltage = 5000;
2834     stats.min_rssi = 99;
2835     stats.max_altitude = 0;
2836 }
2837
2838 static void osdUpdateStats(void)
2839 {
2840     int16_t value;
2841
2842     if (feature(FEATURE_GPS)) {
2843         value = osdGet3DSpeed();
2844         if (stats.max_speed < value)
2845             stats.max_speed = value;
2846
2847         if (stats.max_distance < GPS_distanceToHome)
2848             stats.max_distance = GPS_distanceToHome;
2849     }
2850
2851     value = getBatteryVoltage();
2852     if (stats.min_voltage > value)
2853         stats.min_voltage = value;
2854
2855     value = abs(getAmperage() / 100);
2856     if (stats.max_current < value)
2857         stats.max_current = value;
2858
2859     value = abs(getPower() / 100);
2860     if (stats.max_power < value)
2861         stats.max_power = value;
2862
2863     value = osdConvertRSSI();
2864     if (stats.min_rssi > value)
2865         stats.min_rssi = value;
2866
2867     stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude());
2868 }
2869
2870 /* Attention: NTSC screen only has 12 fully visible lines - it is FULL now! */
2871 static void osdShowStats(void)
2872 {
2873     const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" };
2874     uint8_t top = 1;    /* first fully visible line */
2875     const uint8_t statNameX = 1;
2876     const uint8_t statValuesX = 20;
2877     char buff[10];
2878
2879     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
2880     displayClearScreen(osdDisplayPort);
2881     if (osdDisplayIsPAL())
2882         displayWrite(osdDisplayPort, statNameX, top++, "  --- STATS ---");
2883
2884     if (STATE(GPS_FIX)) {
2885         displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED        :");
2886         osdFormatVelocityStr(buff, stats.max_speed, true);
2887         osdLeftAlignString(buff);
2888         displayWrite(osdDisplayPort, statValuesX, top++, buff);
2889
2890         displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE     :");
2891         osdFormatDistanceStr(buff, stats.max_distance*100);
2892         displayWrite(osdDisplayPort, statValuesX, top++, buff);
2893
2894         displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:");
2895         osdFormatDistanceStr(buff, getTotalTravelDistance());
2896         displayWrite(osdDisplayPort, statValuesX, top++, buff);
2897     }
2898
2899     displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE     :");
2900     osdFormatAltitudeStr(buff, stats.max_altitude);
2901     displayWrite(osdDisplayPort, statValuesX, top++, buff);
2902
2903     displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
2904     osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
2905     strcat(buff, "V");
2906     osdLeftAlignString(buff);
2907     displayWrite(osdDisplayPort, statValuesX, top++, buff);
2908
2909     displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI         :");
2910     itoa(stats.min_rssi, buff, 10);
2911     strcat(buff, "%");
2912     displayWrite(osdDisplayPort, statValuesX, top++, buff);
2913
2914     if (feature(FEATURE_CURRENT_METER)) {
2915         displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT      :");
2916         itoa(stats.max_current, buff, 10);
2917         strcat(buff, "A");
2918         displayWrite(osdDisplayPort, statValuesX, top++, buff);
2919
2920         displayWrite(osdDisplayPort, statNameX, top, "MAX POWER        :");
2921         itoa(stats.max_power, buff, 10);
2922         strcat(buff, "W");
2923         displayWrite(osdDisplayPort, statValuesX, top++, buff);
2924
2925         if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
2926             displayWrite(osdDisplayPort, statNameX, top, "USED MAH         :");
2927             tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH);
2928         } else {
2929             displayWrite(osdDisplayPort, statNameX, top, "USED WH          :");
2930             osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
2931             strcat(buff, "\xAB"); // SYM_WH
2932         }
2933         displayWrite(osdDisplayPort, statValuesX, top++, buff);
2934
2935         int32_t totalDistance = getTotalTravelDistance();
2936         if (totalDistance > 0) {
2937             displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY   :");
2938             if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH)
2939                 tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance),
2940                     SYM_MAH_KM_0, SYM_MAH_KM_1);
2941             else {
2942                 osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3);
2943                 buff[3] = SYM_WH_KM_0;
2944                 buff[4] = SYM_WH_KM_1;
2945                 buff[5] = '\0';
2946             }
2947             displayWrite(osdDisplayPort, statValuesX, top++, buff);
2948         }
2949     }
2950
2951     displayWrite(osdDisplayPort, statNameX, top, "FLY TIME         :");
2952     uint16_t flySeconds = getFlightTime();
2953     uint16_t flyMinutes = flySeconds / 60;
2954     flySeconds %= 60;
2955     uint16_t flyHours = flyMinutes / 60;
2956     flyMinutes %= 60;
2957     tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
2958     displayWrite(osdDisplayPort, statValuesX, top++, buff);
2959
2960     const float max_gforce = accGetMeasuredMaxG();
2961     displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE      :");
2962     osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3);
2963     displayWrite(osdDisplayPort, statValuesX, top++, buff);
2964
2965     const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
2966     displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
2967     osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4);
2968     strcat(buff,"/");
2969     displayWrite(osdDisplayPort, statValuesX, top, buff);
2970     osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3);
2971     displayWrite(osdDisplayPort, statValuesX + 5, top++, buff);
2972
2973     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
2974     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
2975     displayCommitTransaction(osdDisplayPort);
2976 }
2977
2978 // called when motors armed
2979 static void osdShowArmed(void)
2980 {
2981     dateTime_t dt;
2982     char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)];
2983     char craftNameBuf[MAX_NAME_LENGTH];
2984     char *date;
2985     char *time;
2986     // We need 10 visible rows
2987     uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 10 - 1);
2988
2989     displayClearScreen(osdDisplayPort);
2990     displayWrite(osdDisplayPort, 12, y, "ARMED");
2991     y += 2;
2992
2993     if (strlen(systemConfig()->name) > 0) {
2994         osdFormatCraftName(craftNameBuf);
2995         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf );
2996         y += 2;
2997     }
2998
2999 #if defined(USE_GPS)
3000     if (feature(FEATURE_GPS)) {
3001         if (STATE(GPS_FIX_HOME)) {
3002             osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
3003             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
3004             osdFormatCoordinate(buf, SYM_LON, GPS_home.lon);
3005             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
3006             int digits = osdConfig()->plus_code_digits;
3007             olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
3008             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf);
3009             y += 4;
3010 #if defined (USE_SAFE_HOME)
3011             if (isSafeHomeInUse()) {
3012                 textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
3013                 char buf2[12]; // format the distance first
3014                 osdFormatDistanceStr(buf2, safehome_distance);
3015                 tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used);
3016                 // write this message above the ARMED message to make it obvious
3017                 displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
3018             }
3019 #endif
3020         } else {
3021             strcpy(buf, "!NO HOME POSITION!");
3022             displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
3023             y += 1;
3024         }
3025     }
3026 #endif
3027
3028     if (rtcGetDateTime(&dt)) {
3029         dateTimeFormatLocal(buf, &dt);
3030         dateTimeSplitFormatted(buf, &date, &time);
3031
3032         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date);
3033         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time);
3034     }
3035 }
3036
3037 static void osdFilterData(timeUs_t currentTimeUs) {
3038     static timeUs_t lastRefresh = 0;
3039     float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6;
3040
3041     GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
3042     for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
3043
3044     if (lastRefresh) {
3045         GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT);
3046         for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT);
3047     } else {
3048         pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0);
3049         pt1FilterReset(&GForceFilter, GForce);
3050
3051         for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
3052             pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0);
3053             pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]);
3054         }
3055     }
3056
3057     lastRefresh = currentTimeUs;
3058 }
3059
3060 static void osdRefresh(timeUs_t currentTimeUs)
3061 {
3062     osdFilterData(currentTimeUs);
3063
3064 #ifdef USE_CMS
3065     if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) {
3066 #else
3067     if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) {
3068 #endif
3069       displayClearScreen(osdDisplayPort);
3070       armState = ARMING_FLAG(ARMED);
3071       return;
3072     }
3073
3074     // detect arm/disarm
3075     if (armState != ARMING_FLAG(ARMED)) {
3076         if (ARMING_FLAG(ARMED)) {
3077             osdResetStats();
3078             osdShowArmed(); // reset statistic etc
3079             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
3080 #if defined(USE_SAFE_HOME)
3081             if (isSafeHomeInUse())
3082                 delay *= 3;
3083 #endif
3084             osdSetNextRefreshIn(delay);
3085         } else {
3086             osdShowStats(); // show statistic
3087             osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
3088         }
3089
3090         armState = ARMING_FLAG(ARMED);
3091     }
3092
3093     if (resumeRefreshAt) {
3094         // If we already reached he time for the next refresh,
3095         // or THR is high or PITCH is high, resume refreshing.
3096         // Clear the screen first to erase other elements which
3097         // might have been drawn while the OSD wasn't refreshing.
3098
3099         if (!DELAYED_REFRESH_RESUME_COMMAND)
3100             refreshWaitForResumeCmdRelease = false;
3101
3102         if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) {
3103             displayClearScreen(osdDisplayPort);
3104             resumeRefreshAt = 0;
3105         } else {
3106             displayHeartbeat(osdDisplayPort);
3107         }
3108         return;
3109     }
3110
3111 #ifdef USE_CMS
3112     if (!displayIsGrabbed(osdDisplayPort)) {
3113         displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
3114         if (fullRedraw) {
3115             displayClearScreen(osdDisplayPort);
3116             fullRedraw = false;
3117         }
3118         osdDrawNextElement();
3119         displayHeartbeat(osdDisplayPort);
3120         displayCommitTransaction(osdDisplayPort);
3121 #ifdef OSD_CALLS_CMS
3122     } else {
3123         cmsUpdate(currentTimeUs);
3124 #endif
3125     }
3126 #endif
3127 }
3128
3129 /*
3130  * Called periodically by the scheduler
3131  */
3132 void osdUpdate(timeUs_t currentTimeUs)
3133 {
3134     static uint32_t counter = 0;
3135
3136     // don't touch buffers if DMA transaction is in progress
3137     if (displayIsTransferInProgress(osdDisplayPort)) {
3138         return;
3139     }
3140
3141     if (!osdDisplayIsReady) {
3142         osdCompleteAsyncInitialization();
3143         return;
3144     }
3145
3146 #if defined(OSD_ALTERNATE_LAYOUT_COUNT) && OSD_ALTERNATE_LAYOUT_COUNT > 0
3147     // Check if the layout has changed. Higher numbered
3148     // boxes take priority.
3149     unsigned activeLayout;
3150     if (layoutOverride >= 0) {
3151         activeLayout = layoutOverride;
3152         // Check for timed override, it will go into effect on
3153         // the next OSD iteration
3154         if (layoutOverrideUntil > 0 && millis() > layoutOverrideUntil) {
3155             layoutOverrideUntil = 0;
3156             layoutOverride = -1;
3157         }
3158     } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) {
3159         activeLayout = 0;
3160     } else {
3161 #if OSD_ALTERNATE_LAYOUT_COUNT > 2
3162         if (IS_RC_MODE_ACTIVE(BOXOSDALT3))
3163             activeLayout = 3;
3164         else
3165 #endif
3166 #if OSD_ALTERNATE_LAYOUT_COUNT > 1
3167         if (IS_RC_MODE_ACTIVE(BOXOSDALT2))
3168             activeLayout = 2;
3169         else
3170 #endif
3171         if (IS_RC_MODE_ACTIVE(BOXOSDALT1))
3172             activeLayout = 1;
3173         else
3174 #ifdef USE_PROGRAMMING_FRAMEWORK
3175         if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT))
3176             activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT);
3177         else
3178 #endif
3179             activeLayout = 0;
3180     }
3181     if (currentLayout != activeLayout) {
3182         currentLayout = activeLayout;
3183         osdStartFullRedraw();
3184     }
3185 #endif
3186
3187 #define DRAW_FREQ_DENOM     4
3188 #define STATS_FREQ_DENOM    50
3189     counter++;
3190
3191     if ((counter % STATS_FREQ_DENOM) == 0) {
3192         osdUpdateStats();
3193     }
3194
3195     if ((counter & DRAW_FREQ_DENOM) == 0) {
3196         // redraw values in buffer
3197         osdRefresh(currentTimeUs);
3198     } else {
3199         // rest of time redraw screen
3200         displayDrawScreen(osdDisplayPort);
3201     }
3202
3203 #ifdef USE_CMS
3204     // do not allow ARM if we are in menu
3205     if (displayIsGrabbed(osdDisplayPort)) {
3206         ENABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU);
3207     } else {
3208         DISABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU);
3209     }
3210 #endif
3211 }
3212
3213 void osdStartFullRedraw(void)
3214 {
3215     fullRedraw = true;
3216 }
3217
3218 void osdOverrideLayout(int layout, timeMs_t duration)
3219 {
3220     layoutOverride = constrain(layout, -1, ARRAYLEN(osdLayoutsConfig()->item_pos) - 1);
3221     if (layoutOverride >= 0 && duration > 0) {
3222         layoutOverrideUntil = millis() + duration;
3223     } else {
3224         layoutOverrideUntil = 0;
3225     }
3226 }
3227
3228 int osdGetActiveLayout(bool *overridden)
3229 {
3230     if (overridden) {
3231         *overridden = layoutOverride >= 0;
3232     }
3233     return currentLayout;
3234 }
3235
3236 bool osdItemIsFixed(osd_items_e item)
3237 {
3238     return item == OSD_CROSSHAIRS ||
3239         item == OSD_ARTIFICIAL_HORIZON ||
3240         item == OSD_HORIZON_SIDEBARS;
3241 }
3242
3243 displayPort_t *osdGetDisplayPort(void)
3244 {
3245     return osdDisplayPort;
3246 }
3247
3248 displayCanvas_t *osdGetDisplayPortCanvas(void)
3249 {
3250 #if defined(USE_CANVAS)
3251     if (osdDisplayHasCanvas) {
3252         return &osdCanvas;
3253     }
3254 #endif
3255     return NULL;
3256 }
3257
3258 #endif // OSD