Merge pull request #10309 from etracer65/gps_rescue_disable_headfree
[betaflight.git] / src / main / osd / osd.c
blobb2a6c977d376438643e822e87a3dabcde3d11eea
1 /*
2 * This file is part of Cleanflight and Betaflight.
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.
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
22 Created by Marcin Baliniak
23 some functions based on MinimOSD
25 OSD-CMS separation by jflyper
28 #include <stdbool.h>
29 #include <stdint.h>
30 #include <stdlib.h>
31 #include <string.h>
32 #include <ctype.h>
33 #include <math.h>
35 #include "platform.h"
37 #ifdef USE_OSD
39 #include "blackbox/blackbox.h"
40 #include "blackbox/blackbox_io.h"
42 #include "build/build_config.h"
43 #include "build/version.h"
45 #include "cms/cms.h"
47 #include "common/axis.h"
48 #include "common/maths.h"
49 #include "common/printf.h"
50 #include "common/typeconversion.h"
51 #include "common/utils.h"
53 #include "config/feature.h"
55 #include "drivers/display.h"
56 #include "drivers/dshot.h"
57 #include "drivers/flash.h"
58 #include "drivers/osd_symbols.h"
59 #include "drivers/sdcard.h"
60 #include "drivers/time.h"
62 #include "fc/rc_controls.h"
63 #include "fc/rc_modes.h"
64 #include "fc/runtime_config.h"
66 #if defined(USE_GYRO_DATA_ANALYSE)
67 #include "flight/gyroanalyse.h"
68 #endif
69 #include "flight/imu.h"
70 #include "flight/mixer.h"
71 #include "flight/position.h"
73 #include "io/asyncfatfs/asyncfatfs.h"
74 #include "io/beeper.h"
75 #include "io/flashfs.h"
76 #include "io/gps.h"
78 #include "osd/osd.h"
79 #include "osd/osd_elements.h"
81 #include "pg/motor.h"
82 #include "pg/pg.h"
83 #include "pg/pg_ids.h"
84 #include "pg/stats.h"
86 #include "rx/crsf.h"
87 #include "rx/rx.h"
89 #include "sensors/acceleration.h"
90 #include "sensors/battery.h"
91 #include "sensors/esc_sensor.h"
92 #include "sensors/sensors.h"
94 #ifdef USE_HARDWARE_REVISION_DETECTION
95 #include "hardware_revision.h"
96 #endif
98 typedef enum {
99 OSD_LOGO_ARMING_OFF,
100 OSD_LOGO_ARMING_ON,
101 OSD_LOGO_ARMING_FIRST
102 } osd_logo_on_arming_e;
104 const char * const osdTimerSourceNames[] = {
105 "ON TIME ",
106 "TOTAL ARM",
107 "LAST ARM ",
108 "ON/ARM "
111 // Things in both OSD and CMS
113 #define IS_HI(X) (rcData[X] > 1750)
114 #define IS_LO(X) (rcData[X] < 1250)
115 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
117 timeUs_t osdFlyTime = 0;
118 #if defined(USE_ACC)
119 float osdGForce = 0;
120 #endif
122 static bool showVisualBeeper = false;
124 static statistic_t stats;
125 timeUs_t resumeRefreshAt = 0;
126 #define REFRESH_1S 1000 * 1000
128 static uint8_t armState;
129 #ifdef USE_OSD_PROFILES
130 static uint8_t osdProfile = 1;
131 #endif
132 static displayPort_t *osdDisplayPort;
133 static osdDisplayPortDevice_e osdDisplayPortDeviceType;
134 static bool osdIsReady;
136 static bool suppressStatsDisplay = false;
137 static uint8_t osdStatsRowCount = 0;
139 static bool backgroundLayerSupported = false;
141 #ifdef USE_ESC_SENSOR
142 escSensorData_t *osdEscDataCombined;
143 #endif
145 STATIC_ASSERT(OSD_POS_MAX == OSD_POS(31,31), OSD_POS_MAX_incorrect);
147 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8);
149 PG_REGISTER_WITH_RESET_FN(osdElementConfig_t, osdElementConfig, PG_OSD_ELEMENT_CONFIG, 0);
151 // Controls the display order of the OSD post-flight statistics.
152 // Adjust the ordering here to control how the post-flight stats are presented.
153 // Every entry in osd_stats_e should be represented. Any that are missing will not
154 // be shown on the the post-flight statistics page.
155 // If you reorder the stats it's likely that you'll need to make likewise updates
156 // to the unit tests.
158 // If adding new stats, please add to the osdStatsNeedAccelerometer() function
159 // if the statistic utilizes the accelerometer.
161 const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
162 OSD_STAT_RTC_DATE_TIME,
163 OSD_STAT_TIMER_1,
164 OSD_STAT_TIMER_2,
165 OSD_STAT_MAX_ALTITUDE,
166 OSD_STAT_MAX_SPEED,
167 OSD_STAT_MAX_DISTANCE,
168 OSD_STAT_FLIGHT_DISTANCE,
169 OSD_STAT_MIN_BATTERY,
170 OSD_STAT_END_BATTERY,
171 OSD_STAT_BATTERY,
172 OSD_STAT_MIN_RSSI,
173 OSD_STAT_MAX_CURRENT,
174 OSD_STAT_USED_MAH,
175 OSD_STAT_BLACKBOX,
176 OSD_STAT_BLACKBOX_NUMBER,
177 OSD_STAT_MAX_G_FORCE,
178 OSD_STAT_MAX_ESC_TEMP,
179 OSD_STAT_MAX_ESC_RPM,
180 OSD_STAT_MIN_LINK_QUALITY,
181 OSD_STAT_MAX_FFT,
182 OSD_STAT_MIN_RSSI_DBM,
183 OSD_STAT_TOTAL_FLIGHTS,
184 OSD_STAT_TOTAL_TIME,
185 OSD_STAT_TOTAL_DIST,
188 void osdStatSetState(uint8_t statIndex, bool enabled)
190 if (enabled) {
191 osdConfigMutable()->enabled_stats |= (1 << statIndex);
192 } else {
193 osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
197 bool osdStatGetState(uint8_t statIndex)
199 return osdConfig()->enabled_stats & (1 << statIndex);
202 void osdWarnSetState(uint8_t warningIndex, bool enabled)
204 if (enabled) {
205 osdConfigMutable()->enabledWarnings |= (1 << warningIndex);
206 } else {
207 osdConfigMutable()->enabledWarnings &= ~(1 << warningIndex);
211 bool osdWarnGetState(uint8_t warningIndex)
213 return osdConfig()->enabledWarnings & (1 << warningIndex);
216 #ifdef USE_OSD_PROFILES
217 void setOsdProfile(uint8_t value)
219 // 1 ->> 001
220 // 2 ->> 010
221 // 3 ->> 100
222 if (value <= OSD_PROFILE_COUNT) {
223 if (value == 0) {
224 osdProfile = 1;
225 } else {
226 osdProfile = 1 << (value - 1);
231 uint8_t getCurrentOsdProfileIndex(void)
233 return osdConfig()->osdProfileIndex;
236 void changeOsdProfileIndex(uint8_t profileIndex)
238 if (profileIndex <= OSD_PROFILE_COUNT) {
239 osdConfigMutable()->osdProfileIndex = profileIndex;
240 setOsdProfile(profileIndex);
241 osdAnalyzeActiveElements();
244 #endif
246 void osdAnalyzeActiveElements(void)
248 osdAddActiveElements();
249 osdDrawActiveElementsBackground(osdDisplayPort);
252 static void osdDrawElements(timeUs_t currentTimeUs)
254 // Hide OSD when OSDSW mode is active
255 if (IS_RC_MODE_ACTIVE(BOXOSD)) {
256 displayClearScreen(osdDisplayPort);
257 return;
260 if (backgroundLayerSupported) {
261 // Background layer is supported, overlay it onto the foreground
262 // so that we only need to draw the active parts of the elements.
263 displayLayerCopy(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND, DISPLAYPORT_LAYER_BACKGROUND);
264 } else {
265 // Background layer not supported, just clear the foreground in preparation
266 // for drawing the elements including their backgrounds.
267 displayClearScreen(osdDisplayPort);
270 osdDrawActiveElements(osdDisplayPort, currentTimeUs);
273 const uint16_t osdTimerDefault[OSD_TIMER_COUNT] = {
274 OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10),
275 OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10)
278 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
280 // Enable the default stats
281 osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
282 osdStatSetState(OSD_STAT_MAX_SPEED, true);
283 osdStatSetState(OSD_STAT_MIN_BATTERY, true);
284 osdStatSetState(OSD_STAT_MIN_RSSI, true);
285 osdStatSetState(OSD_STAT_MAX_CURRENT, true);
286 osdStatSetState(OSD_STAT_USED_MAH, true);
287 osdStatSetState(OSD_STAT_BLACKBOX, true);
288 osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, true);
289 osdStatSetState(OSD_STAT_TIMER_2, true);
291 osdConfig->units = OSD_UNIT_METRIC;
293 // Enable all warnings by default
294 for (int i=0; i < OSD_WARNING_COUNT; i++) {
295 osdWarnSetState(i, true);
297 // turn off RSSI & Link Quality warnings by default
298 osdWarnSetState(OSD_WARNING_RSSI, false);
299 osdWarnSetState(OSD_WARNING_LINK_QUALITY, false);
300 osdWarnSetState(OSD_WARNING_RSSI_DBM, false);
301 // turn off the over mah capacity warning
302 osdWarnSetState(OSD_WARNING_OVER_CAP, false);
304 osdConfig->timers[OSD_TIMER_1] = osdTimerDefault[OSD_TIMER_1];
305 osdConfig->timers[OSD_TIMER_2] = osdTimerDefault[OSD_TIMER_2];
307 osdConfig->overlay_radio_mode = 2;
309 osdConfig->rssi_alarm = 20;
310 osdConfig->link_quality_alarm = 80;
311 osdConfig->cap_alarm = 2200;
312 osdConfig->alt_alarm = 100; // meters or feet depend on configuration
313 osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
314 osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
315 osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
316 osdConfig->core_temp_alarm = 70; // a temperature above 70C should produce a warning, lockups have been reported above 80C
318 osdConfig->ahMaxPitch = 20; // 20 degrees
319 osdConfig->ahMaxRoll = 40; // 40 degrees
321 osdConfig->osdProfileIndex = 1;
322 osdConfig->ahInvert = false;
323 for (int i=0; i < OSD_PROFILE_COUNT; i++) {
324 osdConfig->profile[i][0] = '\0';
326 osdConfig->rssi_dbm_alarm = -60;
327 osdConfig->gps_sats_show_hdop = false;
329 for (int i = 0; i < OSD_RCCHANNELS_COUNT; i++) {
330 osdConfig->rcChannels[i] = -1;
333 osdConfig->displayPortDevice = OSD_DISPLAYPORT_DEVICE_AUTO;
335 osdConfig->distance_alarm = 0;
336 osdConfig->logo_on_arming = OSD_LOGO_ARMING_OFF;
337 osdConfig->logo_on_arming_duration = 5; // 0.5 seconds
339 osdConfig->camera_frame_width = 24;
340 osdConfig->camera_frame_height = 11;
343 void pgResetFn_osdElementConfig(osdElementConfig_t *osdElementConfig)
345 // Position elements near centre of screen and disabled by default
346 for (int i = 0; i < OSD_ITEM_COUNT; i++) {
347 osdElementConfig->item_pos[i] = OSD_POS(10, 7);
350 // Always enable warnings elements by default
351 uint16_t profileFlags = 0;
352 for (unsigned i = 1; i <= OSD_PROFILE_COUNT; i++) {
353 profileFlags |= OSD_PROFILE_FLAG(i);
355 osdElementConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | profileFlags;
357 // Default to old fixed positions for these elements
358 osdElementConfig->item_pos[OSD_CROSSHAIRS] = OSD_POS(13, 6);
359 osdElementConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
360 osdElementConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(14, 6);
361 osdElementConfig->item_pos[OSD_CAMERA_FRAME] = OSD_POS(3, 1);
364 static void osdDrawLogo(int x, int y)
366 // display logo and help
367 int fontOffset = 160;
368 for (int row = 0; row < 4; row++) {
369 for (int column = 0; column < 24; column++) {
370 if (fontOffset <= SYM_END_OF_FONT)
371 displayWriteChar(osdDisplayPort, x + column, y + row, DISPLAYPORT_ATTR_NONE, fontOffset++);
376 static void osdCompleteInitialization(void)
378 armState = ARMING_FLAG(ARMED);
380 osdResetAlarms();
382 backgroundLayerSupported = displayLayerSupported(osdDisplayPort, DISPLAYPORT_LAYER_BACKGROUND);
383 displayLayerSelect(osdDisplayPort, DISPLAYPORT_LAYER_FOREGROUND);
385 displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
386 displayClearScreen(osdDisplayPort);
388 osdDrawLogo(3, 1);
390 char string_buffer[30];
391 tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
392 displayWrite(osdDisplayPort, 20, 6, DISPLAYPORT_ATTR_NONE, string_buffer);
393 #ifdef USE_CMS
394 displayWrite(osdDisplayPort, 7, 8, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT1);
395 displayWrite(osdDisplayPort, 11, 9, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT2);
396 displayWrite(osdDisplayPort, 11, 10, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT3);
397 #endif
399 #ifdef USE_RTC_TIME
400 char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
401 if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
402 displayWrite(osdDisplayPort, 5, 12, DISPLAYPORT_ATTR_NONE, dateTimeBuffer);
404 #endif
406 resumeRefreshAt = micros() + (4 * REFRESH_1S);
407 #ifdef USE_OSD_PROFILES
408 setOsdProfile(osdConfig()->osdProfileIndex);
409 #endif
411 osdElementsInit(backgroundLayerSupported);
412 osdAnalyzeActiveElements();
413 displayCommitTransaction(osdDisplayPort);
415 osdIsReady = true;
418 void osdInit(displayPort_t *osdDisplayPortToUse, osdDisplayPortDevice_e displayPortDeviceType)
420 osdDisplayPortDeviceType = displayPortDeviceType;
422 if (!osdDisplayPortToUse) {
423 return;
426 osdDisplayPort = osdDisplayPortToUse;
427 #ifdef USE_CMS
428 cmsDisplayPortRegister(osdDisplayPort);
429 #endif
431 if (displayIsReady(osdDisplayPort)) {
432 osdCompleteInitialization();
436 bool osdInitialized(void)
438 return osdDisplayPort;
441 static void osdResetStats(void)
443 stats.max_current = 0;
444 stats.max_speed = 0;
445 stats.min_voltage = 5000;
446 stats.end_voltage = 0;
447 stats.min_rssi = 99; // percent
448 stats.max_altitude = 0;
449 stats.max_distance = 0;
450 stats.armed_time = 0;
451 stats.max_g_force = 0;
452 stats.max_esc_temp = 0;
453 stats.max_esc_rpm = 0;
454 stats.min_link_quality = (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) ? 100 : 99; // percent
455 stats.min_rssi_dbm = CRSF_SNR_MAX;
458 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
459 static int32_t getAverageEscRpm(void)
461 #ifdef USE_DSHOT_TELEMETRY
462 if (motorConfig()->dev.useDshotTelemetry) {
463 uint32_t rpm = 0;
464 for (int i = 0; i < getMotorCount(); i++) {
465 rpm += getDshotTelemetry(i);
467 rpm = rpm / getMotorCount();
468 return rpm * 100 * 2 / motorConfig()->motorPoleCount;
470 #endif
471 #ifdef USE_ESC_SENSOR
472 if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
473 return calcEscRpm(osdEscDataCombined->rpm);
475 #endif
476 return 0;
478 #endif
480 static void osdUpdateStats(void)
482 int16_t value = 0;
484 #ifdef USE_GPS
485 if (gpsConfig()->gps_use_3d_speed) {
486 value = gpsSol.speed3d;
487 } else {
488 value = gpsSol.groundSpeed;
490 if (stats.max_speed < value) {
491 stats.max_speed = value;
493 #endif
495 value = getBatteryVoltage();
496 if (stats.min_voltage > value) {
497 stats.min_voltage = value;
500 value = getAmperage() / 100;
501 if (stats.max_current < value) {
502 stats.max_current = value;
505 value = getRssiPercent();
506 if (stats.min_rssi > value) {
507 stats.min_rssi = value;
510 int32_t altitudeCm = getEstimatedAltitudeCm();
511 if (stats.max_altitude < altitudeCm) {
512 stats.max_altitude = altitudeCm;
515 #if defined(USE_ACC)
516 if (stats.max_g_force < osdGForce) {
517 stats.max_g_force = osdGForce;
519 #endif
521 #ifdef USE_RX_LINK_QUALITY_INFO
522 value = rxGetLinkQualityPercent();
523 if (stats.min_link_quality > value) {
524 stats.min_link_quality = value;
526 #endif
528 #ifdef USE_RX_RSSI_DBM
529 value = getRssiDbm();
530 if (stats.min_rssi_dbm > value) {
531 stats.min_rssi_dbm = value;
533 #endif
535 #ifdef USE_GPS
536 if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
537 if (stats.max_distance < GPS_distanceToHome) {
538 stats.max_distance = GPS_distanceToHome;
541 #endif
543 #ifdef USE_ESC_SENSOR
544 if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
545 value = osdEscDataCombined->temperature;
546 if (stats.max_esc_temp < value) {
547 stats.max_esc_temp = value;
550 #endif
552 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
553 int32_t rpm = getAverageEscRpm();
554 if (stats.max_esc_rpm < rpm) {
555 stats.max_esc_rpm = rpm;
557 #endif
560 #ifdef USE_BLACKBOX
562 static void osdGetBlackboxStatusString(char * buff)
564 bool storageDeviceIsWorking = isBlackboxDeviceWorking();
565 uint32_t storageUsed = 0;
566 uint32_t storageTotal = 0;
568 switch (blackboxConfig()->device) {
569 #ifdef USE_SDCARD
570 case BLACKBOX_DEVICE_SDCARD:
571 if (storageDeviceIsWorking) {
572 storageTotal = sdcard_getMetadata()->numBlocks / 2000;
573 storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
575 break;
576 #endif
578 #ifdef USE_FLASHFS
579 case BLACKBOX_DEVICE_FLASH:
580 if (storageDeviceIsWorking) {
582 const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
583 const flashGeometry_t *flashGeometry = flashGetGeometry();
585 storageTotal = ((FLASH_PARTITION_SECTOR_COUNT(flashPartition) * flashGeometry->sectorSize) / 1024);
586 storageUsed = flashfsGetOffset() / 1024;
588 break;
589 #endif
591 default:
592 break;
595 if (storageDeviceIsWorking) {
596 const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
597 tfp_sprintf(buff, "%d%%", storageUsedPercent);
598 } else {
599 tfp_sprintf(buff, "FAULT");
602 #endif
604 static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
606 displayWrite(osdDisplayPort, 2, y, DISPLAYPORT_ATTR_NONE, text);
607 displayWrite(osdDisplayPort, 20, y, DISPLAYPORT_ATTR_NONE, ":");
608 displayWrite(osdDisplayPort, 22, y, DISPLAYPORT_ATTR_NONE, value);
612 * Test if there's some stat enabled
614 static bool isSomeStatEnabled(void)
616 return (osdConfig()->enabled_stats != 0);
619 // *** IMPORTANT ***
620 // The stats display order was previously required to match the enumeration definition so it matched
621 // the order shown in the configurator. However, to allow reordering this screen without breaking the
622 // compatibility, this requirement has been relaxed to a best effort approach. Reordering the elements
623 // on the stats screen will have to be more beneficial than the hassle of not matching exactly to the
624 // configurator list.
626 static bool osdDisplayStat(int statistic, uint8_t displayRow)
628 char buff[OSD_ELEMENT_BUFFER_LENGTH];
630 switch (statistic) {
631 case OSD_STAT_RTC_DATE_TIME: {
632 bool success = false;
633 #ifdef USE_RTC_TIME
634 success = osdFormatRtcDateTime(&buff[0]);
635 #endif
636 if (!success) {
637 tfp_sprintf(buff, "NO RTC");
640 displayWrite(osdDisplayPort, 2, displayRow, DISPLAYPORT_ATTR_NONE, buff);
641 return true;
644 case OSD_STAT_TIMER_1:
645 osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
646 osdDisplayStatisticLabel(displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
647 return true;
649 case OSD_STAT_TIMER_2:
650 osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
651 osdDisplayStatisticLabel(displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
652 return true;
654 case OSD_STAT_MAX_ALTITUDE: {
655 const int alt = osdGetMetersToSelectedUnit(stats.max_altitude) / 10;
656 tfp_sprintf(buff, "%d.%d%c", alt / 10, alt % 10, osdGetMetersToSelectedUnitSymbol());
657 osdDisplayStatisticLabel(displayRow, "MAX ALTITUDE", buff);
658 return true;
661 #ifdef USE_GPS
662 case OSD_STAT_MAX_SPEED:
663 if (featureIsEnabled(FEATURE_GPS)) {
664 tfp_sprintf(buff, "%d%c", osdGetSpeedToSelectedUnit(stats.max_speed), osdGetSpeedToSelectedUnitSymbol());
665 osdDisplayStatisticLabel(displayRow, "MAX SPEED", buff);
666 return true;
668 break;
670 case OSD_STAT_MAX_DISTANCE:
671 if (featureIsEnabled(FEATURE_GPS)) {
672 osdFormatDistanceString(buff, stats.max_distance, SYM_NONE);
673 osdDisplayStatisticLabel(displayRow, "MAX DISTANCE", buff);
674 return true;
676 break;
678 case OSD_STAT_FLIGHT_DISTANCE:
679 if (featureIsEnabled(FEATURE_GPS)) {
680 const int distanceFlown = GPS_distanceFlownInCm / 100;
681 osdFormatDistanceString(buff, distanceFlown, SYM_NONE);
682 osdDisplayStatisticLabel(displayRow, "FLIGHT DISTANCE", buff);
683 return true;
685 break;
686 #endif
688 case OSD_STAT_MIN_BATTERY:
689 tfp_sprintf(buff, "%d.%02d%c", stats.min_voltage / 100, stats.min_voltage % 100, SYM_VOLT);
690 osdDisplayStatisticLabel(displayRow, "MIN BATTERY", buff);
691 return true;
693 case OSD_STAT_END_BATTERY:
694 tfp_sprintf(buff, "%d.%02d%c", stats.end_voltage / 100, stats.end_voltage % 100, SYM_VOLT);
695 osdDisplayStatisticLabel(displayRow, "END BATTERY", buff);
696 return true;
698 case OSD_STAT_BATTERY:
699 tfp_sprintf(buff, "%d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
700 osdDisplayStatisticLabel(displayRow, "BATTERY", buff);
701 return true;
703 case OSD_STAT_MIN_RSSI:
704 itoa(stats.min_rssi, buff, 10);
705 strcat(buff, "%");
706 osdDisplayStatisticLabel(displayRow, "MIN RSSI", buff);
707 return true;
709 case OSD_STAT_MAX_CURRENT:
710 if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
711 tfp_sprintf(buff, "%d%c", stats.max_current, SYM_AMP);
712 osdDisplayStatisticLabel(displayRow, "MAX CURRENT", buff);
713 return true;
715 break;
717 case OSD_STAT_USED_MAH:
718 if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
719 tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
720 osdDisplayStatisticLabel(displayRow, "USED MAH", buff);
721 return true;
723 break;
725 #ifdef USE_BLACKBOX
726 case OSD_STAT_BLACKBOX:
727 if (blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
728 osdGetBlackboxStatusString(buff);
729 osdDisplayStatisticLabel(displayRow, "BLACKBOX", buff);
730 return true;
732 break;
734 case OSD_STAT_BLACKBOX_NUMBER:
736 int32_t logNumber = blackboxGetLogNumber();
737 if (logNumber >= 0) {
738 itoa(logNumber, buff, 10);
739 osdDisplayStatisticLabel(displayRow, "BB LOG NUM", buff);
740 return true;
743 break;
744 #endif
746 #if defined(USE_ACC)
747 case OSD_STAT_MAX_G_FORCE:
748 if (sensors(SENSOR_ACC)) {
749 const int gForce = lrintf(stats.max_g_force * 10);
750 tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10);
751 osdDisplayStatisticLabel(displayRow, "MAX G-FORCE", buff);
752 return true;
754 break;
755 #endif
757 #ifdef USE_ESC_SENSOR
758 case OSD_STAT_MAX_ESC_TEMP:
759 tfp_sprintf(buff, "%d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
760 osdDisplayStatisticLabel(displayRow, "MAX ESC TEMP", buff);
761 return true;
762 #endif
764 #if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
765 case OSD_STAT_MAX_ESC_RPM:
766 itoa(stats.max_esc_rpm, buff, 10);
767 osdDisplayStatisticLabel(displayRow, "MAX ESC RPM", buff);
768 return true;
769 #endif
771 #ifdef USE_RX_LINK_QUALITY_INFO
772 case OSD_STAT_MIN_LINK_QUALITY:
773 tfp_sprintf(buff, "%d", stats.min_link_quality);
774 strcat(buff, "%");
775 osdDisplayStatisticLabel(displayRow, "MIN LINK", buff);
776 return true;
777 #endif
779 #if defined(USE_GYRO_DATA_ANALYSE)
780 case OSD_STAT_MAX_FFT:
781 if (featureIsEnabled(FEATURE_DYNAMIC_FILTER)) {
782 int value = getMaxFFT();
783 if (value > 0) {
784 tfp_sprintf(buff, "%dHZ", value);
785 osdDisplayStatisticLabel(displayRow, "PEAK FFT", buff);
786 } else {
787 osdDisplayStatisticLabel(displayRow, "PEAK FFT", "THRT<20%");
789 return true;
791 break;
792 #endif
794 #ifdef USE_RX_RSSI_DBM
795 case OSD_STAT_MIN_RSSI_DBM:
796 tfp_sprintf(buff, "%3d", stats.min_rssi_dbm);
797 osdDisplayStatisticLabel(displayRow, "MIN RSSI DBM", buff);
798 return true;
799 #endif
801 #ifdef USE_PERSISTENT_STATS
802 case OSD_STAT_TOTAL_FLIGHTS:
803 itoa(statsConfig()->stats_total_flights, buff, 10);
804 osdDisplayStatisticLabel(displayRow, "TOTAL FLIGHTS", buff);
805 return true;
807 case OSD_STAT_TOTAL_TIME: {
808 int minutes = statsConfig()->stats_total_time_s / 60;
809 tfp_sprintf(buff, "%d:%02dH", minutes / 60, minutes % 60);
810 osdDisplayStatisticLabel(displayRow, "TOTAL FLIGHT TIME", buff);
811 return true;
814 case OSD_STAT_TOTAL_DIST:
815 #define METERS_PER_KILOMETER 1000
816 #define METERS_PER_MILE 1609
817 if (osdConfig()->units == OSD_UNIT_IMPERIAL) {
818 tfp_sprintf(buff, "%d%c", statsConfig()->stats_total_dist_m / METERS_PER_MILE, SYM_MILES);
819 } else {
820 tfp_sprintf(buff, "%d%c", statsConfig()->stats_total_dist_m / METERS_PER_KILOMETER, SYM_KM);
822 osdDisplayStatisticLabel(displayRow, "TOTAL DISTANCE", buff);
823 return true;
824 #endif
826 return false;
829 static uint8_t osdShowStats(int statsRowCount)
831 uint8_t top = 0;
832 bool displayLabel = false;
834 // if statsRowCount is 0 then we're running an initial analysis of the active stats items
835 if (statsRowCount > 0) {
836 const int availableRows = osdDisplayPort->rows;
837 int displayRows = MIN(statsRowCount, availableRows);
838 if (statsRowCount < availableRows) {
839 displayLabel = true;
840 displayRows++;
842 top = (availableRows - displayRows) / 2; // center the stats vertically
845 if (displayLabel) {
846 displayWrite(osdDisplayPort, 2, top++, DISPLAYPORT_ATTR_NONE, " --- STATS ---");
849 for (int i = 0; i < OSD_STAT_COUNT; i++) {
850 if (osdStatGetState(osdStatsDisplayOrder[i])) {
851 if (osdDisplayStat(osdStatsDisplayOrder[i], top)) {
852 top++;
856 return top;
859 static void osdRefreshStats(void)
861 displayClearScreen(osdDisplayPort);
862 if (osdStatsRowCount == 0) {
863 // No stats row count has been set yet.
864 // Go through the logic one time to determine how many stats are actually displayed.
865 osdStatsRowCount = osdShowStats(0);
866 // Then clear the screen and commence with normal stats display which will
867 // determine if the heading should be displayed and also center the content vertically.
868 displayClearScreen(osdDisplayPort);
870 osdShowStats(osdStatsRowCount);
873 static timeDelta_t osdShowArmed(void)
875 timeDelta_t ret;
877 displayClearScreen(osdDisplayPort);
879 if ((osdConfig()->logo_on_arming == OSD_LOGO_ARMING_ON) || ((osdConfig()->logo_on_arming == OSD_LOGO_ARMING_FIRST) && !ARMING_FLAG(WAS_EVER_ARMED))) {
880 osdDrawLogo(3, 1);
881 ret = osdConfig()->logo_on_arming_duration * 1e5;
882 } else {
883 ret = (REFRESH_1S / 2);
885 displayWrite(osdDisplayPort, 12, 7, DISPLAYPORT_ATTR_NONE, "ARMED");
887 return ret;
890 STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
892 static timeUs_t lastTimeUs = 0;
893 static bool osdStatsEnabled = false;
894 static bool osdStatsVisible = false;
895 static timeUs_t osdStatsRefreshTimeUs;
897 if (!osdIsReady) {
898 if (!displayIsReady(osdDisplayPort)) {
899 displayResync(osdDisplayPort);
900 return;
902 osdCompleteInitialization();
905 // detect arm/disarm
906 if (armState != ARMING_FLAG(ARMED)) {
907 if (ARMING_FLAG(ARMED)) {
908 osdStatsEnabled = false;
909 osdStatsVisible = false;
910 osdResetStats();
911 resumeRefreshAt = osdShowArmed() + currentTimeUs;
912 } else if (isSomeStatEnabled()
913 && !suppressStatsDisplay
914 && (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))
915 || !VISIBLE(osdElementConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
916 osdStatsEnabled = true;
917 resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
918 stats.end_voltage = getBatteryVoltage();
919 osdStatsRowCount = 0; // reset to 0 so it will be recalculated on the next stats refresh
922 armState = ARMING_FLAG(ARMED);
926 if (ARMING_FLAG(ARMED)) {
927 osdUpdateStats();
928 timeUs_t deltaT = currentTimeUs - lastTimeUs;
929 osdFlyTime += deltaT;
930 stats.armed_time += deltaT;
931 } else if (osdStatsEnabled) { // handle showing/hiding stats based on OSD disable switch position
932 if (displayIsGrabbed(osdDisplayPort)) {
933 osdStatsEnabled = false;
934 resumeRefreshAt = 0;
935 stats.armed_time = 0;
936 } else {
937 if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
938 osdStatsVisible = false;
939 displayClearScreen(osdDisplayPort);
940 } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
941 if (!osdStatsVisible) {
942 osdStatsVisible = true;
943 osdStatsRefreshTimeUs = 0;
945 if (currentTimeUs >= osdStatsRefreshTimeUs) {
946 osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
947 osdRefreshStats();
952 lastTimeUs = currentTimeUs;
954 displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
956 if (resumeRefreshAt) {
957 if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
958 // in timeout period, check sticks for activity to resume display.
959 if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
960 resumeRefreshAt = currentTimeUs;
962 displayHeartbeat(osdDisplayPort);
963 return;
964 } else {
965 displayClearScreen(osdDisplayPort);
966 resumeRefreshAt = 0;
967 osdStatsEnabled = false;
968 stats.armed_time = 0;
972 #ifdef USE_ESC_SENSOR
973 if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
974 osdEscDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
976 #endif
978 #if defined(USE_ACC)
979 if (sensors(SENSOR_ACC)
980 && (VISIBLE(osdElementConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
981 // only calculate the G force if the element is visible or the stat is enabled
982 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
983 const float a = accAverage[axis];
984 osdGForce += a * a;
986 osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
988 #endif
990 #ifdef USE_CMS
991 if (!displayIsGrabbed(osdDisplayPort))
992 #endif
994 osdUpdateAlarms();
995 osdDrawElements(currentTimeUs);
996 displayHeartbeat(osdDisplayPort);
998 displayCommitTransaction(osdDisplayPort);
1002 * Called periodically by the scheduler
1004 void osdUpdate(timeUs_t currentTimeUs)
1006 static uint32_t counter = 0;
1008 if (isBeeperOn()) {
1009 showVisualBeeper = true;
1012 #ifdef MAX7456_DMA_CHANNEL_TX
1013 // don't touch buffers if DMA transaction is in progress
1014 if (displayIsTransferInProgress(osdDisplayPort)) {
1015 return;
1017 #endif // MAX7456_DMA_CHANNEL_TX
1019 #ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
1020 static uint32_t idlecounter = 0;
1021 if (!ARMING_FLAG(ARMED)) {
1022 if (idlecounter++ % 4 != 0) {
1023 return;
1026 #endif
1028 // redraw values in buffer
1029 #ifdef USE_MAX7456
1030 #define DRAW_FREQ_DENOM 5
1031 #else
1032 #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud (
1033 #endif
1035 if (counter % DRAW_FREQ_DENOM == 0) {
1036 osdRefresh(currentTimeUs);
1037 showVisualBeeper = false;
1038 } else {
1039 bool doDrawScreen = true;
1040 #if defined(USE_CMS) && defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT)
1041 // For the MSP displayPort device only do the drawScreen once per
1042 // logical OSD cycle as there is no output buffering needing to be flushed.
1043 if (osdDisplayPortDeviceType == OSD_DISPLAYPORT_DEVICE_MSP) {
1044 doDrawScreen = (counter % DRAW_FREQ_DENOM == 1);
1046 #endif
1047 // Redraw a portion of the chars per idle to spread out the load and SPI bus utilization
1048 if (doDrawScreen) {
1049 displayDrawScreen(osdDisplayPort);
1052 ++counter;
1055 void osdSuppressStats(bool flag)
1057 suppressStatsDisplay = flag;
1060 #ifdef USE_OSD_PROFILES
1061 bool osdElementVisible(uint16_t value)
1063 return (bool)((((value & OSD_PROFILE_MASK) >> OSD_PROFILE_BITS_POS) & osdProfile) != 0);
1065 #endif
1067 bool osdGetVisualBeeperState(void)
1069 return showVisualBeeper;
1072 statistic_t *osdGetStats(void)
1074 return &stats;
1077 #ifdef USE_ACC
1078 // Determine if there are any enabled stats that need
1079 // the ACC (currently only MAX_G_FORCE).
1080 static bool osdStatsNeedAccelerometer(void)
1082 return osdStatGetState(OSD_STAT_MAX_G_FORCE);
1085 // Check if any enabled elements or stats need the ACC
1086 bool osdNeedsAccelerometer(void)
1088 return osdStatsNeedAccelerometer() || osdElementsNeedAccelerometer();
1090 #endif // USE_ACC
1092 displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *displayPortDeviceType)
1094 if (displayPortDeviceType) {
1095 *displayPortDeviceType = osdDisplayPortDeviceType;
1097 return osdDisplayPort;
1100 #endif // USE_OSD