src/main/io/osd.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  Created by Marcin Baliniak
  23  some functions based on MinimOSD
  24
  25  OSD-CMS separation by jflyper
  26  */
  27
  28 #include <stdbool.h>
  29 #include <stdint.h>
  30 #include <stdlib.h>
  31 #include <string.h>
  32 #include <ctype.h>
  33 #include <math.h>
  34
  35 #include "platform.h"
  36
  37 #ifdef USE_OSD
  38
  39 #include "blackbox/blackbox.h"
  40 #include "blackbox/blackbox_io.h"
  41
  42 #include "build/build_config.h"
  43 #include "build/debug.h"
  44 #include "build/version.h"
  45
  46 #include "cms/cms.h"
  47 #include "cms/cms_types.h"
  48
  49 #include "common/maths.h"
  50 #include "common/printf.h"
  51 #include "common/typeconversion.h"
  52 #include "common/utils.h"
  53
  54 #include "config/feature.h"
  55
  56 #include "drivers/display.h"
  57 #include "drivers/flash.h"
  58 #include "drivers/max7456_symbols.h"
  59 #include "drivers/sdcard.h"
  60 #include "drivers/time.h"
  61
  62 #include "fc/config.h"
  63 #include "fc/fc_core.h"
  64 #include "fc/rc_adjustments.h"
  65 #include "fc/rc_controls.h"
  66 #include "fc/runtime_config.h"
  67
  68 #include "flight/position.h"
  69 #include "flight/imu.h"
  70 #ifdef USE_ESC_SENSOR
  71 #include "flight/mixer.h"
  72 #endif
  73 #include "flight/pid.h"
  74
  75 #include "io/asyncfatfs/asyncfatfs.h"
  76 #include "io/beeper.h"
  77 #include "io/flashfs.h"
  78 #include "io/gps.h"
  79 #include "io/osd.h"
  80 #include "io/vtx_string.h"
  81 #include "io/vtx.h"
  82
  83 #include "pg/pg.h"
  84 #include "pg/pg_ids.h"
  85
  86 #include "rx/rx.h"
  87
  88 #include "sensors/adcinternal.h"
  89 #include "sensors/barometer.h"
  90 #include "sensors/battery.h"
  91 #include "sensors/esc_sensor.h"
  92 #include "sensors/sensors.h"
  93
  94 #ifdef USE_HARDWARE_REVISION_DETECTION
  95 #include "hardware_revision.h"
  96 #endif
  97
  98 #define VIDEO_BUFFER_CHARS_PAL    480
  99 #define FULL_CIRCLE 360
 100
 101 const char * const osdTimerSourceNames[] = {
 102     "ON TIME  ",
 103     "TOTAL ARM",
 104     "LAST ARM "
 105 };
 106
 107 // Blink control
 108
 109 static bool blinkState = true;
 110 static bool showVisualBeeper = false;
 111
 112 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
 113 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
 114 #define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
 115 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 116 #define BLINK(item) (IS_BLINK(item) && blinkState)
 117
 118 // Things in both OSD and CMS
 119
 120 #define IS_HI(X)  (rcData[X] > 1750)
 121 #define IS_LO(X)  (rcData[X] < 1250)
 122 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
 123
 124 static timeUs_t flyTime = 0;
 125
 126 typedef struct statistic_s {
 127     timeUs_t armed_time;
 128     int16_t max_speed;
 129     int16_t min_voltage; // /10
 130     int16_t max_current; // /10
 131     int16_t min_rssi;
 132     int32_t max_altitude;
 133     int16_t max_distance;
 134 } statistic_t;
 135
 136 static statistic_t stats;
 137
 138 timeUs_t resumeRefreshAt = 0;
 139 #define REFRESH_1S    1000 * 1000
 140
 141 static uint8_t armState;
 142 static bool lastArmState;
 143
 144 static displayPort_t *osdDisplayPort;
 145
 146 #ifdef USE_ESC_SENSOR
 147 static escSensorData_t *escDataCombined;
 148 #endif
 149
 150 #define AH_SYMBOL_COUNT 9
 151 #define AH_SIDEBAR_WIDTH_POS 7
 152 #define AH_SIDEBAR_HEIGHT_POS 3
 153
 154 static const char compassBar[] = {
 155   SYM_HEADING_W,
 156   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 157   SYM_HEADING_N,
 158   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 159   SYM_HEADING_E,
 160   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 161   SYM_HEADING_S,
 162   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 163   SYM_HEADING_W,
 164   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
 165   SYM_HEADING_N,
 166   SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE
 167 };
 168
 169 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3);
 170
 171 /**
 172  * Gets the correct altitude symbol for the current unit system
 173  */
 174 static char osdGetMetersToSelectedUnitSymbol(void)
 175 {
 176     switch (osdConfig()->units) {
 177     case OSD_UNIT_IMPERIAL:
 178         return SYM_FT;
 179     default:
 180         return SYM_M;
 181     }
 182 }
 183
 184 /**
 185  * Gets average battery cell voltage in 0.01V units.
 186  */
 187 static int osdGetBatteryAverageCellVoltage(void)
 188 {
 189     return (getBatteryVoltage() * 10) / getBatteryCellCount();
 190 }
 191
 192 static char osdGetBatterySymbol(int cellVoltage)
 193 {
 194     if (getBatteryState() == BATTERY_CRITICAL) {
 195         return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
 196     } else {
 197         // Calculate a symbol offset using cell voltage over full cell voltage range
 198         const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7);
 199         return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
 200     }
 201 }
 202
 203 /**
 204  * Converts altitude based on the current unit system.
 205  * @param meters Value in meters to convert
 206  */
 207 static int32_t osdGetMetersToSelectedUnit(int32_t meters)
 208 {
 209     switch (osdConfig()->units) {
 210     case OSD_UNIT_IMPERIAL:
 211         return (meters * 328) / 100; // Convert to feet / 100
 212     default:
 213         return meters;               // Already in metre / 100
 214     }
 215 }
 216
 217 #if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
 218 STATIC_UNIT_TESTED int osdConvertTemperatureToSelectedUnit(int tempInDeciDegrees)
 219 {
 220     switch (osdConfig()->units) {
 221     case OSD_UNIT_IMPERIAL:
 222         return ((tempInDeciDegrees * 9) / 5) + 320;
 223     default:
 224         return tempInDeciDegrees;
 225     }
 226 }
 227
 228 static char osdGetTemperatureSymbolForSelectedUnit(void)
 229 {
 230     switch (osdConfig()->units) {
 231     case OSD_UNIT_IMPERIAL:
 232         return 'F';
 233     default:
 234         return 'C';
 235     }
 236 }
 237 #endif
 238
 239 static void osdFormatAltitudeString(char * buff, int altitude, bool pad)
 240 {
 241     const int alt = osdGetMetersToSelectedUnit(altitude);
 242     int altitudeIntergerPart = abs(alt / 100);
 243     if (alt < 0) {
 244         altitudeIntergerPart *= -1;
 245     }
 246     tfp_sprintf(buff, pad ? "%4d.%01d%c" : "%d.%01d%c", altitudeIntergerPart, abs((alt % 100) / 10), osdGetMetersToSelectedUnitSymbol());
 247 }
 248
 249 static void osdFormatPID(char * buff, const char * label, const pid8_t * pid)
 250 {
 251     tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
 252 }
 253
 254 static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
 255 {
 256     heading += FULL_CIRCLE;  // Ensure positive value
 257
 258     // Split input heading 0..359 into sectors 0..(directions-1), but offset
 259     // by half a sector so that sector 0 gets centered around heading 0.
 260     // We multiply heading by directions to not loose precision in divisions
 261     // In this way each segment will be a FULL_CIRCLE length
 262     int direction = (heading * directions + FULL_CIRCLE / 2) /  FULL_CIRCLE; // scale with rounding
 263     direction %= directions; // normalize
 264
 265     return direction; // return segment number
 266 }
 267
 268 static uint8_t osdGetDirectionSymbolFromHeading(int heading)
 269 {
 270     heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
 271
 272     // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
 273     // Our symbols are Down=0, Right=4, Up=8 and Left=12
 274     // There're 16 arrow symbols. Transform it.
 275     heading = 16 - heading;
 276     heading = (heading + 8) % 16;
 277
 278     return SYM_ARROW_SOUTH + heading;
 279 }
 280
 281 static char osdGetTimerSymbol(osd_timer_source_e src)
 282 {
 283     switch (src) {
 284     case OSD_TIMER_SRC_ON:
 285         return SYM_ON_M;
 286     case OSD_TIMER_SRC_TOTAL_ARMED:
 287     case OSD_TIMER_SRC_LAST_ARMED:
 288         return SYM_FLY_M;
 289     default:
 290         return ' ';
 291     }
 292 }
 293
 294 static timeUs_t osdGetTimerValue(osd_timer_source_e src)
 295 {
 296     switch (src) {
 297     case OSD_TIMER_SRC_ON:
 298         return micros();
 299     case OSD_TIMER_SRC_TOTAL_ARMED:
 300         return flyTime;
 301     case OSD_TIMER_SRC_LAST_ARMED:
 302         return stats.armed_time;
 303     default:
 304         return 0;
 305     }
 306 }
 307
 308 STATIC_UNIT_TESTED void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
 309 {
 310     int seconds = time / 1000000;
 311     const int minutes = seconds / 60;
 312     seconds = seconds % 60;
 313
 314     switch (precision) {
 315     case OSD_TIMER_PREC_SECOND:
 316     default:
 317         tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
 318         break;
 319     case OSD_TIMER_PREC_HUNDREDTHS:
 320         {
 321             const int hundredths = (time / 10000) % 100;
 322             tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
 323             break;
 324         }
 325     }
 326 }
 327
 328 STATIC_UNIT_TESTED void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
 329 {
 330     const uint16_t timer = osdConfig()->timers[timerIndex];
 331     const uint8_t src = OSD_TIMER_SRC(timer);
 332
 333     if (showSymbol) {
 334         *(buff++) = osdGetTimerSymbol(src);
 335     }
 336
 337     osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
 338 }
 339
 340 #ifdef USE_GPS
 341 static void osdFormatCoordinate(char *buff, char sym, int32_t val)
 342 {
 343     // latitude maximum integer width is 3 (-90).
 344     // longitude maximum integer width is 4 (-180).
 345     // We show 7 decimals, so we need to use 12 characters:
 346     // eg: s-180.1234567z   s=symbol, z=zero terminator, decimal separator  between 0 and 1
 347
 348     static const int coordinateMaxLength = 13;//12 for the number (4 + dot + 7) + 1 for the symbol
 349
 350     buff[0] = sym;
 351     const int32_t integerPart = val / GPS_DEGREES_DIVIDER;
 352     const int32_t decimalPart = labs(val % GPS_DEGREES_DIVIDER);
 353     const int written = tfp_sprintf(buff + 1, "%d.%07d", integerPart, decimalPart);
 354     // pad with blanks to coordinateMaxLength
 355     for (int pos = 1 + written; pos < coordinateMaxLength; ++pos) {
 356         buff[pos] = SYM_BLANK;
 357     }
 358     buff[coordinateMaxLength] = '\0';
 359 }
 360 #endif // USE_GPS
 361
 362 #ifdef USE_RTC_TIME
 363 static bool osdFormatRtcDateTime(char *buffer)
 364 {
 365     dateTime_t dateTime;
 366     if (!rtcGetDateTime(&dateTime)) {
 367         buffer[0] = '\0';
 368
 369         return false;
 370     }
 371
 372     dateTimeFormatLocalShort(buffer, &dateTime);
 373
 374     return true;
 375 }
 376 #endif
 377
 378 static void osdFormatMessage(char *buff, size_t size, const char *message)
 379 {
 380     memset(buff, SYM_BLANK, size);
 381     if (message) {
 382         memcpy(buff, message, strlen(message));
 383     }
 384     // Ensure buff is zero terminated
 385     buff[size - 1] = '\0';
 386 }
 387
 388 void osdStatSetState(uint8_t statIndex, bool enabled)
 389 {
 390     if (enabled) {
 391         osdConfigMutable()->enabled_stats |= (1 << statIndex);
 392     } else {
 393         osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
 394     }
 395 }
 396
 397 bool osdStatGetState(uint8_t statIndex)
 398 {
 399     return osdConfig()->enabled_stats & (1 << statIndex);
 400 }
 401
 402 static bool osdDrawSingleElement(uint8_t item)
 403 {
 404     if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
 405         return false;
 406     }
 407
 408     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
 409     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
 410     char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
 411
 412     switch (item) {
 413     case OSD_RSSI_VALUE:
 414         {
 415             uint16_t osdRssi = getRssi() * 100 / 1024; // change range
 416             if (osdRssi >= 100)
 417                 osdRssi = 99;
 418
 419             tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi);
 420             break;
 421         }
 422
 423     case OSD_MAIN_BATT_VOLTAGE:
 424         buff[0] = osdGetBatterySymbol(osdGetBatteryAverageCellVoltage());
 425         tfp_sprintf(buff + 1, "%2d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
 426         break;
 427
 428     case OSD_CURRENT_DRAW:
 429         {
 430             const int32_t amperage = getAmperage();
 431             tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
 432             break;
 433         }
 434
 435     case OSD_MAH_DRAWN:
 436         tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH);
 437         break;
 438
 439 #ifdef USE_GPS
 440     case OSD_GPS_SATS:
 441         tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
 442         break;
 443
 444     case OSD_GPS_SPEED:
 445         // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K (M for MPH)
 446         switch (osdConfig()->units) {
 447         case OSD_UNIT_IMPERIAL:
 448             tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed));
 449             break;
 450         default:
 451             tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
 452             break;
 453         }
 454         break;
 455
 456     case OSD_GPS_LAT:
 457         osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat);
 458         break;
 459
 460     case OSD_GPS_LON:
 461         osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon);
 462         break;
 463
 464     case OSD_HOME_DIR:
 465         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 466             if (GPS_distanceToHome > 0) {
 467                 const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 468                 buff[0] = osdGetDirectionSymbolFromHeading(h);
 469             } else {
 470                 // We don't have a HOME symbol in the font, by now we use this
 471                 buff[0] = SYM_THR1;
 472             }
 473
 474         } else {
 475             // We use this symbol when we don't have a FIX
 476             buff[0] = SYM_COLON;
 477         }
 478
 479         buff[1] = 0;
 480
 481         break;
 482
 483     case OSD_HOME_DIST:
 484         if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
 485             const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome);
 486             tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
 487         } else {
 488             // We use this symbol when we don't have a FIX
 489             buff[0] = SYM_COLON;
 490             // overwrite any previous distance with blanks
 491             memset(buff + 1, SYM_BLANK, 6);
 492             buff[7] = '\0';
 493         }
 494         break;
 495
 496 #endif // GPS
 497
 498     case OSD_COMPASS_BAR:
 499         memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
 500         buff[9] = 0;
 501         break;
 502
 503     case OSD_ALTITUDE:
 504         osdFormatAltitudeString(buff, getEstimatedAltitude(), true);
 505         break;
 506
 507     case OSD_ITEM_TIMER_1:
 508     case OSD_ITEM_TIMER_2:
 509         osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1);
 510         break;
 511
 512     case OSD_REMAINING_TIME_ESTIMATE:
 513         {
 514             const int mAhDrawn = getMAhDrawn();
 515             const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn);
 516
 517             if (mAhDrawn < 0.1 * osdConfig()->cap_alarm) {
 518                 tfp_sprintf(buff, "--:--");
 519             } else if (mAhDrawn > osdConfig()->cap_alarm) {
 520                 tfp_sprintf(buff, "00:00");
 521             } else {
 522                 osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time);
 523             }
 524             break;
 525         }
 526
 527     case OSD_FLYMODE:
 528         {
 529             if (FLIGHT_MODE(FAILSAFE_MODE)) {
 530                 strcpy(buff, "!FS!");
 531             } else if (FLIGHT_MODE(ANGLE_MODE)) {
 532                 strcpy(buff, "STAB");
 533             } else if (FLIGHT_MODE(HORIZON_MODE)) {
 534                 strcpy(buff, "HOR ");
 535             } else if (isAirmodeActive()) {
 536                 strcpy(buff, "AIR ");
 537             } else {
 538                 strcpy(buff, "ACRO");
 539             }
 540
 541             break;
 542         }
 543
 544     case OSD_ANTI_GRAVITY:
 545         {
 546             if (pidItermAccelerator() > 1.0f) {
 547                 strcpy(buff, "AG");
 548             }
 549
 550             break;
 551         }
 552
 553     case OSD_CRAFT_NAME:
 554         // This does not strictly support iterative updating if the craft name changes at run time. But since the craft name is not supposed to be changing this should not matter, and blanking the entire length of the craft name string on update will make it impossible to configure elements to be displayed on the right hand side of the craft name.
 555         //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered.
 556
 557         if (strlen(pilotConfig()->name) == 0) {
 558             strcpy(buff, "CRAFT_NAME");
 559         } else {
 560             unsigned i;
 561             for (i = 0; i < MAX_NAME_LENGTH; i++) {
 562                 if (pilotConfig()->name[i]) {
 563                     buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
 564                 } else {
 565                     break;
 566                 }
 567             }
 568             buff[i] = '\0';
 569         }
 570
 571         break;
 572
 573     case OSD_THROTTLE_POS:
 574         buff[0] = SYM_THR;
 575         buff[1] = SYM_THR1;
 576         tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
 577         break;
 578
 579 #if defined(USE_VTX_COMMON)
 580     case OSD_VTX_CHANNEL:
 581         {
 582             const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band];
 583             const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel];
 584             uint8_t vtxPower = vtxSettingsConfig()->power;
 585             const vtxDevice_t *vtxDevice = vtxCommonDevice();
 586             if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
 587                 vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
 588             }
 589             tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
 590             break;
 591         }
 592 #endif
 593
 594     case OSD_CROSSHAIRS:
 595         buff[0] = SYM_AH_CENTER_LINE;
 596         buff[1] = SYM_AH_CENTER;
 597         buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 598         buff[3] = 0;
 599         break;
 600
 601     case OSD_ARTIFICIAL_HORIZON:
 602         {
 603             // Get pitch and roll limits in tenths of degrees
 604             const int maxPitch = osdConfig()->ahMaxPitch * 10;
 605             const int maxRoll = osdConfig()->ahMaxRoll * 10;
 606             const int rollAngle = constrain(attitude.values.roll, -maxRoll, maxRoll);
 607             int pitchAngle = constrain(attitude.values.pitch, -maxPitch, maxPitch);
 608             // Convert pitchAngle to y compensation value
 609             // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
 610             pitchAngle = ((pitchAngle * 25) / maxPitch) - 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
 611
 612             for (int x = -4; x <= 4; x++) {
 613                 const int y = ((-rollAngle * x) / 64) - pitchAngle;
 614                 if (y >= 0 && y <= 81) {
 615                     displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
 616                 }
 617             }
 618
 619             return true;
 620         }
 621
 622     case OSD_HORIZON_SIDEBARS:
 623         {
 624             // Draw AH sides
 625             const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
 626             const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
 627             for (int y = -hudheight; y <= hudheight; y++) {
 628                 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION);
 629                 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION);
 630             }
 631
 632             // AH level indicators
 633             displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
 634             displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
 635
 636             return true;
 637         }
 638
 639     case OSD_ROLL_PIDS:
 640         osdFormatPID(buff, "ROL", &currentPidProfile->pid[PID_ROLL]);
 641         break;
 642
 643     case OSD_PITCH_PIDS:
 644         osdFormatPID(buff, "PIT", &currentPidProfile->pid[PID_PITCH]);
 645         break;
 646
 647     case OSD_YAW_PIDS:
 648         osdFormatPID(buff, "YAW", &currentPidProfile->pid[PID_YAW]);
 649         break;
 650
 651     case OSD_POWER:
 652         tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000);
 653         break;
 654
 655     case OSD_PIDRATE_PROFILE:
 656         tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
 657         break;
 658
 659     case OSD_WARNINGS:
 660         {
 661
 662 #define OSD_WARNINGS_MAX_SIZE 11
 663 #define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
 664
 665             STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
 666
 667             const uint16_t enabledWarnings = osdConfig()->enabledWarnings;
 668
 669             const batteryState_e batteryState = getBatteryState();
 670
 671             if (enabledWarnings & OSD_WARNING_BATTERY_CRITICAL && batteryState == BATTERY_CRITICAL) {
 672                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW");
 673                 break;
 674             }
 675
 676 #ifdef USE_ESC_SENSOR
 677             // Show warning if we lose motor output, the ESC is overheating or excessive current draw
 678             if (feature(FEATURE_ESC_SENSOR) && enabledWarnings & OSD_WARNING_ESC_FAIL) {
 679                 char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
 680                 unsigned pos = 0;
 681
 682                 const char *title = "ESC";
 683
 684                 // center justify message
 685                 while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
 686                     escWarningMsg[pos++] = ' ';
 687                 }
 688
 689                 strcpy(escWarningMsg + pos, title);
 690                 pos += strlen(title);
 691
 692                 unsigned i = 0;
 693                 unsigned escWarningCount = 0;
 694                 while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
 695                     escSensorData_t *escData = getEscSensorData(i);
 696                     const char motorNumber = '1' + i;
 697                     // if everything is OK just display motor number else R, T or C
 698                     char warnFlag = motorNumber;
 699                     if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
 700                         warnFlag = 'R';
 701                     }
 702                     if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
 703                         warnFlag = 'T';
 704                     }
 705                     if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
 706                         warnFlag = 'C';
 707                     }
 708
 709                     escWarningMsg[pos++] = warnFlag;
 710
 711                     if (warnFlag != motorNumber) {
 712                         escWarningCount++;
 713                     }
 714
 715                     i++;
 716                 }
 717
 718                 escWarningMsg[pos] = '\0';
 719
 720                 if (escWarningCount > 0) {
 721                     osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg);
 722                 }
 723                 break;
 724             }
 725 #endif
 726
 727             // Warn when in flip over after crash mode
 728             if ((enabledWarnings & OSD_WARNING_CRASH_FLIP)
 729                   && (isFlipOverAfterCrashMode())) {
 730                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP");
 731                 break;
 732             }
 733
 734             // Show most severe reason for arming being disabled
 735             if (enabledWarnings & OSD_WARNING_ARMING_DISABLE && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
 736                 const armingDisableFlags_e flags = getArmingDisableFlags();
 737                 for (int i = 0; i < ARMING_DISABLE_FLAGS_COUNT; i++) {
 738                     if (flags & (1 << i)) {
 739                         osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[i]);
 740                         break;
 741                     }
 742                 }
 743                 break;
 744             }
 745
 746             if (enabledWarnings & OSD_WARNING_BATTERY_WARNING && batteryState == BATTERY_WARNING) {
 747                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
 748                 break;
 749             }
 750
 751             // Show warning if battery is not fresh
 752             if (enabledWarnings & OSD_WARNING_BATTERY_NOT_FULL && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK)
 753                   && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
 754                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL");
 755                 break;
 756             }
 757
 758             // Visual beeper
 759             if (enabledWarnings & OSD_WARNING_VISUAL_BEEPER && showVisualBeeper) {
 760                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "  * * * *");
 761                 break;
 762             }
 763
 764             osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL);
 765             break;
 766         }
 767
 768     case OSD_AVG_CELL_VOLTAGE:
 769         {
 770             const int cellV = osdGetBatteryAverageCellVoltage();
 771             buff[0] = osdGetBatterySymbol(cellV);
 772             tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
 773             break;
 774         }
 775
 776     case OSD_DEBUG:
 777         tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
 778         break;
 779
 780     case OSD_PITCH_ANGLE:
 781     case OSD_ROLL_ANGLE:
 782         {
 783             const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
 784             tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
 785             break;
 786         }
 787
 788     case OSD_MAIN_BATT_USAGE:
 789         {
 790             // Set length of indicator bar
 791             #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
 792
 793             // Calculate constrained value
 794             const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
 795
 796             // Calculate mAh used progress
 797             const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
 798
 799             // Create empty battery indicator bar
 800             buff[0] = SYM_PB_START;
 801             for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
 802                 buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
 803             }
 804             buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
 805             if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
 806                 buff[1 + mAhUsedProgress] = SYM_PB_END;
 807             }
 808             buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
 809             break;
 810         }
 811
 812     case OSD_DISARMED:
 813         if (!ARMING_FLAG(ARMED)) {
 814             tfp_sprintf(buff, "DISARMED");
 815         } else {
 816             if (!lastArmState) {  // previously disarmed - blank out the message one time
 817                 tfp_sprintf(buff, "        ");
 818             }
 819         }
 820         break;
 821
 822     case OSD_NUMERICAL_HEADING:
 823         {
 824             const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 825             tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
 826             break;
 827         }
 828
 829     case OSD_NUMERICAL_VARIO:
 830         {
 831             const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
 832             const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH;
 833             tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
 834             break;
 835         }
 836
 837 #ifdef USE_ESC_SENSOR
 838     case OSD_ESC_TMP:
 839         if (feature(FEATURE_ESC_SENSOR)) {
 840             tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
 841         }
 842         break;
 843
 844     case OSD_ESC_RPM:
 845         if (feature(FEATURE_ESC_SENSOR)) {
 846             tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm));
 847         }
 848         break;
 849 #endif
 850
 851 #ifdef USE_RTC_TIME
 852     case OSD_RTC_DATETIME:
 853         osdFormatRtcDateTime(&buff[0]);
 854         break;
 855 #endif
 856
 857 #ifdef USE_OSD_ADJUSTMENTS
 858     case OSD_ADJUSTMENT_RANGE:
 859         tfp_sprintf(buff, "%s: %3d", adjustmentRangeName, adjustmentRangeValue);
 860         break;
 861 #endif
 862
 863 #ifdef USE_ADC_INTERNAL
 864     case OSD_CORE_TEMPERATURE:
 865         tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius() * 10) / 10, osdGetTemperatureSymbolForSelectedUnit());
 866         break;
 867 #endif
 868
 869     default:
 870         return false;
 871     }
 872
 873     displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
 874
 875     return true;
 876 }
 877
 878 static void osdDrawElements(void)
 879 {
 880     displayClearScreen(osdDisplayPort);
 881
 882     // Hide OSD when OSDSW mode is active
 883     if (IS_RC_MODE_ACTIVE(BOXOSD)) {
 884         return;
 885     }
 886
 887     if (sensors(SENSOR_ACC)) {
 888         osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
 889     }
 890
 891     osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE);
 892     osdDrawSingleElement(OSD_RSSI_VALUE);
 893     osdDrawSingleElement(OSD_CROSSHAIRS);
 894     osdDrawSingleElement(OSD_HORIZON_SIDEBARS);
 895     osdDrawSingleElement(OSD_ITEM_TIMER_1);
 896     osdDrawSingleElement(OSD_ITEM_TIMER_2);
 897     osdDrawSingleElement(OSD_REMAINING_TIME_ESTIMATE);
 898     osdDrawSingleElement(OSD_FLYMODE);
 899     osdDrawSingleElement(OSD_THROTTLE_POS);
 900     osdDrawSingleElement(OSD_VTX_CHANNEL);
 901     osdDrawSingleElement(OSD_CURRENT_DRAW);
 902     osdDrawSingleElement(OSD_MAH_DRAWN);
 903     osdDrawSingleElement(OSD_CRAFT_NAME);
 904     osdDrawSingleElement(OSD_ALTITUDE);
 905     osdDrawSingleElement(OSD_ROLL_PIDS);
 906     osdDrawSingleElement(OSD_PITCH_PIDS);
 907     osdDrawSingleElement(OSD_YAW_PIDS);
 908     osdDrawSingleElement(OSD_POWER);
 909     osdDrawSingleElement(OSD_PIDRATE_PROFILE);
 910     osdDrawSingleElement(OSD_WARNINGS);
 911     osdDrawSingleElement(OSD_AVG_CELL_VOLTAGE);
 912     osdDrawSingleElement(OSD_DEBUG);
 913     osdDrawSingleElement(OSD_PITCH_ANGLE);
 914     osdDrawSingleElement(OSD_ROLL_ANGLE);
 915     osdDrawSingleElement(OSD_MAIN_BATT_USAGE);
 916     osdDrawSingleElement(OSD_DISARMED);
 917     osdDrawSingleElement(OSD_NUMERICAL_HEADING);
 918     osdDrawSingleElement(OSD_NUMERICAL_VARIO);
 919     osdDrawSingleElement(OSD_COMPASS_BAR);
 920     osdDrawSingleElement(OSD_ANTI_GRAVITY);
 921
 922 #ifdef USE_GPS
 923     if (sensors(SENSOR_GPS)) {
 924         osdDrawSingleElement(OSD_GPS_SATS);
 925         osdDrawSingleElement(OSD_GPS_SPEED);
 926         osdDrawSingleElement(OSD_GPS_LAT);
 927         osdDrawSingleElement(OSD_GPS_LON);
 928         osdDrawSingleElement(OSD_HOME_DIST);
 929         osdDrawSingleElement(OSD_HOME_DIR);
 930     }
 931 #endif // GPS
 932
 933 #ifdef USE_ESC_SENSOR
 934     if (feature(FEATURE_ESC_SENSOR)) {
 935         osdDrawSingleElement(OSD_ESC_TMP);
 936         osdDrawSingleElement(OSD_ESC_RPM);
 937     }
 938 #endif
 939
 940 #ifdef USE_RTC_TIME
 941     osdDrawSingleElement(OSD_RTC_DATETIME);
 942 #endif
 943
 944 #ifdef USE_OSD_ADJUSTMENTS
 945     osdDrawSingleElement(OSD_ADJUSTMENT_RANGE);
 946 #endif
 947
 948 #ifdef USE_ADC_INTERNAL
 949     osdDrawSingleElement(OSD_CORE_TEMPERATURE);
 950 #endif
 951 }
 952
 953 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 954 {
 955     // Position elements near centre of screen and disabled by default
 956     for (int i = 0; i < OSD_ITEM_COUNT; i++) {
 957         osdConfig->item_pos[i] = OSD_POS(10, 7);
 958     }
 959
 960     // Always enable warnings elements by default
 961     osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | VISIBLE_FLAG;
 962
 963     // Default to old fixed positions for these elements
 964     osdConfig->item_pos[OSD_CROSSHAIRS]         = OSD_POS(13, 6);
 965     osdConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
 966     osdConfig->item_pos[OSD_HORIZON_SIDEBARS]   = OSD_POS(14, 6);
 967
 968     // Enable the default stats
 969     osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
 970     osdStatSetState(OSD_STAT_MAX_SPEED, true);
 971     osdStatSetState(OSD_STAT_MIN_BATTERY, true);
 972     osdStatSetState(OSD_STAT_MIN_RSSI, true);
 973     osdStatSetState(OSD_STAT_MAX_CURRENT, true);
 974     osdStatSetState(OSD_STAT_USED_MAH, true);
 975     osdStatSetState(OSD_STAT_BLACKBOX, true);
 976     osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, true);
 977     osdStatSetState(OSD_STAT_TIMER_2, true);
 978
 979     osdConfig->units = OSD_UNIT_METRIC;
 980
 981     // Enable all warnings by default
 982     osdConfig->enabledWarnings = UINT16_MAX;
 983
 984     osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10);
 985     osdConfig->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10);
 986
 987     osdConfig->rssi_alarm = 20;
 988     osdConfig->cap_alarm  = 2200;
 989     osdConfig->alt_alarm  = 100; // meters or feet depend on configuration
 990     osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
 991     osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
 992     osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
 993
 994     osdConfig->ahMaxPitch = 20; // 20 degrees
 995     osdConfig->ahMaxRoll = 40; // 40 degrees
 996 }
 997
 998 static void osdDrawLogo(int x, int y)
 999 {
1000     // display logo and help
1001     int fontOffset = 160;
1002     for (int row = 0; row < 4; row++) {
1003         for (int column = 0; column < 24; column++) {
1004             if (fontOffset <= SYM_END_OF_FONT)
1005                 displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
1006         }
1007     }
1008 }
1009
1010 void osdInit(displayPort_t *osdDisplayPortToUse)
1011 {
1012     if (!osdDisplayPortToUse) {
1013         return;
1014     }
1015
1016     BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
1017
1018     osdDisplayPort = osdDisplayPortToUse;
1019 #ifdef USE_CMS
1020     cmsDisplayPortRegister(osdDisplayPort);
1021 #endif
1022
1023     armState = ARMING_FLAG(ARMED);
1024
1025     memset(blinkBits, 0, sizeof(blinkBits));
1026
1027     displayClearScreen(osdDisplayPort);
1028
1029     osdDrawLogo(3, 1);
1030
1031     char string_buffer[30];
1032     tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
1033     displayWrite(osdDisplayPort, 20, 6, string_buffer);
1034 #ifdef USE_CMS
1035     displayWrite(osdDisplayPort, 7, 8,  CMS_STARTUP_HELP_TEXT1);
1036     displayWrite(osdDisplayPort, 11, 9, CMS_STARTUP_HELP_TEXT2);
1037     displayWrite(osdDisplayPort, 11, 10, CMS_STARTUP_HELP_TEXT3);
1038 #endif
1039
1040 #ifdef USE_RTC_TIME
1041     char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
1042     if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
1043         displayWrite(osdDisplayPort, 5, 12, dateTimeBuffer);
1044     }
1045 #endif
1046
1047     displayResync(osdDisplayPort);
1048
1049     resumeRefreshAt = micros() + (4 * REFRESH_1S);
1050 }
1051
1052 void osdUpdateAlarms(void)
1053 {
1054     // This is overdone?
1055
1056     int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
1057
1058     if (scaleRange(getRssi(), 0, 1024, 0, 100) < osdConfig()->rssi_alarm) {
1059         SET_BLINK(OSD_RSSI_VALUE);
1060     } else {
1061         CLR_BLINK(OSD_RSSI_VALUE);
1062     }
1063
1064     if (getBatteryState() == BATTERY_OK) {
1065         CLR_BLINK(OSD_WARNINGS);
1066         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1067         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1068     } else {
1069         SET_BLINK(OSD_WARNINGS);
1070         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
1071         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
1072     }
1073
1074     if (STATE(GPS_FIX) == 0) {
1075         SET_BLINK(OSD_GPS_SATS);
1076     } else {
1077         CLR_BLINK(OSD_GPS_SATS);
1078     }
1079
1080     for (int i = 0; i < OSD_TIMER_COUNT; i++) {
1081         const uint16_t timer = osdConfig()->timers[i];
1082         const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
1083         const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
1084         if (alarmTime != 0 && time >= alarmTime) {
1085             SET_BLINK(OSD_ITEM_TIMER_1 + i);
1086         } else {
1087             CLR_BLINK(OSD_ITEM_TIMER_1 + i);
1088         }
1089     }
1090
1091     if (getMAhDrawn() >= osdConfig()->cap_alarm) {
1092         SET_BLINK(OSD_MAH_DRAWN);
1093         SET_BLINK(OSD_MAIN_BATT_USAGE);
1094         SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1095     } else {
1096         CLR_BLINK(OSD_MAH_DRAWN);
1097         CLR_BLINK(OSD_MAIN_BATT_USAGE);
1098         CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1099     }
1100
1101     if (alt >= osdConfig()->alt_alarm) {
1102         SET_BLINK(OSD_ALTITUDE);
1103     } else {
1104         CLR_BLINK(OSD_ALTITUDE);
1105     }
1106
1107 #ifdef USE_ESC_SENSOR
1108     if (feature(FEATURE_ESC_SENSOR)) {
1109         // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
1110         if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
1111             SET_BLINK(OSD_ESC_TMP);
1112         } else {
1113             CLR_BLINK(OSD_ESC_TMP);
1114         }
1115     }
1116 #endif
1117 }
1118
1119 void osdResetAlarms(void)
1120 {
1121     CLR_BLINK(OSD_RSSI_VALUE);
1122     CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
1123     CLR_BLINK(OSD_WARNINGS);
1124     CLR_BLINK(OSD_GPS_SATS);
1125     CLR_BLINK(OSD_MAH_DRAWN);
1126     CLR_BLINK(OSD_ALTITUDE);
1127     CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
1128     CLR_BLINK(OSD_MAIN_BATT_USAGE);
1129     CLR_BLINK(OSD_ITEM_TIMER_1);
1130     CLR_BLINK(OSD_ITEM_TIMER_2);
1131     CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
1132     CLR_BLINK(OSD_ESC_TMP);
1133 }
1134
1135 static void osdResetStats(void)
1136 {
1137     stats.max_current  = 0;
1138     stats.max_speed    = 0;
1139     stats.min_voltage  = 500;
1140     stats.max_current  = 0;
1141     stats.min_rssi     = 99;
1142     stats.max_altitude = 0;
1143     stats.max_distance = 0;
1144     stats.armed_time   = 0;
1145 }
1146
1147 static void osdUpdateStats(void)
1148 {
1149     int16_t value = 0;
1150 #ifdef USE_GPS
1151     switch (osdConfig()->units) {
1152     case OSD_UNIT_IMPERIAL:
1153         value = CM_S_TO_MPH(gpsSol.groundSpeed);
1154         break;
1155     default:
1156         value = CM_S_TO_KM_H(gpsSol.groundSpeed);
1157         break;
1158     }
1159 #endif
1160     if (stats.max_speed < value) {
1161         stats.max_speed = value;
1162     }
1163
1164     value = getBatteryVoltage();
1165     if (stats.min_voltage > value) {
1166         stats.min_voltage = value;
1167     }
1168
1169     value = getAmperage() / 100;
1170     if (stats.max_current < value) {
1171         stats.max_current = value;
1172     }
1173
1174     value = scaleRange(getRssi(), 0, 1024, 0, 100);
1175     if (stats.min_rssi > value) {
1176         stats.min_rssi = value;
1177     }
1178
1179     int altitude = getEstimatedAltitude();
1180     if (stats.max_altitude < altitude) {
1181         stats.max_altitude = altitude;
1182     }
1183
1184 #ifdef USE_GPS
1185     if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
1186         value = GPS_distanceToHome;
1187
1188         if (stats.max_distance < GPS_distanceToHome) {
1189             stats.max_distance = GPS_distanceToHome;
1190         }
1191     }
1192 #endif
1193 }
1194
1195 #ifdef USE_BLACKBOX
1196 static void osdGetBlackboxStatusString(char * buff)
1197 {
1198     bool storageDeviceIsWorking = false;
1199     uint32_t storageUsed = 0;
1200     uint32_t storageTotal = 0;
1201
1202     switch (blackboxConfig()->device) {
1203 #ifdef USE_SDCARD
1204     case BLACKBOX_DEVICE_SDCARD:
1205         storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY);
1206         if (storageDeviceIsWorking) {
1207             storageTotal = sdcard_getMetadata()->numBlocks / 2000;
1208             storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
1209         }
1210         break;
1211 #endif
1212
1213 #ifdef USE_FLASHFS
1214     case BLACKBOX_DEVICE_FLASH:
1215         storageDeviceIsWorking = flashfsIsReady();
1216         if (storageDeviceIsWorking) {
1217             const flashGeometry_t *geometry = flashfsGetGeometry();
1218             storageTotal = geometry->totalSize / 1024;
1219             storageUsed = flashfsGetOffset() / 1024;
1220         }
1221         break;
1222 #endif
1223
1224     default:
1225         break;
1226     }
1227
1228     if (storageDeviceIsWorking) {
1229         const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
1230         tfp_sprintf(buff, "%d%%", storageUsedPercent);
1231     } else {
1232         tfp_sprintf(buff, "FAULT");
1233     }
1234 }
1235 #endif
1236
1237 static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
1238 {
1239     displayWrite(osdDisplayPort, 2, y, text);
1240     displayWrite(osdDisplayPort, 20, y, ":");
1241     displayWrite(osdDisplayPort, 22, y, value);
1242 }
1243
1244 /*
1245  * Test if there's some stat enabled
1246  */
1247 static bool isSomeStatEnabled(void)
1248 {
1249     return (osdConfig()->enabled_stats != 0);
1250 }
1251
1252 // *** IMPORTANT ***
1253 // The order of the OSD stats as displayed on-screen must match the osd_stats_e enumeration.
1254 // This is because the fields are presented in the configurator in the order of the enumeration
1255 // and we want the configuration order to match the on-screen display order.  If you change the
1256 // display order you *must* update the osd_stats_e enumeration to match. Additionally the
1257 // changes to the stats display order *must* be implemented in the configurator otherwise the
1258 // stats selections will not be populated correctly and the settings will become corrupted.
1259
1260 static void osdShowStats(uint16_t endBatteryVoltage)
1261 {
1262     uint8_t top = 2;
1263     char buff[OSD_ELEMENT_BUFFER_LENGTH];
1264
1265     displayClearScreen(osdDisplayPort);
1266     displayWrite(osdDisplayPort, 2, top++, "  --- STATS ---");
1267
1268     if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
1269         bool success = false;
1270 #ifdef USE_RTC_TIME
1271         success = osdFormatRtcDateTime(&buff[0]);
1272 #endif
1273         if (!success) {
1274             tfp_sprintf(buff, "NO RTC");
1275         }
1276
1277         displayWrite(osdDisplayPort, 2, top++, buff);
1278     }
1279
1280     if (osdStatGetState(OSD_STAT_TIMER_1)) {
1281         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
1282         osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
1283     }
1284
1285     if (osdStatGetState(OSD_STAT_TIMER_2)) {
1286         osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
1287         osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
1288     }
1289
1290     if (osdStatGetState(OSD_STAT_MAX_SPEED) && STATE(GPS_FIX)) {
1291         itoa(stats.max_speed, buff, 10);
1292         osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
1293     }
1294
1295     if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) {
1296         tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
1297         osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
1298     }
1299
1300     if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
1301         tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT);
1302         osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
1303     }
1304
1305     if (osdStatGetState(OSD_STAT_END_BATTERY)) {
1306         tfp_sprintf(buff, "%d.%1d%c", endBatteryVoltage / 10, endBatteryVoltage % 10, SYM_VOLT);
1307         osdDisplayStatisticLabel(top++, "END BATTERY", buff);
1308     }
1309
1310     if (osdStatGetState(OSD_STAT_BATTERY)) {
1311         tfp_sprintf(buff, "%d.%1d%c", getBatteryVoltage() / 10, getBatteryVoltage() % 10, SYM_VOLT);
1312         osdDisplayStatisticLabel(top++, "BATTERY", buff);
1313     }
1314
1315     if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
1316         itoa(stats.min_rssi, buff, 10);
1317         strcat(buff, "%");
1318         osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
1319     }
1320
1321     if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
1322         if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
1323             itoa(stats.max_current, buff, 10);
1324             strcat(buff, "A");
1325             osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
1326         }
1327
1328         if (osdStatGetState(OSD_STAT_USED_MAH)) {
1329             tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
1330             osdDisplayStatisticLabel(top++, "USED MAH", buff);
1331         }
1332     }
1333
1334     if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
1335         osdFormatAltitudeString(buff, stats.max_altitude, false);
1336         osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
1337     }
1338
1339 #ifdef USE_BLACKBOX
1340     if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
1341         osdGetBlackboxStatusString(buff);
1342         osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
1343     }
1344
1345     if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
1346         itoa(blackboxGetLogNumber(), buff, 10);
1347         osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
1348     }
1349 #endif
1350
1351 }
1352
1353 static void osdShowArmed(void)
1354 {
1355     displayClearScreen(osdDisplayPort);
1356     displayWrite(osdDisplayPort, 12, 7, "ARMED");
1357 }
1358
1359 STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
1360 {
1361     static timeUs_t lastTimeUs = 0;
1362     static bool osdStatsEnabled = false;
1363     static bool osdStatsVisible = false;
1364     static timeUs_t osdStatsRefreshTimeUs;
1365     static uint16_t endBatteryVoltage;
1366
1367     // detect arm/disarm
1368     if (armState != ARMING_FLAG(ARMED)) {
1369         if (ARMING_FLAG(ARMED)) {
1370             osdStatsEnabled = false;
1371             osdStatsVisible = false;
1372             osdResetStats();
1373             osdShowArmed();
1374             resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
1375         } else if (isSomeStatEnabled()
1376                    && (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
1377                        || !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
1378             osdStatsEnabled = true;
1379             resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
1380             endBatteryVoltage = getBatteryVoltage();
1381         }
1382
1383         armState = ARMING_FLAG(ARMED);
1384     }
1385
1386
1387     if (ARMING_FLAG(ARMED)) {
1388         osdUpdateStats();
1389         timeUs_t deltaT = currentTimeUs - lastTimeUs;
1390         flyTime += deltaT;
1391         stats.armed_time += deltaT;
1392     } else if (osdStatsEnabled) {  // handle showing/hiding stats based on OSD disable switch position
1393         if (displayIsGrabbed(osdDisplayPort)) {
1394             osdStatsEnabled = false;
1395             resumeRefreshAt = 0;
1396             stats.armed_time = 0;
1397         } else {
1398             if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
1399                 osdStatsVisible = false;
1400                 displayClearScreen(osdDisplayPort);
1401             } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
1402                 if (!osdStatsVisible) {
1403                     osdStatsVisible = true;
1404                     osdStatsRefreshTimeUs = 0;
1405                 }
1406                 if (currentTimeUs >= osdStatsRefreshTimeUs) {
1407                     osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
1408                     osdShowStats(endBatteryVoltage);
1409                 }
1410             }
1411         }
1412     }
1413     lastTimeUs = currentTimeUs;
1414
1415     if (resumeRefreshAt) {
1416         if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
1417             // in timeout period, check sticks for activity to resume display.
1418             if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
1419                 resumeRefreshAt = currentTimeUs;
1420             }
1421             displayHeartbeat(osdDisplayPort);
1422             return;
1423         } else {
1424             displayClearScreen(osdDisplayPort);
1425             resumeRefreshAt = 0;
1426             osdStatsEnabled = false;
1427             stats.armed_time = 0;
1428         }
1429     }
1430
1431     blinkState = (currentTimeUs / 200000) % 2;
1432
1433 #ifdef USE_ESC_SENSOR
1434     if (feature(FEATURE_ESC_SENSOR)) {
1435         escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
1436     }
1437 #endif
1438
1439 #ifdef USE_CMS
1440     if (!displayIsGrabbed(osdDisplayPort)) {
1441         osdUpdateAlarms();
1442         osdDrawElements();
1443         displayHeartbeat(osdDisplayPort);
1444 #ifdef OSD_CALLS_CMS
1445     } else {
1446         cmsUpdate(currentTimeUs);
1447 #endif
1448     }
1449 #endif
1450     lastArmState = ARMING_FLAG(ARMED);
1451 }
1452
1453 /*
1454  * Called periodically by the scheduler
1455  */
1456 void osdUpdate(timeUs_t currentTimeUs)
1457 {
1458     static uint32_t counter = 0;
1459
1460     if (isBeeperOn()) {
1461         showVisualBeeper = true;
1462     }
1463
1464 #ifdef MAX7456_DMA_CHANNEL_TX
1465     // don't touch buffers if DMA transaction is in progress
1466     if (displayIsTransferInProgress(osdDisplayPort)) {
1467         return;
1468     }
1469 #endif // MAX7456_DMA_CHANNEL_TX
1470
1471 #ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
1472     static uint32_t idlecounter = 0;
1473     if (!ARMING_FLAG(ARMED)) {
1474         if (idlecounter++ % 4 != 0) {
1475             return;
1476         }
1477     }
1478 #endif
1479
1480     // redraw values in buffer
1481 #ifdef USE_MAX7456
1482 #define DRAW_FREQ_DENOM 5
1483 #else
1484 #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud (
1485 #endif
1486 #define STATS_FREQ_DENOM    50
1487
1488     if (counter % DRAW_FREQ_DENOM == 0) {
1489         osdRefresh(currentTimeUs);
1490         showVisualBeeper = false;
1491     } else {
1492         // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
1493         displayDrawScreen(osdDisplayPort);
1494     }
1495     ++counter;
1496
1497 #ifdef USE_CMS
1498     // do not allow ARM if we are in menu
1499     if (displayIsGrabbed(osdDisplayPort)) {
1500         setArmingDisabled(ARMING_DISABLED_OSD_MENU);
1501     } else {
1502         unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
1503     }
1504 #endif
1505 }
1506
1507 #endif // USE_OSD