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