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