diff --git a/make/source.mk b/make/source.mk
index dd974bab92..b284cc495f 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -149,8 +149,9 @@ COMMON_SRC = \
io/rcdevice.c \
io/gps.c \
io/ledstrip.c \
- io/osd.c \
io/pidaudio.c \
+ osd/osd.c \
+ osd/osd_elements.c \
sensors/barometer.c \
sensors/rangefinder.c \
telemetry/telemetry.c \
@@ -289,7 +290,6 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
config/config_streamer.c \
i2c_bst.c \
io/dashboard.c \
- io/osd.c \
io/serial.c \
io/serial_4way.c \
io/serial_4way_avrootloader.c \
@@ -317,6 +317,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
io/vtx_tramp.c \
io/vtx_control.c \
io/spektrum_vtx_control.c \
+ osd/osd.c \
+ osd/osd_elements.c \
pg/pg.h
# F4 and F7 optimizations
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
index 9cddded2f2..2454ea9714 100644
--- a/src/main/cli/cli.c
+++ b/src/main/cli/cli.c
@@ -111,7 +111,6 @@ extern uint8_t __config_end;
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
-#include "io/osd.h"
#include "io/serial.h"
#include "io/transponder_ir.h"
#include "io/usb_msc.h"
@@ -122,6 +121,8 @@ extern uint8_t __config_end;
#include "msp/msp_box.h"
#include "msp/msp_protocol.h"
+#include "osd/osd.h"
+
#include "pg/adc.h"
#include "pg/beeper.h"
#include "pg/beeper_dev.h"
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 8b0413d2f0..e225bad9b2 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -59,11 +59,12 @@
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
-#include "io/osd.h"
#include "io/vtx.h"
#include "io/vtx_control.h"
#include "io/vtx_rtc6705.h"
+#include "osd/osd.h"
+
#include "pg/adc.h"
#include "pg/beeper.h"
#include "pg/beeper_dev.h"
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 54c8a6fce7..be7a75b81e 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -65,9 +65,10 @@
#include "flight/mixer.h"
// For VISIBLE*
-#include "io/osd.h"
#include "io/rcdevice_cam.h"
+#include "osd/osd.h"
+
#include "rx/rx.h"
#ifdef USE_USB_CDC_HID
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index 6de905f0ac..9748a41996 100644
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -40,7 +40,9 @@
#include "pg/pg_ids.h"
#include "io/displayport_max7456.h"
-#include "io/osd.h"
+
+#include "osd/osd.h"
+#include "osd/osd_elements.h"
#ifdef USE_EXTENDED_CMS_MENUS
static uint16_t osdConfig_item_pos[OSD_ITEM_COUNT];
@@ -56,6 +58,7 @@ static long menuOsdActiveElemsOnExit(const OSD_Entry *self)
UNUSED(self);
memcpy(&osdConfigMutable()->item_pos[0], &osdConfig_item_pos[0], sizeof(uint16_t) * OSD_ITEM_COUNT);
+ osdAnalyzeActiveElements();
return 0;
}
@@ -274,8 +277,7 @@ static long cmsx_menuOsdOnExit(const OSD_Entry *self)
UNUSED(self);
#ifdef USE_OSD_PROFILES
- osdConfigMutable()->osdProfileIndex = osdConfig_osdProfileIndex;
- setOsdProfile(osdConfig()->osdProfileIndex);
+ changeOsdProfileIndex(osdConfig_osdProfileIndex);
#endif
#ifdef USE_MAX7456
@@ -284,7 +286,7 @@ static long cmsx_menuOsdOnExit(const OSD_Entry *self)
displayPortProfileMax7456Mutable()->whiteBrightness = displayPortProfileMax7456_whiteBrightness;
#endif
- return 0;
+ return 0;
}
const OSD_Entry cmsx_menuOsdEntries[] =
diff --git a/src/main/drivers/camera_control.c b/src/main/drivers/camera_control.c
index 589e69828f..029216c09c 100644
--- a/src/main/drivers/camera_control.c
+++ b/src/main/drivers/camera_control.c
@@ -56,7 +56,7 @@
#include "timer.h"
#ifdef USE_OSD
-#include "io/osd.h"
+#include "osd/osd.h"
#endif
PG_REGISTER_WITH_RESET_FN(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0);
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 08d9de16aa..add639d013 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -69,7 +69,6 @@
#include "io/beeper.h"
#include "io/gps.h"
#include "io/motors.h"
-#include "io/osd.h"
#include "io/pidaudio.h"
#include "io/servos.h"
#include "io/serial.h"
@@ -78,6 +77,8 @@
#include "io/vtx_control.h"
#include "io/vtx_rtc6705.h"
+#include "osd/osd.h"
+
#include "rx/rx.h"
#include "scheduler/scheduler.h"
diff --git a/src/main/fc/init.c b/src/main/fc/init.c
index f3971b2239..b613b78a39 100644
--- a/src/main/fc/init.c
+++ b/src/main/fc/init.c
@@ -131,7 +131,6 @@
#include "io/dashboard.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/transponder_ir.h"
-#include "io/osd.h"
#include "io/pidaudio.h"
#include "io/piniobox.h"
#include "io/displayport_msp.h"
@@ -141,6 +140,8 @@
#include "io/vtx_smartaudio.h"
#include "io/vtx_tramp.h"
+#include "osd/osd.h"
+
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index 41f7b048c5..1a16d55cd2 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@@ -39,18 +39,19 @@
#include "drivers/time.h"
+#include "fc/config.h"
+#include "fc/controlrate_profile.h"
+#include "fc/rc_controls.h"
+#include "fc/rc.h"
+
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "io/pidaudio.h"
-#include "io/osd.h"
-#include "fc/config.h"
-#include "fc/controlrate_profile.h"
-#include "fc/rc_controls.h"
-#include "fc/rc.h"
+#include "osd/osd.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index c69752e229..a4eeafbeef 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -63,7 +63,6 @@
#include "io/dashboard.h"
#include "io/gps.h"
#include "io/ledstrip.h"
-#include "io/osd.h"
#include "io/piniobox.h"
#include "io/serial.h"
#include "io/transponder_ir.h"
@@ -75,6 +74,8 @@
#include "msp/msp.h"
#include "msp/msp_serial.h"
+#include "osd/osd.h"
+
#include "pg/rx.h"
#include "rx/rx.h"
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index e076a7c82d..59e26ceaf5 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -33,7 +33,8 @@
#include "fc/config.h"
#include "io/displayport_max7456.h"
-#include "io/osd.h"
+
+#include "osd/osd.h"
#include "pg/max7456.h"
#include "pg/pg.h"
@@ -60,7 +61,6 @@ static int grab(displayPort_t *displayPort)
// FIXME this should probably not have a dependency on the OSD or OSD slave code
UNUSED(displayPort);
#ifdef USE_OSD
- osdResetAlarms();
resumeRefreshAt = 0;
#endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
deleted file mode 100644
index e3c6fedf8b..0000000000
--- a/src/main/io/osd.c
+++ /dev/null
@@ -1,2084 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
- *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see .
- */
-
-/*
- Created by Marcin Baliniak
- some functions based on MinimOSD
-
- OSD-CMS separation by jflyper
- */
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "platform.h"
-
-#ifdef USE_OSD
-
-#include "blackbox/blackbox.h"
-#include "blackbox/blackbox_io.h"
-
-#include "build/build_config.h"
-#include "build/debug.h"
-#include "build/version.h"
-
-#include "cms/cms.h"
-#include "cms/cms_types.h"
-
-#include "common/axis.h"
-#include "common/maths.h"
-#include "common/printf.h"
-#include "common/typeconversion.h"
-#include "common/utils.h"
-
-#include "config/feature.h"
-
-#include "drivers/display.h"
-#include "drivers/flash.h"
-#include "drivers/max7456_symbols.h"
-#include "drivers/sdcard.h"
-#include "drivers/time.h"
-
-#include "fc/config.h"
-#include "fc/core.h"
-#include "fc/rc_adjustments.h"
-#include "fc/rc_controls.h"
-#include "fc/rc_modes.h"
-#include "fc/rc.h"
-#include "fc/runtime_config.h"
-
-#include "flight/gps_rescue.h"
-#include "flight/failsafe.h"
-#include "flight/position.h"
-#include "flight/imu.h"
-#include "flight/mixer.h"
-#include "flight/pid.h"
-
-#include "io/asyncfatfs/asyncfatfs.h"
-#include "io/beeper.h"
-#include "io/flashfs.h"
-#include "io/gps.h"
-#include "io/osd.h"
-#include "io/vtx_string.h"
-#include "io/vtx.h"
-
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-#include "pg/rx.h"
-
-#include "rx/rx.h"
-
-#include "sensors/acceleration.h"
-#include "sensors/adcinternal.h"
-#include "sensors/barometer.h"
-#include "sensors/battery.h"
-#include "sensors/esc_sensor.h"
-#include "sensors/sensors.h"
-#if defined(USE_GYRO_DATA_ANALYSE)
-#include "sensors/gyroanalyse.h"
-#endif
-
-#ifdef USE_HARDWARE_REVISION_DETECTION
-#include "hardware_revision.h"
-#endif
-
-#define VIDEO_BUFFER_CHARS_PAL 480
-#define FULL_CIRCLE 360
-
-// Stick overlay size
-#define OSD_STICK_OVERLAY_WIDTH 7
-#define OSD_STICK_OVERLAY_HEIGHT 5
-#define OSD_STICK_OVERLAY_SPRITE_HEIGHT 3
-#define OSD_STICK_OVERLAY_VERTICAL_POSITIONS (OSD_STICK_OVERLAY_HEIGHT * OSD_STICK_OVERLAY_SPRITE_HEIGHT)
-
-const char * const osdTimerSourceNames[] = {
- "ON TIME ",
- "TOTAL ARM",
- "LAST ARM "
-};
-
-// Blink control
-
-static bool blinkState = true;
-static bool showVisualBeeper = false;
-
-static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
-#define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
-#define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
-#define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
-#define BLINK(item) (IS_BLINK(item) && blinkState)
-
-// Things in both OSD and CMS
-
-#define IS_HI(X) (rcData[X] > 1750)
-#define IS_LO(X) (rcData[X] < 1250)
-#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
-
-static timeUs_t flyTime = 0;
-#if defined(USE_ACC)
-static float osdGForce = 0;
-#endif
-
-typedef struct statistic_s {
- timeUs_t armed_time;
- int16_t max_speed;
- int16_t min_voltage; // /100
- int16_t max_current; // /10
- uint8_t min_rssi;
- int32_t max_altitude;
- int16_t max_distance;
- float max_g_force;
- int16_t max_esc_temp;
- int32_t max_esc_rpm;
- uint8_t min_link_quality;
-} statistic_t;
-
-#ifdef USE_OSD_STICK_OVERLAY
-typedef struct radioControls_s {
- uint8_t left_vertical;
- uint8_t left_horizontal;
- uint8_t right_vertical;
- uint8_t right_horizontal;
-} radioControls_t;
-
-static const radioControls_t radioModes[4] = {
- { PITCH, YAW, THROTTLE, ROLL }, // Mode 1
- { THROTTLE, YAW, PITCH, ROLL }, // Mode 2
- { PITCH, ROLL, THROTTLE, YAW }, // Mode 3
- { THROTTLE, ROLL, PITCH, YAW }, // Mode 4
-};
-#endif
-
-static statistic_t stats;
-timeUs_t resumeRefreshAt = 0;
-#define REFRESH_1S 1000 * 1000
-
-static uint8_t armState;
-static bool lastArmState;
-#ifdef USE_OSD_PROFILES
-static uint8_t osdProfile = 1;
-#endif
-static displayPort_t *osdDisplayPort;
-
-static bool suppressStatsDisplay = false;
-
-#ifdef USE_ESC_SENSOR
-static escSensorData_t *escDataCombined;
-#endif
-
-#define AH_SYMBOL_COUNT 9
-#define AH_SIDEBAR_WIDTH_POS 7
-#define AH_SIDEBAR_HEIGHT_POS 3
-
-static const char compassBar[] = {
- SYM_HEADING_W,
- SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
- SYM_HEADING_N,
- SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
- SYM_HEADING_E,
- SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
- SYM_HEADING_S,
- SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
- SYM_HEADING_W,
- SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
- SYM_HEADING_N,
- SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE
-};
-
-static const uint8_t osdElementDisplayOrder[] = {
- OSD_MAIN_BATT_VOLTAGE,
- OSD_RSSI_VALUE,
- OSD_CROSSHAIRS,
- OSD_HORIZON_SIDEBARS,
- OSD_ITEM_TIMER_1,
- OSD_ITEM_TIMER_2,
- OSD_REMAINING_TIME_ESTIMATE,
- OSD_FLYMODE,
- OSD_THROTTLE_POS,
- OSD_VTX_CHANNEL,
- OSD_CURRENT_DRAW,
- OSD_MAH_DRAWN,
- OSD_CRAFT_NAME,
- OSD_ALTITUDE,
- OSD_ROLL_PIDS,
- OSD_PITCH_PIDS,
- OSD_YAW_PIDS,
- OSD_POWER,
- OSD_PIDRATE_PROFILE,
- OSD_WARNINGS,
- OSD_AVG_CELL_VOLTAGE,
- OSD_DEBUG,
- OSD_PITCH_ANGLE,
- OSD_ROLL_ANGLE,
- OSD_MAIN_BATT_USAGE,
- OSD_DISARMED,
- OSD_NUMERICAL_HEADING,
- OSD_NUMERICAL_VARIO,
- OSD_COMPASS_BAR,
- OSD_ANTI_GRAVITY,
- OSD_MOTOR_DIAG,
- OSD_FLIP_ARROW,
- OSD_DISPLAY_NAME,
-#ifdef USE_RTC_TIME
- OSD_RTC_DATETIME,
-#endif
-#ifdef USE_OSD_ADJUSTMENTS
- OSD_ADJUSTMENT_RANGE,
-#endif
-#ifdef USE_ADC_INTERNAL
- OSD_CORE_TEMPERATURE,
-#endif
-#ifdef USE_RX_LINK_QUALITY_INFO
- OSD_LINK_QUALITY,
-#endif
-};
-
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 5);
-
-/**
- * Gets the correct altitude symbol for the current unit system
- */
-static char osdGetMetersToSelectedUnitSymbol(void)
-{
- switch (osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- return SYM_FT;
- default:
- return SYM_M;
- }
-}
-
-static char osdGetBatterySymbol(int cellVoltage)
-{
- if (getBatteryState() == BATTERY_CRITICAL) {
- return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
- } else {
- // Calculate a symbol offset using cell voltage over full cell voltage range
- const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatmaxcellvoltage, 0, 8);
- return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
- }
-}
-
-/**
- * Converts altitude based on the current unit system.
- * @param meters Value in meters to convert
- */
-static int32_t osdGetMetersToSelectedUnit(int32_t meters)
-{
- switch (osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- return (meters * 328) / 100; // Convert to feet / 100
- default:
- return meters; // Already in metre / 100
- }
-}
-
-#if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
-STATIC_UNIT_TESTED int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
-{
- switch (osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- return lrintf(((tempInDegreesCelcius * 9.0f) / 5) + 32);
- default:
- return tempInDegreesCelcius;
- }
-}
-
-static char osdGetTemperatureSymbolForSelectedUnit(void)
-{
- switch (osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- return SYM_F;
- default:
- return SYM_C;
- }
-}
-#endif
-
-static void osdFormatAltitudeString(char * buff, int32_t altitudeCm)
-{
- const int alt = osdGetMetersToSelectedUnit(altitudeCm) / 10;
-
- tfp_sprintf(buff, "%5d %c", alt, osdGetMetersToSelectedUnitSymbol());
- buff[5] = buff[4];
- buff[4] = '.';
-}
-
-static void osdFormatPID(char * buff, const char * label, const pidf_t * pid)
-{
- tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
-}
-
-static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
-{
- heading += FULL_CIRCLE; // Ensure positive value
-
- // Split input heading 0..359 into sectors 0..(directions-1), but offset
- // by half a sector so that sector 0 gets centered around heading 0.
- // We multiply heading by directions to not loose precision in divisions
- // In this way each segment will be a FULL_CIRCLE length
- int direction = (heading * directions + FULL_CIRCLE / 2) / FULL_CIRCLE; // scale with rounding
- direction %= directions; // normalize
-
- return direction; // return segment number
-}
-
-static uint8_t osdGetDirectionSymbolFromHeading(int heading)
-{
- heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
-
- // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
- // Our symbols are Down=0, Right=4, Up=8 and Left=12
- // There're 16 arrow symbols. Transform it.
- heading = 16 - heading;
- heading = (heading + 8) % 16;
-
- return SYM_ARROW_SOUTH + heading;
-}
-
-static char osdGetTimerSymbol(osd_timer_source_e src)
-{
- switch (src) {
- case OSD_TIMER_SRC_ON:
- return SYM_ON_M;
- case OSD_TIMER_SRC_TOTAL_ARMED:
- case OSD_TIMER_SRC_LAST_ARMED:
- return SYM_FLY_M;
- default:
- return ' ';
- }
-}
-
-static timeUs_t osdGetTimerValue(osd_timer_source_e src)
-{
- switch (src) {
- case OSD_TIMER_SRC_ON:
- return micros();
- case OSD_TIMER_SRC_TOTAL_ARMED:
- return flyTime;
- case OSD_TIMER_SRC_LAST_ARMED:
- return stats.armed_time;
- default:
- return 0;
- }
-}
-
-STATIC_UNIT_TESTED void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
-{
- int seconds = time / 1000000;
- const int minutes = seconds / 60;
- seconds = seconds % 60;
-
- switch (precision) {
- case OSD_TIMER_PREC_SECOND:
- default:
- tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
- break;
- case OSD_TIMER_PREC_HUNDREDTHS:
- {
- const int hundredths = (time / 10000) % 100;
- tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
- break;
- }
- }
-}
-
-STATIC_UNIT_TESTED void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
-{
- const uint16_t timer = osdConfig()->timers[timerIndex];
- const uint8_t src = OSD_TIMER_SRC(timer);
-
- if (showSymbol) {
- *(buff++) = osdGetTimerSymbol(src);
- }
-
- osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
-}
-
-#ifdef USE_GPS
-static void osdFormatCoordinate(char *buff, char sym, int32_t val)
-{
- // latitude maximum integer width is 3 (-90).
- // longitude maximum integer width is 4 (-180).
- // We show 7 decimals, so we need to use 12 characters:
- // eg: s-180.1234567z s=symbol, z=zero terminator, decimal separator between 0 and 1
-
- static const int coordinateMaxLength = 13;//12 for the number (4 + dot + 7) + 1 for the symbol
-
- buff[0] = sym;
- const int32_t integerPart = val / GPS_DEGREES_DIVIDER;
- const int32_t decimalPart = labs(val % GPS_DEGREES_DIVIDER);
- const int written = tfp_sprintf(buff + 1, "%d.%07d", integerPart, decimalPart);
- // pad with blanks to coordinateMaxLength
- for (int pos = 1 + written; pos < coordinateMaxLength; ++pos) {
- buff[pos] = SYM_BLANK;
- }
- buff[coordinateMaxLength] = '\0';
-}
-#endif // USE_GPS
-
-static void osdFormatMessage(char *buff, size_t size, const char *message)
-{
- memset(buff, SYM_BLANK, size);
- if (message) {
- memcpy(buff, message, strlen(message));
- }
- // Ensure buff is zero terminated
- buff[size - 1] = '\0';
-}
-
-#ifdef USE_RTC_TIME
-static bool osdFormatRtcDateTime(char *buffer)
-{
- dateTime_t dateTime;
- if (!rtcGetDateTime(&dateTime)) {
- buffer[0] = '\0';
-
- return false;
- }
-
- dateTimeFormatLocalShort(buffer, &dateTime);
-
- return true;
-}
-#endif
-
-void osdStatSetState(uint8_t statIndex, bool enabled)
-{
- if (enabled) {
- osdConfigMutable()->enabled_stats |= (1 << statIndex);
- } else {
- osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
- }
-}
-
-bool osdStatGetState(uint8_t statIndex)
-{
- return osdConfig()->enabled_stats & (1 << statIndex);
-}
-
-void osdWarnSetState(uint8_t warningIndex, bool enabled)
-{
- if (enabled) {
- osdConfigMutable()->enabledWarnings |= (1 << warningIndex);
- } else {
- osdConfigMutable()->enabledWarnings &= ~(1 << warningIndex);
- }
-}
-
-bool osdWarnGetState(uint8_t warningIndex)
-{
- return osdConfig()->enabledWarnings & (1 << warningIndex);
-}
-
-#ifdef USE_OSD_PROFILES
-void setOsdProfile(uint8_t value)
-{
- // 1 ->> 001
- // 2 ->> 010
- // 3 ->> 100
- if (value <= OSD_PROFILE_COUNT) {
- if (value == 0) {
- osdProfile = 1;
- } else {
- osdProfile = 1 << (value - 1);
- }
- }
- }
-
-uint8_t getCurrentOsdProfileIndex(void)
-{
- return osdConfig()->osdProfileIndex;
-}
-
-void changeOsdProfileIndex(uint8_t profileIndex)
-{
- if (profileIndex <= OSD_PROFILE_COUNT) {
- osdConfigMutable()->osdProfileIndex = profileIndex;
- setOsdProfile(profileIndex);
- }
-}
-#endif
-
-static bool osdDrawSingleElement(uint8_t item)
-{
- if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
- return false;
- }
-
- uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
- uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
- char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
-
- switch (item) {
-
-#if defined(USE_ACC)
- case OSD_FLIP_ARROW:
- {
- int rollAngle = attitude.values.roll / 10;
- const int pitchAngle = attitude.values.pitch / 10;
- if (abs(rollAngle) > 90) {
- rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
- }
-
- if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) {
- if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
- if (pitchAngle > 0) {
- if (rollAngle > 0) {
- buff[0] = SYM_ARROW_WEST + 2;
- } else {
- buff[0] = SYM_ARROW_EAST - 2;
- }
- } else {
- if (rollAngle > 0) {
- buff[0] = SYM_ARROW_WEST - 2;
- } else {
- buff[0] = SYM_ARROW_EAST + 2;
- }
- }
- } else {
- if (abs(pitchAngle) > abs(rollAngle)) {
- if (pitchAngle > 0) {
- buff[0] = SYM_ARROW_SOUTH;
- } else {
- buff[0] = SYM_ARROW_NORTH;
- }
- } else {
- if (rollAngle > 0) {
- buff[0] = SYM_ARROW_WEST;
- } else {
- buff[0] = SYM_ARROW_EAST;
- }
- }
- }
- } else {
- buff[0] = ' ';
- }
-
- buff[1] = '\0';
- break;
- }
-#endif
-
- case OSD_RSSI_VALUE:
- {
- uint16_t osdRssi = getRssi() * 100 / 1024; // change range
- if (osdRssi >= 100) {
- osdRssi = 99;
- }
-
- tfp_sprintf(buff, "%c%2d", SYM_RSSI, osdRssi);
- break;
- }
-
-#ifdef USE_RX_LINK_QUALITY_INFO
- case OSD_LINK_QUALITY:
- {
- // change range to 0-9 (two sig. fig. adds little extra value, also reduces screen estate)
- uint8_t osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
- if (osdLinkQuality >= 10) {
- osdLinkQuality = 9;
- }
-
- tfp_sprintf(buff, "%1d", osdLinkQuality);
- break;
- }
-#endif
-
- case OSD_MAIN_BATT_VOLTAGE:
- buff[0] = osdGetBatterySymbol(getBatteryAverageCellVoltage());
- tfp_sprintf(buff + 1, "%2d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
- break;
-
- case OSD_CURRENT_DRAW:
- {
- const int32_t amperage = getAmperage();
- tfp_sprintf(buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
- break;
- }
-
- case OSD_MAH_DRAWN:
- tfp_sprintf(buff, "%4d%c", getMAhDrawn(), SYM_MAH);
- break;
-
-#ifdef USE_GPS
- case OSD_GPS_SATS:
- tfp_sprintf(buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
- break;
-
- case OSD_GPS_SPEED:
- // 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)
- switch (osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- tfp_sprintf(buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed));
- break;
- default:
- tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
- break;
- }
- break;
-
- case OSD_GPS_LAT:
- // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH
- osdFormatCoordinate(buff, SYM_ARROW_NORTH, gpsSol.llh.lat);
- break;
-
- case OSD_GPS_LON:
- // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST
- osdFormatCoordinate(buff, SYM_ARROW_EAST, gpsSol.llh.lon);
- break;
-
- case OSD_HOME_DIR:
- if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
- if (GPS_distanceToHome > 0) {
- const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
- buff[0] = osdGetDirectionSymbolFromHeading(h);
- } else {
- // We don't have a HOME symbol in the font, by now we use this
- buff[0] = SYM_THR1;
- }
-
- } else {
- // We use this symbol when we don't have a FIX
- buff[0] = SYM_COLON;
- }
-
- buff[1] = 0;
-
- break;
-
- case OSD_HOME_DIST:
- if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
- const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome);
- tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
- } else {
- // We use this symbol when we don't have a FIX
- buff[0] = SYM_COLON;
- // overwrite any previous distance with blanks
- memset(buff + 1, SYM_BLANK, 6);
- buff[7] = '\0';
- }
- break;
-
- case OSD_FLIGHT_DIST:
- if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
- const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceFlownInCm / 100);
- tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
- } else {
- // We use this symbol when we don't have a FIX
- buff[0] = SYM_COLON;
- // overwrite any previous distance with blanks
- memset(buff + 1, SYM_BLANK, 6);
- buff[7] = '\0';
- }
- break;
-
-#endif // GPS
-
- case OSD_COMPASS_BAR:
- memcpy(buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
- buff[9] = 0;
- break;
-
- case OSD_ALTITUDE:
- {
- bool haveBaro = false;
- bool haveGps = false;
-#ifdef USE_BARO
- haveBaro = sensors(SENSOR_BARO);
-#endif
-#ifdef USE_GPS
- haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
-#endif
- if (haveBaro || haveGps) {
- osdFormatAltitudeString(buff, getEstimatedAltitudeCm());
- } else {
- // We use this symbol when we don't have a valid measure
- buff[0] = SYM_COLON;
- // overwrite any previous altitude with blanks
- memset(buff + 1, SYM_BLANK, 6);
- buff[7] = '\0';
- }
- }
-
- break;
-
- case OSD_ITEM_TIMER_1:
- case OSD_ITEM_TIMER_2:
- osdFormatTimer(buff, true, true, item - OSD_ITEM_TIMER_1);
- break;
-
- case OSD_REMAINING_TIME_ESTIMATE:
- {
- const int mAhDrawn = getMAhDrawn();
-
- if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) { // also handles the mAhDrawn == 0 condition
- tfp_sprintf(buff, "--:--");
- } else if (mAhDrawn > osdConfig()->cap_alarm) {
- tfp_sprintf(buff, "00:00");
- } else {
- const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)flyTime) / mAhDrawn);
- osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time);
- }
- break;
- }
-
- case OSD_FLYMODE:
- {
- // Note that flight mode display has precedence in what to display.
- // 1. FS
- // 2. GPS RESCUE
- // 3. ANGLE, HORIZON, ACRO TRAINER
- // 4. AIR
- // 5. ACRO
-
- if (FLIGHT_MODE(FAILSAFE_MODE)) {
- strcpy(buff, "!FS!");
- } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
- strcpy(buff, "RESC");
- } else if (FLIGHT_MODE(HEADFREE_MODE)) {
- strcpy(buff, "HEAD");
- } else if (FLIGHT_MODE(ANGLE_MODE)) {
- strcpy(buff, "STAB");
- } else if (FLIGHT_MODE(HORIZON_MODE)) {
- strcpy(buff, "HOR ");
- } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
- strcpy(buff, "ATRN");
- } else if (airmodeIsEnabled()) {
- strcpy(buff, "AIR ");
- } else {
- strcpy(buff, "ACRO");
- }
-
- break;
- }
-
- case OSD_ANTI_GRAVITY:
- {
- if (pidOsdAntiGravityActive()) {
- strcpy(buff, "AG");
- }
-
- break;
- }
-
- case OSD_MOTOR_DIAG:
- {
- int i = 0;
- const bool motorsRunning = areMotorsRunning();
- for (; i < getMotorCount(); i++) {
- if (motorsRunning) {
- buff[i] = 0x88 - scaleRange(motor[i], motorOutputLow, motorOutputHigh, 0, 8);
- } else {
- buff[i] = 0x88;
- }
- }
- buff[i] = '\0';
- break;
- }
-
- case OSD_CRAFT_NAME:
- // 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.
- //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered.
-
- if (strlen(pilotConfig()->name) == 0) {
- strcpy(buff, "CRAFT_NAME");
- } else {
- unsigned i;
- for (i = 0; i < MAX_NAME_LENGTH; i++) {
- if (pilotConfig()->name[i]) {
- buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
- } else {
- break;
- }
- }
- buff[i] = '\0';
- }
-
- break;
-
- case OSD_DISPLAY_NAME:
- // This does not strictly support iterative updating if the display name changes at run time. But since the display name is not supposed to be changing this should not matter, and blanking the entire length of the display name string on update will make it impossible to configure elements to be displayed on the right hand side of the display name.
- //TODO: When iterative updating is implemented, change this so the display name is only printed once whenever the OSD 'flight' screen is entered.
-
- if (strlen(pilotConfig()->displayName) == 0) {
- strcpy(buff, "DISPLAY_NAME");
- } else {
- unsigned i;
- for (i = 0; i < MAX_NAME_LENGTH; i++) {
- if (pilotConfig()->displayName[i]) {
- buff[i] = toupper((unsigned char)pilotConfig()->displayName[i]);
- } else {
- break;
- }
- }
- buff[i] = '\0';
- }
-
- break;
-
- case OSD_THROTTLE_POS:
- buff[0] = SYM_THR;
- buff[1] = SYM_THR1;
- tfp_sprintf(buff + 2, "%3d", calculateThrottlePercent());
- break;
-
-#if defined(USE_VTX_COMMON)
- case OSD_VTX_CHANNEL:
- {
- const vtxDevice_t *vtxDevice = vtxCommonDevice();
- const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, vtxSettingsConfig()->band);
- const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, vtxSettingsConfig()->channel);
- uint8_t vtxPower = vtxSettingsConfig()->power;
- if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
- vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
- }
- tfp_sprintf(buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
- break;
- }
-#endif
-
- case OSD_CROSSHAIRS:
- buff[0] = SYM_AH_CENTER_LINE;
- buff[1] = SYM_AH_CENTER;
- buff[2] = SYM_AH_CENTER_LINE_RIGHT;
- buff[3] = 0;
- break;
-
-#if defined(USE_ACC)
- case OSD_ARTIFICIAL_HORIZON:
- {
- // Get pitch and roll limits in tenths of degrees
- const int maxPitch = osdConfig()->ahMaxPitch * 10;
- const int maxRoll = osdConfig()->ahMaxRoll * 10;
- const int ahSign = osdConfig()->ahInvert ? -1 : 1;
- const int rollAngle = constrain(attitude.values.roll * ahSign, -maxRoll, maxRoll);
- int pitchAngle = constrain(attitude.values.pitch * ahSign, -maxPitch, maxPitch);
- // Convert pitchAngle to y compensation value
- // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
- if (maxPitch > 0) {
- pitchAngle = ((pitchAngle * 25) / maxPitch);
- }
- pitchAngle -= 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
-
- for (int x = -4; x <= 4; x++) {
- const int y = ((-rollAngle * x) / 64) - pitchAngle;
- if (y >= 0 && y <= 81) {
- displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
- }
- }
-
- return true;
- }
-
- case OSD_G_FORCE:
- {
- const int gForce = lrintf(osdGForce * 10);
- tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10);
- break;
- }
-#endif
-
- case OSD_HORIZON_SIDEBARS:
- {
- // Draw AH sides
- const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
- const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
- for (int y = -hudheight; y <= hudheight; y++) {
- displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION);
- displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION);
- }
-
- // AH level indicators
- displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
- displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
-
- return true;
- }
-
- case OSD_ROLL_PIDS:
- osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]);
- break;
-
- case OSD_PITCH_PIDS:
- osdFormatPID(buff, "PIT", ¤tPidProfile->pid[PID_PITCH]);
- break;
-
- case OSD_YAW_PIDS:
- osdFormatPID(buff, "YAW", ¤tPidProfile->pid[PID_YAW]);
- break;
-
- case OSD_POWER:
- tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 10000);
- break;
-
- case OSD_PIDRATE_PROFILE:
- tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
- break;
-
- case OSD_WARNINGS:
- {
-
-#define OSD_WARNINGS_MAX_SIZE 11
-#define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
-
- STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= sizeof(buff), osd_warnings_size_exceeds_buffer_size);
-
- const batteryState_e batteryState = getBatteryState();
- const timeUs_t currentTimeUs = micros();
-
- static timeUs_t armingDisabledUpdateTimeUs;
- static unsigned armingDisabledDisplayIndex;
-
- CLR_BLINK(OSD_WARNINGS);
-
- // Cycle through the arming disabled reasons
- if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE)) {
- if (IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
- const armingDisableFlags_e armSwitchOnlyFlag = 1 << (ARMING_DISABLE_FLAGS_COUNT - 1);
- armingDisableFlags_e flags = getArmingDisableFlags();
-
- // Remove the ARMSWITCH flag unless it's the only one
- if ((flags & armSwitchOnlyFlag) && (flags != armSwitchOnlyFlag)) {
- flags -= armSwitchOnlyFlag;
- }
-
- // Rotate to the next arming disabled reason after a 0.5 second time delay
- // or if the current flag is no longer set
- if ((currentTimeUs - armingDisabledUpdateTimeUs > 5e5) || !(flags & (1 << armingDisabledDisplayIndex))) {
- if (armingDisabledUpdateTimeUs == 0) {
- armingDisabledDisplayIndex = ARMING_DISABLE_FLAGS_COUNT - 1;
- }
- armingDisabledUpdateTimeUs = currentTimeUs;
-
- do {
- if (++armingDisabledDisplayIndex >= ARMING_DISABLE_FLAGS_COUNT) {
- armingDisabledDisplayIndex = 0;
- }
- } while (!(flags & (1 << armingDisabledDisplayIndex)));
- }
-
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[armingDisabledDisplayIndex]);
- break;
- } else {
- armingDisabledUpdateTimeUs = 0;
- }
- }
-
-#ifdef USE_DSHOT
- if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
- int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - currentTimeUs) / 1e5;
- if (armingDelayTime < 0) {
- armingDelayTime = 0;
- }
- if (armingDelayTime >= (DSHOT_BEACON_GUARD_DELAY_US / 1e5 - 5)) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " BEACON ON"); // Display this message for the first 0.5 seconds
- } else {
- char armingDelayMessage[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
- tfp_sprintf(armingDelayMessage, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDelayMessage);
- }
- break;
- }
-#endif
- if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "FAIL SAFE");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-
- // Warn when in flip over after crash mode
- if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashActive()) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP");
- break;
- }
-
-#ifdef USE_LAUNCH_CONTROL
- // Warn when in launch control mode
- if (osdWarnGetState(OSD_WARNING_LAUNCH_CONTROL) && isLaunchControlActive()) {
-#if defined(USE_ACC)
- if (sensors(SENSOR_ACC)) {
- char launchControlMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
- const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
- tfp_sprintf(launchControlMsg, "LAUNCH %d", pitchAngle);
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, launchControlMsg);
- } else
-#endif
- {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LAUNCH");
- }
- break;
- }
-#endif
-
- if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-
-#ifdef USE_GPS_RESCUE
- if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
- ARMING_FLAG(ARMED) &&
- gpsRescueIsConfigured() &&
- !gpsRescueIsDisabled() &&
- !gpsRescueIsAvailable()) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-
- if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_DISABLED) &&
- ARMING_FLAG(ARMED) &&
- gpsRescueIsConfigured() &&
- gpsRescueIsDisabled() &&
- !cmpTimeUs(stats.armed_time, OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US)) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RESC OFF!!!");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-
-#endif
-
- // Show warning if in HEADFREE flight mode
- if (FLIGHT_MODE(HEADFREE_MODE)) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "HEADFREE");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-
-#ifdef USE_ADC_INTERNAL
- const int16_t coreTemperature = getCoreTemperatureCelsius();
- if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
- char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
- tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
-
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg);
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-#endif
-
-#ifdef USE_ESC_SENSOR
- // Show warning if we lose motor output, the ESC is overheating or excessive current draw
- if (featureIsEnabled(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
- char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
- unsigned pos = 0;
-
- const char *title = "ESC";
-
- // center justify message
- while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
- escWarningMsg[pos++] = ' ';
- }
-
- strcpy(escWarningMsg + pos, title);
- pos += strlen(title);
-
- unsigned i = 0;
- unsigned escWarningCount = 0;
- while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
- escSensorData_t *escData = getEscSensorData(i);
- const char motorNumber = '1' + i;
- // if everything is OK just display motor number else R, T or C
- char warnFlag = motorNumber;
- if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
- warnFlag = 'R';
- }
- if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
- warnFlag = 'T';
- }
- if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
- warnFlag = 'C';
- }
-
- escWarningMsg[pos++] = warnFlag;
-
- if (warnFlag != motorNumber) {
- escWarningCount++;
- }
-
- i++;
- }
-
- escWarningMsg[pos] = '\0';
-
- if (escWarningCount > 0) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg);
- SET_BLINK(OSD_WARNINGS);
- break;
- }
- }
-#endif
-
- if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-
-#ifdef USE_RC_SMOOTHING_FILTER
- // Show warning if rc smoothing hasn't initialized the filters
- if (osdWarnGetState(OSD_WARNING_RC_SMOOTHING) && ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RCSMOOTHING");
- SET_BLINK(OSD_WARNINGS);
- break;
- }
-#endif
-
- // Show warning if battery is not fresh
- if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK)
- && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL");
- break;
- }
-
- // Visual beeper
- if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && showVisualBeeper) {
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " * * * *");
- break;
- }
-
- osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL);
- break;
- }
-
- case OSD_AVG_CELL_VOLTAGE:
- {
- const int cellV = getBatteryAverageCellVoltage();
- buff[0] = osdGetBatterySymbol(cellV);
- tfp_sprintf(buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
- break;
- }
-
- case OSD_DEBUG:
- tfp_sprintf(buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
- break;
-
-#if defined(USE_ACC)
- case OSD_PITCH_ANGLE:
- case OSD_ROLL_ANGLE:
- {
- const int angle = (item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
- tfp_sprintf(buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
- break;
- }
-#endif
-
- case OSD_MAIN_BATT_USAGE:
- {
- // Set length of indicator bar
- #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
-
- // Calculate constrained value
- const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
-
- // Calculate mAh used progress
- const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
-
- // Create empty battery indicator bar
- buff[0] = SYM_PB_START;
- for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
- buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
- }
- buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
- if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
- buff[1 + mAhUsedProgress] = SYM_PB_END;
- }
- buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
- break;
- }
-
- case OSD_DISARMED:
- if (!ARMING_FLAG(ARMED)) {
- tfp_sprintf(buff, "DISARMED");
- } else {
- if (!lastArmState) { // previously disarmed - blank out the message one time
- tfp_sprintf(buff, " ");
- }
- }
- break;
-
- case OSD_NUMERICAL_HEADING:
- {
- const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
- tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
- break;
- }
-#ifdef USE_VARIO
- case OSD_NUMERICAL_VARIO:
- {
- bool haveBaro = false;
- bool haveGps = false;
-#ifdef USE_BARO
- haveBaro = sensors(SENSOR_BARO);
-#endif
-#ifdef USE_GPS
- haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
-#endif
- if (haveBaro || haveGps) {
- const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
- const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH;
- tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
- } else {
- // We use this symbol when we don't have a valid measure
- buff[0] = SYM_COLON;
- // overwrite any previous vertical speed with blanks
- memset(buff + 1, SYM_BLANK, 6);
- buff[7] = '\0';
- }
- break;
- }
-#endif
-
-#ifdef USE_ESC_SENSOR
- case OSD_ESC_TMP:
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
- }
- break;
-
- case OSD_ESC_RPM:
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm));
- }
- break;
-#endif
-
-#ifdef USE_RTC_TIME
- case OSD_RTC_DATETIME:
- osdFormatRtcDateTime(&buff[0]);
- break;
-#endif
-
-#ifdef USE_OSD_ADJUSTMENTS
- case OSD_ADJUSTMENT_RANGE:
- if (getAdjustmentsRangeName()) {
- tfp_sprintf(buff, "%s: %3d", getAdjustmentsRangeName(), getAdjustmentsRangeValue());
- }
- break;
-#endif
-
-#ifdef USE_ADC_INTERNAL
- case OSD_CORE_TEMPERATURE:
- tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius()), osdGetTemperatureSymbolForSelectedUnit());
- break;
-#endif
-
-#ifdef USE_BLACKBOX
- case OSD_LOG_STATUS:
- if (!isBlackboxDeviceWorking()) {
- tfp_sprintf(buff, "L-");
- } else if (isBlackboxDeviceFull()) {
- tfp_sprintf(buff, "L>");
- } else {
- tfp_sprintf(buff, "L%d", blackboxGetLogNumber());
- }
- break;
-#endif
-
- default:
- return false;
- }
-
- displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
-
- return true;
-}
-
-#ifdef USE_OSD_STICK_OVERLAY
-static void osdDrawStickOverlayAxis(uint8_t xpos, uint8_t ypos)
-{
-
- for (unsigned x = 0; x < OSD_STICK_OVERLAY_WIDTH; x++) {
- for (unsigned y = 0; y < OSD_STICK_OVERLAY_HEIGHT; y++) {
- // draw the axes, vertical and horizonal
- if ((x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) && (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
- displayWriteChar(osdDisplayPort, xpos + x, ypos + y, SYM_STICK_OVERLAY_CENTER);
- } else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
- displayWriteChar(osdDisplayPort, xpos + x, ypos + y, SYM_STICK_OVERLAY_VERTICAL);
- } else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
- displayWriteChar(osdDisplayPort, xpos + x, ypos + y, SYM_STICK_OVERLAY_HORIZONTAL);
- }
- }
- }
-}
-
-static void osdDrawStickOverlayAxisItem(osd_items_e osd_item)
-{
- osdDrawStickOverlayAxis(OSD_X(osdConfig()->item_pos[osd_item]),
- OSD_Y(osdConfig()->item_pos[osd_item]));
-}
-
-static void osdDrawStickOverlayPos(osd_items_e osd_item, uint8_t xpos, uint8_t ypos, char cursor)
-{
-
- const uint8_t elemPosX = OSD_X(osdConfig()->item_pos[osd_item]);
- const uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[osd_item]);
-
- displayWriteChar(osdDisplayPort, elemPosX + xpos, elemPosY + ypos, cursor);
-}
-
-static void osdDrawStickOverlayCursor(osd_items_e osd_item)
-{
- rc_alias_e vertical_channel, horizontal_channel;
-
- if (osd_item == OSD_STICK_OVERLAY_LEFT) {
- vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_vertical;
- horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_horizontal;
- } else {
- vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_vertical;
- horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_horizontal;
- }
-
- const uint8_t x_pos = scaleRange(constrain(rcData[horizontal_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_WIDTH);
- const uint8_t y_pos = OSD_STICK_OVERLAY_VERTICAL_POSITIONS - 1 - scaleRange(constrain(rcData[vertical_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_VERTICAL_POSITIONS);
-
- const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (y_pos % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
-
- osdDrawStickOverlayPos(osd_item, x_pos, y_pos / OSD_STICK_OVERLAY_SPRITE_HEIGHT, cursor);
-}
-#endif
-
-static void osdDrawElements(void)
-{
- displayClearScreen(osdDisplayPort);
-
- // Hide OSD when OSDSW mode is active
- if (IS_RC_MODE_ACTIVE(BOXOSD)) {
- return;
- }
-
-#if defined(USE_ACC)
- osdGForce = 0.0f;
- if (sensors(SENSOR_ACC)) {
- // only calculate the G force if the element is visible or the stat is enabled
- if (VISIBLE(osdConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE)) {
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- const float a = accAverage[axis];
- osdGForce += a * a;
- }
- osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
- }
- osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
- osdDrawSingleElement(OSD_G_FORCE);
- }
-#endif
-
- for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
- osdDrawSingleElement(osdElementDisplayOrder[i]);
- }
-
-#ifdef USE_GPS
- if (sensors(SENSOR_GPS)) {
- osdDrawSingleElement(OSD_GPS_SATS);
- osdDrawSingleElement(OSD_GPS_SPEED);
- osdDrawSingleElement(OSD_GPS_LAT);
- osdDrawSingleElement(OSD_GPS_LON);
- osdDrawSingleElement(OSD_HOME_DIST);
- osdDrawSingleElement(OSD_HOME_DIR);
- osdDrawSingleElement(OSD_FLIGHT_DIST);
- }
-#endif // GPS
-
-#ifdef USE_ESC_SENSOR
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- osdDrawSingleElement(OSD_ESC_TMP);
- osdDrawSingleElement(OSD_ESC_RPM);
- }
-#endif
-
-#ifdef USE_BLACKBOX
- if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
- osdDrawSingleElement(OSD_LOG_STATUS);
- }
-#endif
-
-#ifdef USE_OSD_STICK_OVERLAY
- if (VISIBLE(osdConfig()->item_pos[OSD_STICK_OVERLAY_LEFT])) {
- osdDrawStickOverlayAxisItem(OSD_STICK_OVERLAY_LEFT);
- osdDrawStickOverlayCursor(OSD_STICK_OVERLAY_LEFT);
- }
-
- if (VISIBLE(osdConfig()->item_pos[OSD_STICK_OVERLAY_RIGHT])) {
- osdDrawStickOverlayAxisItem(OSD_STICK_OVERLAY_RIGHT);
- osdDrawStickOverlayCursor(OSD_STICK_OVERLAY_RIGHT);
- }
-#endif
-}
-
-void pgResetFn_osdConfig(osdConfig_t *osdConfig)
-{
- // Position elements near centre of screen and disabled by default
- for (int i = 0; i < OSD_ITEM_COUNT; i++) {
- osdConfig->item_pos[i] = OSD_POS(10, 7);
- }
-
- // Always enable warnings elements by default
- uint16_t profileFlags = 0;
- for (unsigned i = 1; i <= OSD_PROFILE_COUNT; i++) {
- profileFlags |= OSD_PROFILE_FLAG(i);
- }
- osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | profileFlags;
-
- // Default to old fixed positions for these elements
- osdConfig->item_pos[OSD_CROSSHAIRS] = OSD_POS(13, 6);
- osdConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
- osdConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(14, 6);
-
- // Enable the default stats
- osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
- osdStatSetState(OSD_STAT_MAX_SPEED, true);
- osdStatSetState(OSD_STAT_MIN_BATTERY, true);
- osdStatSetState(OSD_STAT_MIN_RSSI, true);
- osdStatSetState(OSD_STAT_MAX_CURRENT, true);
- osdStatSetState(OSD_STAT_USED_MAH, true);
- osdStatSetState(OSD_STAT_BLACKBOX, true);
- osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, true);
- osdStatSetState(OSD_STAT_TIMER_2, true);
-
- osdConfig->units = OSD_UNIT_METRIC;
-
- // Enable all warnings by default
- for (int i=0; i < OSD_WARNING_COUNT; i++) {
- osdWarnSetState(i, true);
- }
-
- osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10);
- osdConfig->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10);
-
- osdConfig->overlay_radio_mode = 2;
-
- osdConfig->rssi_alarm = 20;
- osdConfig->cap_alarm = 2200;
- osdConfig->alt_alarm = 100; // meters or feet depend on configuration
- osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
- osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
- osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
- osdConfig->core_temp_alarm = 70; // a temperature above 70C should produce a warning, lockups have been reported above 80C
-
- osdConfig->ahMaxPitch = 20; // 20 degrees
- osdConfig->ahMaxRoll = 40; // 40 degrees
-
- osdConfig->osdProfileIndex = 1;
- osdConfig->ahInvert = false;
-}
-
-static void osdDrawLogo(int x, int y)
-{
- // display logo and help
- int fontOffset = 160;
- for (int row = 0; row < 4; row++) {
- for (int column = 0; column < 24; column++) {
- if (fontOffset <= SYM_END_OF_FONT)
- displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
- }
- }
-}
-
-void osdInit(displayPort_t *osdDisplayPortToUse)
-{
- if (!osdDisplayPortToUse) {
- return;
- }
-
- STATIC_ASSERT(OSD_POS_MAX == OSD_POS(31,31), OSD_POS_MAX_incorrect);
-
- osdDisplayPort = osdDisplayPortToUse;
-#ifdef USE_CMS
- cmsDisplayPortRegister(osdDisplayPort);
-#endif
-
- armState = ARMING_FLAG(ARMED);
-
- memset(blinkBits, 0, sizeof(blinkBits));
-
- displayClearScreen(osdDisplayPort);
-
- osdDrawLogo(3, 1);
-
- char string_buffer[30];
- tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
- displayWrite(osdDisplayPort, 20, 6, string_buffer);
-#ifdef USE_CMS
- displayWrite(osdDisplayPort, 7, 8, CMS_STARTUP_HELP_TEXT1);
- displayWrite(osdDisplayPort, 11, 9, CMS_STARTUP_HELP_TEXT2);
- displayWrite(osdDisplayPort, 11, 10, CMS_STARTUP_HELP_TEXT3);
-#endif
-
-#ifdef USE_RTC_TIME
- char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
- if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
- displayWrite(osdDisplayPort, 5, 12, dateTimeBuffer);
- }
-#endif
-
- displayResync(osdDisplayPort);
-
- resumeRefreshAt = micros() + (4 * REFRESH_1S);
-#ifdef USE_OSD_PROFILES
- changeOsdProfileIndex(osdConfig()->osdProfileIndex);
-#endif
-}
-
-bool osdInitialized(void)
-{
- return osdDisplayPort;
-}
-
-void osdUpdateAlarms(void)
-{
- // This is overdone?
-
- int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
-
- if (getRssiPercent() < osdConfig()->rssi_alarm) {
- SET_BLINK(OSD_RSSI_VALUE);
- } else {
- CLR_BLINK(OSD_RSSI_VALUE);
- }
-
- if (getBatteryState() == BATTERY_OK) {
- CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
- CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
- } else {
- SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
- SET_BLINK(OSD_AVG_CELL_VOLTAGE);
- }
-
-#ifdef USE_GPS
- if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5)
-#if defined(USE_GPS_RESCUE)
- || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
-#endif
- ) {
- SET_BLINK(OSD_GPS_SATS);
- } else {
- CLR_BLINK(OSD_GPS_SATS);
- }
-#endif //USE_GPS
-
- for (int i = 0; i < OSD_TIMER_COUNT; i++) {
- const uint16_t timer = osdConfig()->timers[i];
- const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
- const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
- if (alarmTime != 0 && time >= alarmTime) {
- SET_BLINK(OSD_ITEM_TIMER_1 + i);
- } else {
- CLR_BLINK(OSD_ITEM_TIMER_1 + i);
- }
- }
-
- if (getMAhDrawn() >= osdConfig()->cap_alarm) {
- SET_BLINK(OSD_MAH_DRAWN);
- SET_BLINK(OSD_MAIN_BATT_USAGE);
- SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
- } else {
- CLR_BLINK(OSD_MAH_DRAWN);
- CLR_BLINK(OSD_MAIN_BATT_USAGE);
- CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
- }
-
- if (alt >= osdConfig()->alt_alarm) {
- SET_BLINK(OSD_ALTITUDE);
- } else {
- CLR_BLINK(OSD_ALTITUDE);
- }
-
-#ifdef USE_ESC_SENSOR
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
- if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
- SET_BLINK(OSD_ESC_TMP);
- } else {
- CLR_BLINK(OSD_ESC_TMP);
- }
- }
-#endif
-}
-
-void osdResetAlarms(void)
-{
- CLR_BLINK(OSD_RSSI_VALUE);
- CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
- CLR_BLINK(OSD_WARNINGS);
- CLR_BLINK(OSD_GPS_SATS);
- CLR_BLINK(OSD_MAH_DRAWN);
- CLR_BLINK(OSD_ALTITUDE);
- CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
- CLR_BLINK(OSD_MAIN_BATT_USAGE);
- CLR_BLINK(OSD_ITEM_TIMER_1);
- CLR_BLINK(OSD_ITEM_TIMER_2);
- CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
- CLR_BLINK(OSD_ESC_TMP);
-}
-
-static void osdResetStats(void)
-{
- stats.max_current = 0;
- stats.max_speed = 0;
- stats.min_voltage = 5000;
- stats.min_rssi = 99; // percent
- stats.max_altitude = 0;
- stats.max_distance = 0;
- stats.armed_time = 0;
- stats.max_g_force = 0;
- stats.max_esc_temp = 0;
- stats.max_esc_rpm = 0;
- stats.min_link_quality = 99; // percent
-}
-
-static void osdUpdateStats(void)
-{
- int16_t value = 0;
-#ifdef USE_GPS
- switch (osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- value = CM_S_TO_MPH(gpsSol.groundSpeed);
- break;
- default:
- value = CM_S_TO_KM_H(gpsSol.groundSpeed);
- break;
- }
-#endif
- if (stats.max_speed < value) {
- stats.max_speed = value;
- }
-
- value = getBatteryVoltage();
- if (stats.min_voltage > value) {
- stats.min_voltage = value;
- }
-
- value = getAmperage() / 100;
- if (stats.max_current < value) {
- stats.max_current = value;
- }
-
- value = getRssiPercent();
- if (stats.min_rssi > value) {
- stats.min_rssi = value;
- }
-
- int32_t altitudeCm = getEstimatedAltitudeCm();
- if (stats.max_altitude < altitudeCm) {
- stats.max_altitude = altitudeCm;
- }
-
-#if defined(USE_ACC)
- if (stats.max_g_force < osdGForce) {
- stats.max_g_force = osdGForce;
- }
-#endif
-
-#ifdef USE_RX_LINK_QUALITY_INFO
- value = rxGetLinkQualityPercent();
- if (stats.min_link_quality > value) {
- stats.min_link_quality = value;
- }
-#endif
-
-#ifdef USE_GPS
- if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
- value = GPS_distanceToHome;
-
- if (stats.max_distance < GPS_distanceToHome) {
- stats.max_distance = GPS_distanceToHome;
- }
- }
-#endif
-#ifdef USE_ESC_SENSOR
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- value = escDataCombined->temperature;
- if (stats.max_esc_temp < value) {
- stats.max_esc_temp = value;
- }
- value = calcEscRpm(escDataCombined->rpm);
- if (stats.max_esc_rpm < value) {
- stats.max_esc_rpm = value;
- }
- }
-#endif
-}
-
-#ifdef USE_BLACKBOX
-
-static void osdGetBlackboxStatusString(char * buff)
-{
- bool storageDeviceIsWorking = isBlackboxDeviceWorking();
- uint32_t storageUsed = 0;
- uint32_t storageTotal = 0;
-
- switch (blackboxConfig()->device) {
-#ifdef USE_SDCARD
- case BLACKBOX_DEVICE_SDCARD:
- if (storageDeviceIsWorking) {
- storageTotal = sdcard_getMetadata()->numBlocks / 2000;
- storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
- }
- break;
-#endif
-
-#ifdef USE_FLASHFS
- case BLACKBOX_DEVICE_FLASH:
- if (storageDeviceIsWorking) {
- const flashGeometry_t *geometry = flashfsGetGeometry();
- storageTotal = geometry->totalSize / 1024;
- storageUsed = flashfsGetOffset() / 1024;
- }
- break;
-#endif
-
- default:
- break;
- }
-
- if (storageDeviceIsWorking) {
- const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
- tfp_sprintf(buff, "%d%%", storageUsedPercent);
- } else {
- tfp_sprintf(buff, "FAULT");
- }
-}
-#endif
-
-static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
-{
- displayWrite(osdDisplayPort, 2, y, text);
- displayWrite(osdDisplayPort, 20, y, ":");
- displayWrite(osdDisplayPort, 22, y, value);
-}
-
-/*
- * Test if there's some stat enabled
- */
-static bool isSomeStatEnabled(void)
-{
- return (osdConfig()->enabled_stats != 0);
-}
-
-// *** IMPORTANT ***
-// The stats display order was previously required to match the enumeration definition so it matched
-// the order shown in the configurator. However, to allow reordering this screen without breaking the
-// compatibility, this requirement has been relaxed to a best effort approach. Reordering the elements
-// on the stats screen will have to be more beneficial than the hassle of not matching exactly to the
-// configurator list.
-
-static void osdShowStats(uint16_t endBatteryVoltage)
-{
- uint8_t top = 2;
- char buff[OSD_ELEMENT_BUFFER_LENGTH];
-
- displayClearScreen(osdDisplayPort);
- displayWrite(osdDisplayPort, 2, top++, " --- STATS ---");
-
- if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
- bool success = false;
-#ifdef USE_RTC_TIME
- success = osdFormatRtcDateTime(&buff[0]);
-#endif
- if (!success) {
- tfp_sprintf(buff, "NO RTC");
- }
-
- displayWrite(osdDisplayPort, 2, top++, buff);
- }
-
- if (osdStatGetState(OSD_STAT_TIMER_1)) {
- osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
- osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
- }
-
- if (osdStatGetState(OSD_STAT_TIMER_2)) {
- osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
- osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
- }
-
- if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
- osdFormatAltitudeString(buff, stats.max_altitude);
- osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
- }
-
-#ifdef USE_GPS
- if (featureIsEnabled(FEATURE_GPS)) {
- if (osdStatGetState(OSD_STAT_MAX_SPEED)) {
- itoa(stats.max_speed, buff, 10);
- osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
- }
-
- if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) {
- tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
- osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
- }
-
- if (osdStatGetState(OSD_STAT_FLIGHT_DISTANCE)) {
- const uint32_t distanceFlown = GPS_distanceFlownInCm / 100;
- tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(distanceFlown), osdGetMetersToSelectedUnitSymbol());
- osdDisplayStatisticLabel(top++, "FLIGHT DISTANCE", buff);
- }
- }
-#endif
-
- if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
- tfp_sprintf(buff, "%d.%02d%c", stats.min_voltage / 100, stats.min_voltage % 100, SYM_VOLT);
- osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
- }
-
- if (osdStatGetState(OSD_STAT_END_BATTERY)) {
- tfp_sprintf(buff, "%d.%02d%c", endBatteryVoltage / 100, endBatteryVoltage % 100, SYM_VOLT);
- osdDisplayStatisticLabel(top++, "END BATTERY", buff);
- }
-
- if (osdStatGetState(OSD_STAT_BATTERY)) {
- tfp_sprintf(buff, "%d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
- osdDisplayStatisticLabel(top++, "BATTERY", buff);
- }
-
- if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
- itoa(stats.min_rssi, buff, 10);
- strcat(buff, "%");
- osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
- }
-
- if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
- if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
- itoa(stats.max_current, buff, 10);
- strcat(buff, "A");
- osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
- }
-
- if (osdStatGetState(OSD_STAT_USED_MAH)) {
- tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
- osdDisplayStatisticLabel(top++, "USED MAH", buff);
- }
- }
-
-#ifdef USE_BLACKBOX
- if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
- osdGetBlackboxStatusString(buff);
- osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
- }
-
- if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
- itoa(blackboxGetLogNumber(), buff, 10);
- osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
- }
-#endif
-
-#if defined(USE_ACC)
- if (osdStatGetState(OSD_STAT_MAX_G_FORCE) && sensors(SENSOR_ACC)) {
- const int gForce = lrintf(stats.max_g_force * 10);
- tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10);
- osdDisplayStatisticLabel(top++, "MAX G-FORCE", buff);
- }
-#endif
-
-#ifdef USE_ESC_SENSOR
- if (osdStatGetState(OSD_STAT_MAX_ESC_TEMP)) {
- tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
- osdDisplayStatisticLabel(top++, "MAX ESC TEMP", buff);
- }
-
- if (osdStatGetState(OSD_STAT_MAX_ESC_RPM)) {
- itoa(stats.max_esc_rpm, buff, 10);
- osdDisplayStatisticLabel(top++, "MAX ESC RPM", buff);
- }
-#endif
-
-#ifdef USE_RX_LINK_QUALITY_INFO
- if (osdStatGetState(OSD_STAT_MIN_LINK_QUALITY)) {
- itoa(stats.min_link_quality, buff, 10);
- strcat(buff, "%");
- osdDisplayStatisticLabel(top++, "MIN LINK", buff);
- }
-#endif
-
-#if defined(USE_GYRO_DATA_ANALYSE)
- if (osdStatGetState(OSD_STAT_MAX_FFT) && featureIsEnabled(FEATURE_DYNAMIC_FILTER)) {
- int value = getMaxFFT();
- if (value > 0) {
- tfp_sprintf(buff, "%dHZ", value);
- osdDisplayStatisticLabel(top++, "PEAK FFT", buff);
- } else {
- osdDisplayStatisticLabel(top++, "PEAK FFT", "THRT<20%");
- }
- }
-#endif
-}
-
-static void osdShowArmed(void)
-{
- displayClearScreen(osdDisplayPort);
- displayWrite(osdDisplayPort, 12, 7, "ARMED");
-}
-
-STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
-{
- static timeUs_t lastTimeUs = 0;
- static bool osdStatsEnabled = false;
- static bool osdStatsVisible = false;
- static timeUs_t osdStatsRefreshTimeUs;
- static uint16_t endBatteryVoltage;
-
- // detect arm/disarm
- if (armState != ARMING_FLAG(ARMED)) {
- if (ARMING_FLAG(ARMED)) {
- osdStatsEnabled = false;
- osdStatsVisible = false;
- osdResetStats();
- osdShowArmed();
- resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
- } else if (isSomeStatEnabled()
- && !suppressStatsDisplay
- && (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
- || !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
- osdStatsEnabled = true;
- resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
- endBatteryVoltage = getBatteryVoltage();
- }
-
- armState = ARMING_FLAG(ARMED);
- }
-
-
- if (ARMING_FLAG(ARMED)) {
- osdUpdateStats();
- timeUs_t deltaT = currentTimeUs - lastTimeUs;
- flyTime += deltaT;
- stats.armed_time += deltaT;
- } else if (osdStatsEnabled) { // handle showing/hiding stats based on OSD disable switch position
- if (displayIsGrabbed(osdDisplayPort)) {
- osdStatsEnabled = false;
- resumeRefreshAt = 0;
- stats.armed_time = 0;
- } else {
- if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
- osdStatsVisible = false;
- displayClearScreen(osdDisplayPort);
- } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
- if (!osdStatsVisible) {
- osdStatsVisible = true;
- osdStatsRefreshTimeUs = 0;
- }
- if (currentTimeUs >= osdStatsRefreshTimeUs) {
- osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
- osdShowStats(endBatteryVoltage);
- }
- }
- }
- }
- lastTimeUs = currentTimeUs;
-
- if (resumeRefreshAt) {
- if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
- // in timeout period, check sticks for activity to resume display.
- if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
- resumeRefreshAt = currentTimeUs;
- }
- displayHeartbeat(osdDisplayPort);
- return;
- } else {
- displayClearScreen(osdDisplayPort);
- resumeRefreshAt = 0;
- osdStatsEnabled = false;
- stats.armed_time = 0;
- }
- }
-
- blinkState = (currentTimeUs / 200000) % 2;
-
-#ifdef USE_ESC_SENSOR
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
- }
-#endif
-
-#ifdef USE_CMS
- if (!displayIsGrabbed(osdDisplayPort))
-#endif
- {
- osdUpdateAlarms();
- osdDrawElements();
- displayHeartbeat(osdDisplayPort);
- }
- lastArmState = ARMING_FLAG(ARMED);
-}
-
-/*
- * Called periodically by the scheduler
- */
-void osdUpdate(timeUs_t currentTimeUs)
-{
- static uint32_t counter = 0;
-
- if (isBeeperOn()) {
- showVisualBeeper = true;
- }
-
-#ifdef MAX7456_DMA_CHANNEL_TX
- // don't touch buffers if DMA transaction is in progress
- if (displayIsTransferInProgress(osdDisplayPort)) {
- return;
- }
-#endif // MAX7456_DMA_CHANNEL_TX
-
-#ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
- static uint32_t idlecounter = 0;
- if (!ARMING_FLAG(ARMED)) {
- if (idlecounter++ % 4 != 0) {
- return;
- }
- }
-#endif
-
- // redraw values in buffer
-#ifdef USE_MAX7456
-#define DRAW_FREQ_DENOM 5
-#else
-#define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud (
-#endif
-#define STATS_FREQ_DENOM 50
-
- if (counter % DRAW_FREQ_DENOM == 0) {
- osdRefresh(currentTimeUs);
- showVisualBeeper = false;
- } else {
- // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
- displayDrawScreen(osdDisplayPort);
- }
- ++counter;
-}
-
-void osdSuppressStats(bool flag)
-{
- suppressStatsDisplay = flag;
-}
-
-#ifdef USE_OSD_PROFILES
-bool osdElementVisible(uint16_t value)
-{
- return (bool)((((value & OSD_PROFILE_MASK) >> OSD_PROFILE_BITS_POS) & osdProfile) != 0);
-}
-#endif
-#endif // USE_OSD
diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c
index 599645c6aa..8c0a045c50 100644
--- a/src/main/io/vtx_control.c
+++ b/src/main/io/vtx_control.c
@@ -37,11 +37,12 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
-#include "io/osd.h"
#include "io/spektrum_vtx_control.h"
#include "io/vtx.h"
#include "io/vtx_control.h"
+#include "osd/osd.h"
+
#include "pg/pg.h"
#include "pg/pg_ids.h"
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index f3cfc60029..46a4147aff 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -85,7 +85,6 @@
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/motors.h"
-#include "io/osd.h"
#include "io/serial.h"
#include "io/serial_4way.h"
#include "io/servos.h"
@@ -99,6 +98,9 @@
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
+#include "osd/osd.h"
+#include "osd/osd_elements.h"
+
#include "pg/beeper.h"
#include "pg/board.h"
#include "pg/gyrodev.h"
@@ -2672,8 +2674,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPos
// API >= 1.41
// selected OSD profile
#ifdef USE_OSD_PROFILES
- osdConfigMutable()->osdProfileIndex = sbufReadU8(src);
- setOsdProfile(osdConfig()->osdProfileIndex);
+ changeOsdProfileIndex(sbufReadU8(src));
#else
sbufReadU8(src);
#endif // USE_OSD_PROFILES
@@ -2714,6 +2715,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPos
} else if (addr < OSD_ITEM_COUNT) {
/* Set element positions */
osdConfigMutable()->item_pos[addr] = value;
+ osdAnalyzeActiveElements();
} else {
return MSP_RESULT_ERROR;
}
diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c
new file mode 100644
index 0000000000..acce6694a2
--- /dev/null
+++ b/src/main/osd/osd.c
@@ -0,0 +1,786 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ Created by Marcin Baliniak
+ some functions based on MinimOSD
+
+ OSD-CMS separation by jflyper
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_OSD
+
+#include "blackbox/blackbox.h"
+#include "blackbox/blackbox_io.h"
+
+#include "build/build_config.h"
+#include "build/version.h"
+
+#include "cms/cms.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/printf.h"
+#include "common/typeconversion.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+
+#include "drivers/display.h"
+#include "drivers/flash.h"
+#include "drivers/max7456_symbols.h"
+#include "drivers/sdcard.h"
+#include "drivers/time.h"
+
+#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+
+#include "flight/position.h"
+#include "flight/imu.h"
+
+#include "io/asyncfatfs/asyncfatfs.h"
+#include "io/beeper.h"
+#include "io/flashfs.h"
+#include "io/gps.h"
+
+#include "osd/osd.h"
+#include "osd/osd_elements.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "rx/rx.h"
+
+#include "sensors/acceleration.h"
+#include "sensors/battery.h"
+#include "sensors/esc_sensor.h"
+#include "sensors/sensors.h"
+#if defined(USE_GYRO_DATA_ANALYSE)
+#include "sensors/gyroanalyse.h"
+#endif
+
+#ifdef USE_HARDWARE_REVISION_DETECTION
+#include "hardware_revision.h"
+#endif
+
+const char * const osdTimerSourceNames[] = {
+ "ON TIME ",
+ "TOTAL ARM",
+ "LAST ARM "
+};
+
+// Things in both OSD and CMS
+
+#define IS_HI(X) (rcData[X] > 1750)
+#define IS_LO(X) (rcData[X] < 1250)
+#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
+
+timeUs_t osdFlyTime = 0;
+#if defined(USE_ACC)
+float osdGForce = 0;
+#endif
+
+static bool showVisualBeeper = false;
+
+static statistic_t stats;
+timeUs_t resumeRefreshAt = 0;
+#define REFRESH_1S 1000 * 1000
+
+static uint8_t armState;
+#ifdef USE_OSD_PROFILES
+static uint8_t osdProfile = 1;
+#endif
+static displayPort_t *osdDisplayPort;
+
+static bool suppressStatsDisplay = false;
+
+#ifdef USE_ESC_SENSOR
+escSensorData_t *osdEscDataCombined;
+#endif
+
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 5);
+
+void osdStatSetState(uint8_t statIndex, bool enabled)
+{
+ if (enabled) {
+ osdConfigMutable()->enabled_stats |= (1 << statIndex);
+ } else {
+ osdConfigMutable()->enabled_stats &= ~(1 << statIndex);
+ }
+}
+
+bool osdStatGetState(uint8_t statIndex)
+{
+ return osdConfig()->enabled_stats & (1 << statIndex);
+}
+
+void osdWarnSetState(uint8_t warningIndex, bool enabled)
+{
+ if (enabled) {
+ osdConfigMutable()->enabledWarnings |= (1 << warningIndex);
+ } else {
+ osdConfigMutable()->enabledWarnings &= ~(1 << warningIndex);
+ }
+}
+
+bool osdWarnGetState(uint8_t warningIndex)
+{
+ return osdConfig()->enabledWarnings & (1 << warningIndex);
+}
+
+#ifdef USE_OSD_PROFILES
+void setOsdProfile(uint8_t value)
+{
+ // 1 ->> 001
+ // 2 ->> 010
+ // 3 ->> 100
+ if (value <= OSD_PROFILE_COUNT) {
+ if (value == 0) {
+ osdProfile = 1;
+ } else {
+ osdProfile = 1 << (value - 1);
+ }
+ }
+ }
+
+uint8_t getCurrentOsdProfileIndex(void)
+{
+ return osdConfig()->osdProfileIndex;
+}
+
+void changeOsdProfileIndex(uint8_t profileIndex)
+{
+ if (profileIndex <= OSD_PROFILE_COUNT) {
+ osdConfigMutable()->osdProfileIndex = profileIndex;
+ setOsdProfile(profileIndex);
+ osdAnalyzeActiveElements();
+ }
+}
+#endif
+
+static void osdDrawElements(timeUs_t currentTimeUs)
+{
+ displayClearScreen(osdDisplayPort);
+
+ // Hide OSD when OSDSW mode is active
+ if (IS_RC_MODE_ACTIVE(BOXOSD)) {
+ return;
+ }
+
+ osdDrawActiveElements(osdDisplayPort, currentTimeUs);
+}
+
+void pgResetFn_osdConfig(osdConfig_t *osdConfig)
+{
+ // Position elements near centre of screen and disabled by default
+ for (int i = 0; i < OSD_ITEM_COUNT; i++) {
+ osdConfig->item_pos[i] = OSD_POS(10, 7);
+ }
+
+ // Always enable warnings elements by default
+ uint16_t profileFlags = 0;
+ for (unsigned i = 1; i <= OSD_PROFILE_COUNT; i++) {
+ profileFlags |= OSD_PROFILE_FLAG(i);
+ }
+ osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | profileFlags;
+
+ // Default to old fixed positions for these elements
+ osdConfig->item_pos[OSD_CROSSHAIRS] = OSD_POS(13, 6);
+ osdConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
+ osdConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(14, 6);
+
+ // Enable the default stats
+ osdConfig->enabled_stats = 0; // reset all to off and enable only a few initially
+ osdStatSetState(OSD_STAT_MAX_SPEED, true);
+ osdStatSetState(OSD_STAT_MIN_BATTERY, true);
+ osdStatSetState(OSD_STAT_MIN_RSSI, true);
+ osdStatSetState(OSD_STAT_MAX_CURRENT, true);
+ osdStatSetState(OSD_STAT_USED_MAH, true);
+ osdStatSetState(OSD_STAT_BLACKBOX, true);
+ osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, true);
+ osdStatSetState(OSD_STAT_TIMER_2, true);
+
+ osdConfig->units = OSD_UNIT_METRIC;
+
+ // Enable all warnings by default
+ for (int i=0; i < OSD_WARNING_COUNT; i++) {
+ osdWarnSetState(i, true);
+ }
+
+ osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10);
+ osdConfig->timers[OSD_TIMER_2] = OSD_TIMER(OSD_TIMER_SRC_TOTAL_ARMED, OSD_TIMER_PREC_SECOND, 10);
+
+ osdConfig->overlay_radio_mode = 2;
+
+ osdConfig->rssi_alarm = 20;
+ osdConfig->cap_alarm = 2200;
+ osdConfig->alt_alarm = 100; // meters or feet depend on configuration
+ osdConfig->esc_temp_alarm = ESC_TEMP_ALARM_OFF; // off by default
+ osdConfig->esc_rpm_alarm = ESC_RPM_ALARM_OFF; // off by default
+ osdConfig->esc_current_alarm = ESC_CURRENT_ALARM_OFF; // off by default
+ osdConfig->core_temp_alarm = 70; // a temperature above 70C should produce a warning, lockups have been reported above 80C
+
+ osdConfig->ahMaxPitch = 20; // 20 degrees
+ osdConfig->ahMaxRoll = 40; // 40 degrees
+
+ osdConfig->osdProfileIndex = 1;
+ osdConfig->ahInvert = false;
+}
+
+static void osdDrawLogo(int x, int y)
+{
+ // display logo and help
+ int fontOffset = 160;
+ for (int row = 0; row < 4; row++) {
+ for (int column = 0; column < 24; column++) {
+ if (fontOffset <= SYM_END_OF_FONT)
+ displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
+ }
+ }
+}
+
+void osdInit(displayPort_t *osdDisplayPortToUse)
+{
+ if (!osdDisplayPortToUse) {
+ return;
+ }
+
+ STATIC_ASSERT(OSD_POS_MAX == OSD_POS(31,31), OSD_POS_MAX_incorrect);
+
+ osdDisplayPort = osdDisplayPortToUse;
+#ifdef USE_CMS
+ cmsDisplayPortRegister(osdDisplayPort);
+#endif
+
+ armState = ARMING_FLAG(ARMED);
+
+ osdResetAlarms();
+
+ displayClearScreen(osdDisplayPort);
+
+ osdDrawLogo(3, 1);
+
+ char string_buffer[30];
+ tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
+ displayWrite(osdDisplayPort, 20, 6, string_buffer);
+#ifdef USE_CMS
+ displayWrite(osdDisplayPort, 7, 8, CMS_STARTUP_HELP_TEXT1);
+ displayWrite(osdDisplayPort, 11, 9, CMS_STARTUP_HELP_TEXT2);
+ displayWrite(osdDisplayPort, 11, 10, CMS_STARTUP_HELP_TEXT3);
+#endif
+
+#ifdef USE_RTC_TIME
+ char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
+ if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
+ displayWrite(osdDisplayPort, 5, 12, dateTimeBuffer);
+ }
+#endif
+
+ displayResync(osdDisplayPort);
+
+ resumeRefreshAt = micros() + (4 * REFRESH_1S);
+#ifdef USE_OSD_PROFILES
+ setOsdProfile(osdConfig()->osdProfileIndex);
+#endif
+ osdAnalyzeActiveElements();
+}
+
+bool osdInitialized(void)
+{
+ return osdDisplayPort;
+}
+
+static void osdResetStats(void)
+{
+ stats.max_current = 0;
+ stats.max_speed = 0;
+ stats.min_voltage = 5000;
+ stats.min_rssi = 99; // percent
+ stats.max_altitude = 0;
+ stats.max_distance = 0;
+ stats.armed_time = 0;
+ stats.max_g_force = 0;
+ stats.max_esc_temp = 0;
+ stats.max_esc_rpm = 0;
+ stats.min_link_quality = 99; // percent
+}
+
+static void osdUpdateStats(void)
+{
+ int16_t value = 0;
+#ifdef USE_GPS
+ switch (osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ value = CM_S_TO_MPH(gpsSol.groundSpeed);
+ break;
+ default:
+ value = CM_S_TO_KM_H(gpsSol.groundSpeed);
+ break;
+ }
+#endif
+ if (stats.max_speed < value) {
+ stats.max_speed = value;
+ }
+
+ value = getBatteryVoltage();
+ if (stats.min_voltage > value) {
+ stats.min_voltage = value;
+ }
+
+ value = getAmperage() / 100;
+ if (stats.max_current < value) {
+ stats.max_current = value;
+ }
+
+ value = getRssiPercent();
+ if (stats.min_rssi > value) {
+ stats.min_rssi = value;
+ }
+
+ int32_t altitudeCm = getEstimatedAltitudeCm();
+ if (stats.max_altitude < altitudeCm) {
+ stats.max_altitude = altitudeCm;
+ }
+
+#if defined(USE_ACC)
+ if (stats.max_g_force < osdGForce) {
+ stats.max_g_force = osdGForce;
+ }
+#endif
+
+#ifdef USE_RX_LINK_QUALITY_INFO
+ value = rxGetLinkQualityPercent();
+ if (stats.min_link_quality > value) {
+ stats.min_link_quality = value;
+ }
+#endif
+
+#ifdef USE_GPS
+ if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
+ value = GPS_distanceToHome;
+
+ if (stats.max_distance < GPS_distanceToHome) {
+ stats.max_distance = GPS_distanceToHome;
+ }
+ }
+#endif
+#ifdef USE_ESC_SENSOR
+ if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
+ value = osdEscDataCombined->temperature;
+ if (stats.max_esc_temp < value) {
+ stats.max_esc_temp = value;
+ }
+ value = calcEscRpm(osdEscDataCombined->rpm);
+ if (stats.max_esc_rpm < value) {
+ stats.max_esc_rpm = value;
+ }
+ }
+#endif
+}
+
+#ifdef USE_BLACKBOX
+
+static void osdGetBlackboxStatusString(char * buff)
+{
+ bool storageDeviceIsWorking = isBlackboxDeviceWorking();
+ uint32_t storageUsed = 0;
+ uint32_t storageTotal = 0;
+
+ switch (blackboxConfig()->device) {
+#ifdef USE_SDCARD
+ case BLACKBOX_DEVICE_SDCARD:
+ if (storageDeviceIsWorking) {
+ storageTotal = sdcard_getMetadata()->numBlocks / 2000;
+ storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
+ }
+ break;
+#endif
+
+#ifdef USE_FLASHFS
+ case BLACKBOX_DEVICE_FLASH:
+ if (storageDeviceIsWorking) {
+ const flashGeometry_t *geometry = flashfsGetGeometry();
+ storageTotal = geometry->totalSize / 1024;
+ storageUsed = flashfsGetOffset() / 1024;
+ }
+ break;
+#endif
+
+ default:
+ break;
+ }
+
+ if (storageDeviceIsWorking) {
+ const uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
+ tfp_sprintf(buff, "%d%%", storageUsedPercent);
+ } else {
+ tfp_sprintf(buff, "FAULT");
+ }
+}
+#endif
+
+static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
+{
+ displayWrite(osdDisplayPort, 2, y, text);
+ displayWrite(osdDisplayPort, 20, y, ":");
+ displayWrite(osdDisplayPort, 22, y, value);
+}
+
+/*
+ * Test if there's some stat enabled
+ */
+static bool isSomeStatEnabled(void)
+{
+ return (osdConfig()->enabled_stats != 0);
+}
+
+// *** IMPORTANT ***
+// The stats display order was previously required to match the enumeration definition so it matched
+// the order shown in the configurator. However, to allow reordering this screen without breaking the
+// compatibility, this requirement has been relaxed to a best effort approach. Reordering the elements
+// on the stats screen will have to be more beneficial than the hassle of not matching exactly to the
+// configurator list.
+
+static void osdShowStats(uint16_t endBatteryVoltage)
+{
+ uint8_t top = 2;
+ char buff[OSD_ELEMENT_BUFFER_LENGTH];
+
+ displayClearScreen(osdDisplayPort);
+ displayWrite(osdDisplayPort, 2, top++, " --- STATS ---");
+
+ if (osdStatGetState(OSD_STAT_RTC_DATE_TIME)) {
+ bool success = false;
+#ifdef USE_RTC_TIME
+ success = osdFormatRtcDateTime(&buff[0]);
+#endif
+ if (!success) {
+ tfp_sprintf(buff, "NO RTC");
+ }
+
+ displayWrite(osdDisplayPort, 2, top++, buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_TIMER_1)) {
+ osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_1);
+ osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_TIMER_2)) {
+ osdFormatTimer(buff, false, (OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2]) == OSD_TIMER_SRC_ON ? false : true), OSD_TIMER_2);
+ osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_MAX_ALTITUDE)) {
+ osdFormatAltitudeString(buff, stats.max_altitude);
+ osdDisplayStatisticLabel(top++, "MAX ALTITUDE", buff);
+ }
+
+#ifdef USE_GPS
+ if (featureIsEnabled(FEATURE_GPS)) {
+ if (osdStatGetState(OSD_STAT_MAX_SPEED)) {
+ itoa(stats.max_speed, buff, 10);
+ osdDisplayStatisticLabel(top++, "MAX SPEED", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) {
+ tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol());
+ osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_FLIGHT_DISTANCE)) {
+ const uint32_t distanceFlown = GPS_distanceFlownInCm / 100;
+ tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(distanceFlown), osdGetMetersToSelectedUnitSymbol());
+ osdDisplayStatisticLabel(top++, "FLIGHT DISTANCE", buff);
+ }
+ }
+#endif
+
+ if (osdStatGetState(OSD_STAT_MIN_BATTERY)) {
+ tfp_sprintf(buff, "%d.%02d%c", stats.min_voltage / 100, stats.min_voltage % 100, SYM_VOLT);
+ osdDisplayStatisticLabel(top++, "MIN BATTERY", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_END_BATTERY)) {
+ tfp_sprintf(buff, "%d.%02d%c", endBatteryVoltage / 100, endBatteryVoltage % 100, SYM_VOLT);
+ osdDisplayStatisticLabel(top++, "END BATTERY", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_BATTERY)) {
+ tfp_sprintf(buff, "%d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
+ osdDisplayStatisticLabel(top++, "BATTERY", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_MIN_RSSI)) {
+ itoa(stats.min_rssi, buff, 10);
+ strcat(buff, "%");
+ osdDisplayStatisticLabel(top++, "MIN RSSI", buff);
+ }
+
+ if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
+ if (osdStatGetState(OSD_STAT_MAX_CURRENT)) {
+ itoa(stats.max_current, buff, 10);
+ strcat(buff, "A");
+ osdDisplayStatisticLabel(top++, "MAX CURRENT", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_USED_MAH)) {
+ tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
+ osdDisplayStatisticLabel(top++, "USED MAH", buff);
+ }
+ }
+
+#ifdef USE_BLACKBOX
+ if (osdStatGetState(OSD_STAT_BLACKBOX) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
+ osdGetBlackboxStatusString(buff);
+ osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_BLACKBOX_NUMBER) && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
+ itoa(blackboxGetLogNumber(), buff, 10);
+ osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
+ }
+#endif
+
+#if defined(USE_ACC)
+ if (osdStatGetState(OSD_STAT_MAX_G_FORCE) && sensors(SENSOR_ACC)) {
+ const int gForce = lrintf(stats.max_g_force * 10);
+ tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10);
+ osdDisplayStatisticLabel(top++, "MAX G-FORCE", buff);
+ }
+#endif
+
+#ifdef USE_ESC_SENSOR
+ if (osdStatGetState(OSD_STAT_MAX_ESC_TEMP)) {
+ tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
+ osdDisplayStatisticLabel(top++, "MAX ESC TEMP", buff);
+ }
+
+ if (osdStatGetState(OSD_STAT_MAX_ESC_RPM)) {
+ itoa(stats.max_esc_rpm, buff, 10);
+ osdDisplayStatisticLabel(top++, "MAX ESC RPM", buff);
+ }
+#endif
+
+#ifdef USE_RX_LINK_QUALITY_INFO
+ if (osdStatGetState(OSD_STAT_MIN_LINK_QUALITY)) {
+ itoa(stats.min_link_quality, buff, 10);
+ strcat(buff, "%");
+ osdDisplayStatisticLabel(top++, "MIN LINK", buff);
+ }
+#endif
+
+#if defined(USE_GYRO_DATA_ANALYSE)
+ if (osdStatGetState(OSD_STAT_MAX_FFT) && featureIsEnabled(FEATURE_DYNAMIC_FILTER)) {
+ int value = getMaxFFT();
+ if (value > 0) {
+ tfp_sprintf(buff, "%dHZ", value);
+ osdDisplayStatisticLabel(top++, "PEAK FFT", buff);
+ } else {
+ osdDisplayStatisticLabel(top++, "PEAK FFT", "THRT<20%");
+ }
+ }
+#endif
+}
+
+static void osdShowArmed(void)
+{
+ displayClearScreen(osdDisplayPort);
+ displayWrite(osdDisplayPort, 12, 7, "ARMED");
+}
+
+STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
+{
+ static timeUs_t lastTimeUs = 0;
+ static bool osdStatsEnabled = false;
+ static bool osdStatsVisible = false;
+ static timeUs_t osdStatsRefreshTimeUs;
+ static uint16_t endBatteryVoltage;
+
+ // detect arm/disarm
+ if (armState != ARMING_FLAG(ARMED)) {
+ if (ARMING_FLAG(ARMED)) {
+ osdStatsEnabled = false;
+ osdStatsVisible = false;
+ osdResetStats();
+ osdShowArmed();
+ resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
+ } else if (isSomeStatEnabled()
+ && !suppressStatsDisplay
+ && (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
+ || !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
+ osdStatsEnabled = true;
+ resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
+ endBatteryVoltage = getBatteryVoltage();
+ }
+
+ armState = ARMING_FLAG(ARMED);
+ }
+
+
+ if (ARMING_FLAG(ARMED)) {
+ osdUpdateStats();
+ timeUs_t deltaT = currentTimeUs - lastTimeUs;
+ osdFlyTime += deltaT;
+ stats.armed_time += deltaT;
+ } else if (osdStatsEnabled) { // handle showing/hiding stats based on OSD disable switch position
+ if (displayIsGrabbed(osdDisplayPort)) {
+ osdStatsEnabled = false;
+ resumeRefreshAt = 0;
+ stats.armed_time = 0;
+ } else {
+ if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
+ osdStatsVisible = false;
+ displayClearScreen(osdDisplayPort);
+ } else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
+ if (!osdStatsVisible) {
+ osdStatsVisible = true;
+ osdStatsRefreshTimeUs = 0;
+ }
+ if (currentTimeUs >= osdStatsRefreshTimeUs) {
+ osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
+ osdShowStats(endBatteryVoltage);
+ }
+ }
+ }
+ }
+ lastTimeUs = currentTimeUs;
+
+ if (resumeRefreshAt) {
+ if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
+ // in timeout period, check sticks for activity to resume display.
+ if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
+ resumeRefreshAt = currentTimeUs;
+ }
+ displayHeartbeat(osdDisplayPort);
+ return;
+ } else {
+ displayClearScreen(osdDisplayPort);
+ resumeRefreshAt = 0;
+ osdStatsEnabled = false;
+ stats.armed_time = 0;
+ }
+ }
+
+#ifdef USE_ESC_SENSOR
+ if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
+ osdEscDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
+ }
+#endif
+
+#if defined(USE_ACC)
+ if (sensors(SENSOR_ACC)
+ && (VISIBLE(osdConfig()->item_pos[OSD_G_FORCE]) || osdStatGetState(OSD_STAT_MAX_G_FORCE))) {
+ // only calculate the G force if the element is visible or the stat is enabled
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ const float a = accAverage[axis];
+ osdGForce += a * a;
+ }
+ osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
+ }
+#endif
+
+#ifdef USE_CMS
+ if (!displayIsGrabbed(osdDisplayPort))
+#endif
+ {
+ osdUpdateAlarms();
+ osdDrawElements(currentTimeUs);
+ displayHeartbeat(osdDisplayPort);
+ }
+}
+
+/*
+ * Called periodically by the scheduler
+ */
+void osdUpdate(timeUs_t currentTimeUs)
+{
+ static uint32_t counter = 0;
+
+ if (isBeeperOn()) {
+ showVisualBeeper = true;
+ }
+
+#ifdef MAX7456_DMA_CHANNEL_TX
+ // don't touch buffers if DMA transaction is in progress
+ if (displayIsTransferInProgress(osdDisplayPort)) {
+ return;
+ }
+#endif // MAX7456_DMA_CHANNEL_TX
+
+#ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
+ static uint32_t idlecounter = 0;
+ if (!ARMING_FLAG(ARMED)) {
+ if (idlecounter++ % 4 != 0) {
+ return;
+ }
+ }
+#endif
+
+ // redraw values in buffer
+#ifdef USE_MAX7456
+#define DRAW_FREQ_DENOM 5
+#else
+#define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud (
+#endif
+#define STATS_FREQ_DENOM 50
+
+ if (counter % DRAW_FREQ_DENOM == 0) {
+ osdRefresh(currentTimeUs);
+ showVisualBeeper = false;
+ } else {
+ // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
+ displayDrawScreen(osdDisplayPort);
+ }
+ ++counter;
+}
+
+void osdSuppressStats(bool flag)
+{
+ suppressStatsDisplay = flag;
+}
+
+#ifdef USE_OSD_PROFILES
+bool osdElementVisible(uint16_t value)
+{
+ return (bool)((((value & OSD_PROFILE_MASK) >> OSD_PROFILE_BITS_POS) & osdProfile) != 0);
+}
+#endif
+
+bool osdGetVisualBeeperState(void)
+{
+ return showVisualBeeper;
+}
+
+statistic_t *osdGetStats(void)
+{
+ return &stats;
+}
+#endif // USE_OSD
diff --git a/src/main/io/osd.h b/src/main/osd/osd.h
similarity index 91%
rename from src/main/io/osd.h
rename to src/main/osd/osd.h
index dae1577604..2a35ee508d 100644
--- a/src/main/io/osd.h
+++ b/src/main/osd/osd.h
@@ -21,8 +21,11 @@
#pragma once
#include "common/time.h"
+
#include "pg/pg.h"
+#include "sensors/esc_sensor.h"
+
#define OSD_NUM_TIMER_TYPES 3
extern const char * const osdTimerSourceNames[OSD_NUM_TIMER_TYPES];
@@ -68,8 +71,8 @@ extern const char * const osdTimerSourceNames[OSD_NUM_TIMER_TYPES];
// NB: to ensure backwards compatibility, new enum values must be appended at the end but before the OSD_XXXX_COUNT entry.
// *** IMPORTANT ***
-// If you are adding additional elements that do not require any conditional display logic,
-// you must add the elements to the osdElementDisplayOrder[] array in src/main/io/osd.c
+// See the information at the top of osd/osd_elements.c for instructions
+// on how to add OSD elements.
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,
@@ -238,12 +241,33 @@ typedef struct osdConfig_s {
PG_DECLARE(osdConfig_t, osdConfig);
+typedef struct statistic_s {
+ timeUs_t armed_time;
+ int16_t max_speed;
+ int16_t min_voltage; // /100
+ int16_t max_current; // /10
+ uint8_t min_rssi;
+ int32_t max_altitude;
+ int16_t max_distance;
+ float max_g_force;
+ int16_t max_esc_temp;
+ int32_t max_esc_rpm;
+ uint8_t min_link_quality;
+} statistic_t;
+
extern timeUs_t resumeRefreshAt;
+extern timeUs_t osdFlyTime;
+#if defined(USE_ACC)
+extern float osdGForce;
+#endif
+#ifdef USE_ESC_SENSOR
+extern escSensorData_t *osdEscDataCombined;
+#endif
+
struct displayPort_s;
void osdInit(struct displayPort_s *osdDisplayPort);
bool osdInitialized(void);
-void osdResetAlarms(void);
void osdUpdate(timeUs_t currentTimeUs);
void osdStatSetState(uint8_t statIndex, bool enabled);
bool osdStatGetState(uint8_t statIndex);
@@ -251,7 +275,8 @@ void osdWarnSetState(uint8_t warningIndex, bool enabled);
bool osdWarnGetState(uint8_t warningIndex);
void osdSuppressStats(bool flag);
-void setOsdProfile(uint8_t value);
uint8_t getCurrentOsdProfileIndex(void);
void changeOsdProfileIndex(uint8_t profileIndex);
bool osdElementVisible(uint16_t value);
+bool osdGetVisualBeeperState(void);
+statistic_t *osdGetStats(void);
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
new file mode 100644
index 0000000000..b8083d05e2
--- /dev/null
+++ b/src/main/osd/osd_elements.c
@@ -0,0 +1,1499 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+/*
+ *****************************************
+ Instructions for adding new OSD Elements:
+ *****************************************
+
+ First add the new element to the osd_items_e enumeration in osd/osd.h. The
+ element must be added to the end just before OSD_ITEM_COUNT.
+
+ Next add the element to the osdElementDisplayOrder array defined in this file.
+ If the element needs special runtime conditional processing then it should be added
+ to the osdAnalyzeActiveElements() function instead.
+
+ Create the function to "draw" the element. It should be named like "osdElementSomething()"
+ where the "Something" describes the element.
+
+ Finally add the mapping from the element ID added in the first step to the function
+ created in the third step to the osdElementDrawFunction array.
+*/
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_OSD
+
+#include "blackbox/blackbox.h"
+#include "blackbox/blackbox_io.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/printf.h"
+#include "common/typeconversion.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+
+#include "drivers/display.h"
+#include "drivers/max7456_symbols.h"
+#include "drivers/time.h"
+#include "drivers/vtx_common.h"
+
+#include "fc/config.h"
+#include "fc/core.h"
+#include "fc/rc_adjustments.h"
+#include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
+#include "fc/rc.h"
+#include "fc/runtime_config.h"
+
+#include "flight/gps_rescue.h"
+#include "flight/failsafe.h"
+#include "flight/position.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "io/beeper.h"
+#include "io/gps.h"
+#include "io/vtx.h"
+
+#include "osd/osd.h"
+#include "osd/osd_elements.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "rx/rx.h"
+
+#include "sensors/acceleration.h"
+#include "sensors/adcinternal.h"
+#include "sensors/barometer.h"
+#include "sensors/battery.h"
+#include "sensors/esc_sensor.h"
+#include "sensors/sensors.h"
+
+
+#define AH_SYMBOL_COUNT 9
+#define AH_SIDEBAR_WIDTH_POS 7
+#define AH_SIDEBAR_HEIGHT_POS 3
+
+// Stick overlay size
+#define OSD_STICK_OVERLAY_WIDTH 7
+#define OSD_STICK_OVERLAY_HEIGHT 5
+#define OSD_STICK_OVERLAY_SPRITE_HEIGHT 3
+#define OSD_STICK_OVERLAY_VERTICAL_POSITIONS (OSD_STICK_OVERLAY_HEIGHT * OSD_STICK_OVERLAY_SPRITE_HEIGHT)
+
+#define FULL_CIRCLE 360
+
+#ifdef USE_OSD_STICK_OVERLAY
+typedef struct radioControls_s {
+ uint8_t left_vertical;
+ uint8_t left_horizontal;
+ uint8_t right_vertical;
+ uint8_t right_horizontal;
+} radioControls_t;
+
+static const radioControls_t radioModes[4] = {
+ { PITCH, YAW, THROTTLE, ROLL }, // Mode 1
+ { THROTTLE, YAW, PITCH, ROLL }, // Mode 2
+ { PITCH, ROLL, THROTTLE, YAW }, // Mode 3
+ { THROTTLE, ROLL, PITCH, YAW }, // Mode 4
+};
+#endif
+
+static const char compassBar[] = {
+ SYM_HEADING_W,
+ SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
+ SYM_HEADING_N,
+ SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
+ SYM_HEADING_E,
+ SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
+ SYM_HEADING_S,
+ SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
+ SYM_HEADING_W,
+ SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE,
+ SYM_HEADING_N,
+ SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE
+};
+
+static unsigned activeOsdElementCount = 0;
+static uint8_t activeOsdElementArray[OSD_ITEM_COUNT];
+
+// Blink control
+static bool blinkState = true;
+static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32];
+#define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
+#define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
+#define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
+#define BLINK(item) (IS_BLINK(item) && blinkState)
+
+
+#if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
+int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
+{
+ switch (osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ return lrintf(((tempInDegreesCelcius * 9.0f) / 5) + 32);
+ default:
+ return tempInDegreesCelcius;
+ }
+}
+#endif
+
+#ifdef USE_GPS
+static void osdFormatCoordinate(char *buff, char sym, int32_t val)
+{
+ // latitude maximum integer width is 3 (-90).
+ // longitude maximum integer width is 4 (-180).
+ // We show 7 decimals, so we need to use 12 characters:
+ // eg: s-180.1234567z s=symbol, z=zero terminator, decimal separator between 0 and 1
+
+ static const int coordinateMaxLength = 13;//12 for the number (4 + dot + 7) + 1 for the symbol
+
+ buff[0] = sym;
+ const int32_t integerPart = val / GPS_DEGREES_DIVIDER;
+ const int32_t decimalPart = labs(val % GPS_DEGREES_DIVIDER);
+ const int written = tfp_sprintf(buff + 1, "%d.%07d", integerPart, decimalPart);
+ // pad with blanks to coordinateMaxLength
+ for (int pos = 1 + written; pos < coordinateMaxLength; ++pos) {
+ buff[pos] = SYM_BLANK;
+ }
+ buff[coordinateMaxLength] = '\0';
+}
+#endif // USE_GPS
+
+static void osdFormatMessage(char *buff, size_t size, const char *message)
+{
+ memset(buff, SYM_BLANK, size);
+ if (message) {
+ memcpy(buff, message, strlen(message));
+ }
+ // Ensure buff is zero terminated
+ buff[size - 1] = '\0';
+}
+
+static void osdFormatPID(char * buff, const char * label, const pidf_t * pid)
+{
+ tfp_sprintf(buff, "%s %3d %3d %3d", label, pid->P, pid->I, pid->D);
+}
+
+#ifdef USE_RTC_TIME
+bool osdFormatRtcDateTime(char *buffer)
+{
+ dateTime_t dateTime;
+ if (!rtcGetDateTime(&dateTime)) {
+ buffer[0] = '\0';
+
+ return false;
+ }
+
+ dateTimeFormatLocalShort(buffer, &dateTime);
+
+ return true;
+}
+#endif
+
+void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time)
+{
+ int seconds = time / 1000000;
+ const int minutes = seconds / 60;
+ seconds = seconds % 60;
+
+ switch (precision) {
+ case OSD_TIMER_PREC_SECOND:
+ default:
+ tfp_sprintf(buff, "%02d:%02d", minutes, seconds);
+ break;
+ case OSD_TIMER_PREC_HUNDREDTHS:
+ {
+ const int hundredths = (time / 10000) % 100;
+ tfp_sprintf(buff, "%02d:%02d.%02d", minutes, seconds, hundredths);
+ break;
+ }
+ }
+}
+
+static char osdGetTimerSymbol(osd_timer_source_e src)
+{
+ switch (src) {
+ case OSD_TIMER_SRC_ON:
+ return SYM_ON_M;
+ case OSD_TIMER_SRC_TOTAL_ARMED:
+ case OSD_TIMER_SRC_LAST_ARMED:
+ return SYM_FLY_M;
+ default:
+ return ' ';
+ }
+}
+
+static timeUs_t osdGetTimerValue(osd_timer_source_e src)
+{
+ switch (src) {
+ case OSD_TIMER_SRC_ON:
+ return micros();
+ case OSD_TIMER_SRC_TOTAL_ARMED:
+ return osdFlyTime;
+ case OSD_TIMER_SRC_LAST_ARMED: {
+ statistic_t *stats = osdGetStats();
+ return stats->armed_time;
+ }
+ default:
+ return 0;
+ }
+}
+
+void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex)
+{
+ const uint16_t timer = osdConfig()->timers[timerIndex];
+ const uint8_t src = OSD_TIMER_SRC(timer);
+
+ if (showSymbol) {
+ *(buff++) = osdGetTimerSymbol(src);
+ }
+
+ osdFormatTime(buff, (usePrecision ? OSD_TIMER_PRECISION(timer) : OSD_TIMER_PREC_SECOND), osdGetTimerValue(src));
+}
+
+static char osdGetBatterySymbol(int cellVoltage)
+{
+ if (getBatteryState() == BATTERY_CRITICAL) {
+ return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark
+ } else {
+ // Calculate a symbol offset using cell voltage over full cell voltage range
+ const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatmaxcellvoltage, 0, 8);
+ return SYM_BATT_EMPTY - constrain(symOffset, 0, 6);
+ }
+}
+
+static uint8_t osdGetHeadingIntoDiscreteDirections(int heading, unsigned directions)
+{
+ heading += FULL_CIRCLE; // Ensure positive value
+
+ // Split input heading 0..359 into sectors 0..(directions-1), but offset
+ // by half a sector so that sector 0 gets centered around heading 0.
+ // We multiply heading by directions to not loose precision in divisions
+ // In this way each segment will be a FULL_CIRCLE length
+ int direction = (heading * directions + FULL_CIRCLE / 2) / FULL_CIRCLE; // scale with rounding
+ direction %= directions; // normalize
+
+ return direction; // return segment number
+}
+
+static uint8_t osdGetDirectionSymbolFromHeading(int heading)
+{
+ heading = osdGetHeadingIntoDiscreteDirections(heading, 16);
+
+ // Now heading has a heading with Up=0, Right=4, Down=8 and Left=12
+ // Our symbols are Down=0, Right=4, Up=8 and Left=12
+ // There're 16 arrow symbols. Transform it.
+ heading = 16 - heading;
+ heading = (heading + 8) % 16;
+
+ return SYM_ARROW_SOUTH + heading;
+}
+
+
+/**
+ * Converts altitude based on the current unit system.
+ * @param meters Value in meters to convert
+ */
+int32_t osdGetMetersToSelectedUnit(int32_t meters)
+{
+ switch (osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ return (meters * 328) / 100; // Convert to feet / 100
+ default:
+ return meters; // Already in metre / 100
+ }
+}
+
+/**
+ * Gets the correct altitude symbol for the current unit system
+ */
+char osdGetMetersToSelectedUnitSymbol(void)
+{
+ switch (osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ return SYM_FT;
+ default:
+ return SYM_M;
+ }
+}
+
+#if defined(USE_ADC_INTERNAL) || defined(USE_ESC_SENSOR)
+char osdGetTemperatureSymbolForSelectedUnit(void)
+{
+ switch (osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ return SYM_F;
+ default:
+ return SYM_C;
+ }
+}
+#endif
+
+// *************************
+// Element drawing functions
+// *************************
+
+#ifdef USE_OSD_ADJUSTMENTS
+static void osdElementAdjustmentRange(osdElementParms_t *element)
+{
+ if (getAdjustmentsRangeName()) {
+ tfp_sprintf(element->buff, "%s: %3d", getAdjustmentsRangeName(), getAdjustmentsRangeValue());
+ }
+}
+#endif // USE_OSD_ADJUSTMENTS
+
+static void osdElementAltitude(osdElementParms_t *element)
+{
+ bool haveBaro = false;
+ bool haveGps = false;
+#ifdef USE_BARO
+ haveBaro = sensors(SENSOR_BARO);
+#endif // USE_BARO
+#ifdef USE_GPS
+ haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
+#endif // USE_GPS
+ if (haveBaro || haveGps) {
+ osdFormatAltitudeString(element->buff, getEstimatedAltitudeCm());
+ } else {
+ // We use this symbol when we don't have a valid measure
+ element->buff[0] = SYM_COLON;
+ // overwrite any previous altitude with blanks
+ memset(element->buff + 1, SYM_BLANK, 6);
+ element->buff[7] = '\0';
+ }
+}
+
+#ifdef USE_ACC
+static void osdElementAngleRollPitch(osdElementParms_t *element)
+{
+ const int angle = (element->item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll;
+ tfp_sprintf(element->buff, "%c%02d.%01d", angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10));
+}
+#endif
+
+static void osdElementAntiGravity(osdElementParms_t *element)
+{
+ if (pidOsdAntiGravityActive()) {
+ strcpy(element->buff, "AG");
+ }
+}
+
+#ifdef USE_ACC
+static void osdElementArtificialHorizon(osdElementParms_t *element)
+{
+ // Get pitch and roll limits in tenths of degrees
+ const int maxPitch = osdConfig()->ahMaxPitch * 10;
+ const int maxRoll = osdConfig()->ahMaxRoll * 10;
+ const int ahSign = osdConfig()->ahInvert ? -1 : 1;
+ const int rollAngle = constrain(attitude.values.roll * ahSign, -maxRoll, maxRoll);
+ int pitchAngle = constrain(attitude.values.pitch * ahSign, -maxPitch, maxPitch);
+ // Convert pitchAngle to y compensation value
+ // (maxPitch / 25) divisor matches previous settings of fixed divisor of 8 and fixed max AHI pitch angle of 20.0 degrees
+ if (maxPitch > 0) {
+ pitchAngle = ((pitchAngle * 25) / maxPitch);
+ }
+ pitchAngle -= 41; // 41 = 4 * AH_SYMBOL_COUNT + 5
+
+ for (int x = -4; x <= 4; x++) {
+ const int y = ((-rollAngle * x) / 64) - pitchAngle;
+ if (y >= 0 && y <= 81) {
+ displayWriteChar(element->osdDisplayPort, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
+ }
+ }
+
+ element->drawElement = false; // element already drawn
+}
+#endif // USE_ACC
+
+static void osdElementAverageCellVoltage(osdElementParms_t *element)
+{
+ const int cellV = getBatteryAverageCellVoltage();
+ element->buff[0] = osdGetBatterySymbol(cellV);
+ tfp_sprintf(element->buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT);
+}
+
+static void osdElementCompassBar(osdElementParms_t *element)
+{
+ memcpy(element->buff, compassBar + osdGetHeadingIntoDiscreteDirections(DECIDEGREES_TO_DEGREES(attitude.values.yaw), 16), 9);
+ element->buff[9] = 0;
+}
+
+#ifdef USE_ADC_INTERNAL
+static void osdElementCoreTemperature(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%3d%c", osdConvertTemperatureToSelectedUnit(getCoreTemperatureCelsius()), osdGetTemperatureSymbolForSelectedUnit());
+}
+#endif // USE_ADC_INTERNAL
+
+static void osdElementCraftName(osdElementParms_t *element)
+{
+ // 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.
+ //TODO: When iterative updating is implemented, change this so the craft name is only printed once whenever the OSD 'flight' screen is entered.
+
+ if (strlen(pilotConfig()->name) == 0) {
+ strcpy(element->buff, "CRAFT_NAME");
+ } else {
+ unsigned i;
+ for (i = 0; i < MAX_NAME_LENGTH; i++) {
+ if (pilotConfig()->name[i]) {
+ element->buff[i] = toupper((unsigned char)pilotConfig()->name[i]);
+ } else {
+ break;
+ }
+ }
+ element->buff[i] = '\0';
+ }
+}
+
+#ifdef USE_ACC
+static void osdElementCrashFlipArrow(osdElementParms_t *element)
+{
+ int rollAngle = attitude.values.roll / 10;
+ const int pitchAngle = attitude.values.pitch / 10;
+ if (abs(rollAngle) > 90) {
+ rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle;
+ }
+
+ if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) {
+ if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) {
+ if (pitchAngle > 0) {
+ if (rollAngle > 0) {
+ element->buff[0] = SYM_ARROW_WEST + 2;
+ } else {
+ element->buff[0] = SYM_ARROW_EAST - 2;
+ }
+ } else {
+ if (rollAngle > 0) {
+ element->buff[0] = SYM_ARROW_WEST - 2;
+ } else {
+ element->buff[0] = SYM_ARROW_EAST + 2;
+ }
+ }
+ } else {
+ if (abs(pitchAngle) > abs(rollAngle)) {
+ if (pitchAngle > 0) {
+ element->buff[0] = SYM_ARROW_SOUTH;
+ } else {
+ element->buff[0] = SYM_ARROW_NORTH;
+ }
+ } else {
+ if (rollAngle > 0) {
+ element->buff[0] = SYM_ARROW_WEST;
+ } else {
+ element->buff[0] = SYM_ARROW_EAST;
+ }
+ }
+ }
+ } else {
+ element->buff[0] = ' ';
+ }
+
+ element->buff[1] = '\0';
+}
+#endif // USE_ACC
+
+static void osdElementCrosshairs(osdElementParms_t *element)
+{
+ element->buff[0] = SYM_AH_CENTER_LINE;
+ element->buff[1] = SYM_AH_CENTER;
+ element->buff[2] = SYM_AH_CENTER_LINE_RIGHT;
+ element->buff[3] = 0;
+}
+
+static void osdElementCurrentDraw(osdElementParms_t *element)
+{
+ const int32_t amperage = getAmperage();
+ tfp_sprintf(element->buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP);
+}
+
+static void osdElementDebug(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "DBG %5d %5d %5d %5d", debug[0], debug[1], debug[2], debug[3]);
+}
+
+static void osdElementDisarmed(osdElementParms_t *element)
+{
+ if (!ARMING_FLAG(ARMED)) {
+ tfp_sprintf(element->buff, "DISARMED");
+ }
+}
+
+static void osdElementDisplayName(osdElementParms_t *element)
+{
+ // This does not strictly support iterative updating if the display name changes at run time. But since the display name is not supposed to be changing this should not matter, and blanking the entire length of the display name string on update will make it impossible to configure elements to be displayed on the right hand side of the display name.
+ //TODO: When iterative updating is implemented, change this so the display name is only printed once whenever the OSD 'flight' screen is entered.
+
+ if (strlen(pilotConfig()->displayName) == 0) {
+ strcpy(element->buff, "DISPLAY_NAME");
+ } else {
+ unsigned i;
+ for (i = 0; i < MAX_NAME_LENGTH; i++) {
+ if (pilotConfig()->displayName[i]) {
+ element->buff[i] = toupper((unsigned char)pilotConfig()->displayName[i]);
+ } else {
+ break;
+ }
+ }
+ element->buff[i] = '\0';
+ }
+}
+
+#ifdef USE_ESC_SENSOR
+static void osdElementEscTemperature(osdElementParms_t *element)
+{
+ if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
+ tfp_sprintf(element->buff, "%3d%c", osdConvertTemperatureToSelectedUnit(osdEscDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
+ }
+}
+
+static void osdElementEscRpm(osdElementParms_t *element)
+{
+ if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
+ tfp_sprintf(element->buff, "%5d", osdEscDataCombined == NULL ? 0 : calcEscRpm(osdEscDataCombined->rpm));
+ }
+}
+#endif // USE_ESC_SENSOR
+
+static void osdElementFlymode(osdElementParms_t *element)
+{
+ // Note that flight mode display has precedence in what to display.
+ // 1. FS
+ // 2. GPS RESCUE
+ // 3. ANGLE, HORIZON, ACRO TRAINER
+ // 4. AIR
+ // 5. ACRO
+
+ if (FLIGHT_MODE(FAILSAFE_MODE)) {
+ strcpy(element->buff, "!FS!");
+ } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+ strcpy(element->buff, "RESC");
+ } else if (FLIGHT_MODE(HEADFREE_MODE)) {
+ strcpy(element->buff, "HEAD");
+ } else if (FLIGHT_MODE(ANGLE_MODE)) {
+ strcpy(element->buff, "STAB");
+ } else if (FLIGHT_MODE(HORIZON_MODE)) {
+ strcpy(element->buff, "HOR ");
+ } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
+ strcpy(element->buff, "ATRN");
+ } else if (airmodeIsEnabled()) {
+ strcpy(element->buff, "AIR ");
+ } else {
+ strcpy(element->buff, "ACRO");
+ }
+}
+
+#ifdef USE_ACC
+static void osdElementGForce(osdElementParms_t *element)
+{
+ const int gForce = lrintf(osdGForce * 10);
+ tfp_sprintf(element->buff, "%01d.%01dG", gForce / 10, gForce % 10);
+}
+#endif // USE_ACC
+
+#ifdef USE_GPS
+static void osdElementGpsFlightDistance(osdElementParms_t *element)
+{
+ if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
+ const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceFlownInCm / 100);
+ tfp_sprintf(element->buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
+ } else {
+ // We use this symbol when we don't have a FIX
+ element->buff[0] = SYM_COLON;
+ // overwrite any previous distance with blanks
+ memset(element->buff + 1, SYM_BLANK, 6);
+ element->buff[7] = '\0';
+ }
+}
+
+static void osdElementGpsHomeDirection(osdElementParms_t *element)
+{
+ if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
+ if (GPS_distanceToHome > 0) {
+ const int h = GPS_directionToHome - DECIDEGREES_TO_DEGREES(attitude.values.yaw);
+ element->buff[0] = osdGetDirectionSymbolFromHeading(h);
+ } else {
+ // We don't have a HOME symbol in the font, by now we use this
+ element->buff[0] = SYM_THR1;
+ }
+
+ } else {
+ // We use this symbol when we don't have a FIX
+ element->buff[0] = SYM_COLON;
+ }
+
+ element->buff[1] = 0;
+}
+
+static void osdElementGpsHomeDistance(osdElementParms_t *element)
+{
+ if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
+ const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome);
+ tfp_sprintf(element->buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol());
+ } else {
+ // We use this symbol when we don't have a FIX
+ element->buff[0] = SYM_COLON;
+ // overwrite any previous distance with blanks
+ memset(element->buff + 1, SYM_BLANK, 6);
+ element->buff[7] = '\0';
+ }
+}
+
+static void osdElementGpsLatitude(osdElementParms_t *element)
+{
+ // The SYM_LAT symbol in the actual font contains only blank, so we use the SYM_ARROW_NORTH
+ osdFormatCoordinate(element->buff, SYM_ARROW_NORTH, gpsSol.llh.lat);
+}
+
+static void osdElementGpsLongitude(osdElementParms_t *element)
+{
+ // The SYM_LON symbol in the actual font contains only blank, so we use the SYM_ARROW_EAST
+ osdFormatCoordinate(element->buff, SYM_ARROW_EAST, gpsSol.llh.lon);
+}
+
+static void osdElementGpsSats(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat);
+}
+
+static void osdElementGpsSpeed(osdElementParms_t *element)
+{
+ // 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)
+ switch (osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ tfp_sprintf(element->buff, "%3dM", CM_S_TO_MPH(gpsSol.groundSpeed));
+ break;
+ default:
+ tfp_sprintf(element->buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
+ break;
+ }
+}
+#endif // USE_GPS
+
+static void osdElementHorizonSidebars(osdElementParms_t *element)
+{
+ // Draw AH sides
+ const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
+ const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
+ for (int y = -hudheight; y <= hudheight; y++) {
+ displayWriteChar(element->osdDisplayPort, element->elemPosX - hudwidth, element->elemPosY + y, SYM_AH_DECORATION);
+ displayWriteChar(element->osdDisplayPort, element->elemPosX + hudwidth, element->elemPosY + y, SYM_AH_DECORATION);
+ }
+
+ // AH level indicators
+ displayWriteChar(element->osdDisplayPort, element->elemPosX - hudwidth + 1, element->elemPosY, SYM_AH_LEFT);
+ displayWriteChar(element->osdDisplayPort, element->elemPosX + hudwidth - 1, element->elemPosY, SYM_AH_RIGHT);
+
+ element->drawElement = false; // element already drawn
+}
+
+#ifdef USE_RX_LINK_QUALITY_INFO
+static void osdElementLinkQuality(osdElementParms_t *element)
+{
+ // change range to 0-9 (two sig. fig. adds little extra value, also reduces screen estate)
+ uint8_t osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
+ if (osdLinkQuality >= 10) {
+ osdLinkQuality = 9;
+ }
+
+ tfp_sprintf(element->buff, "%1d", osdLinkQuality);
+}
+#endif // USE_RX_LINK_QUALITY_INFO
+
+#ifdef USE_BLACKBOX
+static void osdElementLogStatus(osdElementParms_t *element)
+{
+ if (!isBlackboxDeviceWorking()) {
+ tfp_sprintf(element->buff, "L-");
+ } else if (isBlackboxDeviceFull()) {
+ tfp_sprintf(element->buff, "L>");
+ } else {
+ tfp_sprintf(element->buff, "L%d", blackboxGetLogNumber());
+ }
+}
+#endif // USE_BLACKBOX
+
+static void osdElementMahDrawn(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%4d%c", getMAhDrawn(), SYM_MAH);
+}
+
+static void osdElementMainBatteryUsage(osdElementParms_t *element)
+{
+ // Set length of indicator bar
+ #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centered.
+
+ // Calculate constrained value
+ const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity);
+
+ // Calculate mAh used progress
+ const uint8_t mAhUsedProgress = ceilf((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS)));
+
+ // Create empty battery indicator bar
+ element->buff[0] = SYM_PB_START;
+ for (int i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) {
+ element->buff[i] = i <= mAhUsedProgress ? SYM_PB_FULL : SYM_PB_EMPTY;
+ }
+ element->buff[MAIN_BATT_USAGE_STEPS + 1] = SYM_PB_CLOSE;
+ if (mAhUsedProgress > 0 && mAhUsedProgress < MAIN_BATT_USAGE_STEPS) {
+ element->buff[1 + mAhUsedProgress] = SYM_PB_END;
+ }
+ element->buff[MAIN_BATT_USAGE_STEPS+2] = '\0';
+}
+
+static void osdElementMainBatteryVoltage(osdElementParms_t *element)
+{
+ element->buff[0] = osdGetBatterySymbol(getBatteryAverageCellVoltage());
+ tfp_sprintf(element->buff + 1, "%2d.%02d%c", getBatteryVoltage() / 100, getBatteryVoltage() % 100, SYM_VOLT);
+}
+
+static void osdElementMotorDiagnostics(osdElementParms_t *element)
+{
+ int i = 0;
+ const bool motorsRunning = areMotorsRunning();
+ for (; i < getMotorCount(); i++) {
+ if (motorsRunning) {
+ element->buff[i] = 0x88 - scaleRange(motor[i], motorOutputLow, motorOutputHigh, 0, 8);
+ } else {
+ element->buff[i] = 0x88;
+ }
+ }
+ element->buff[i] = '\0';
+}
+
+static void osdElementNumericalHeading(osdElementParms_t *element)
+{
+ const int heading = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
+ tfp_sprintf(element->buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
+}
+
+#ifdef USE_VARIO
+static void osdElementNumericalVario(osdElementParms_t *element)
+{
+ bool haveBaro = false;
+ bool haveGps = false;
+#ifdef USE_BARO
+ haveBaro = sensors(SENSOR_BARO);
+#endif // USE_BARO
+#ifdef USE_GPS
+ haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
+#endif // USE_GPS
+ if (haveBaro || haveGps) {
+ const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
+ const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SOUTH : SYM_ARROW_NORTH;
+ tfp_sprintf(element->buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
+ } else {
+ // We use this symbol when we don't have a valid measure
+ element->buff[0] = SYM_COLON;
+ // overwrite any previous vertical speed with blanks
+ memset(element->buff + 1, SYM_BLANK, 6);
+ element->buff[7] = '\0';
+ }
+}
+#endif // USE_VARIO
+
+static void osdElementPidRateProfile(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1);
+}
+
+static void osdElementPidsPitch(osdElementParms_t *element)
+{
+ osdFormatPID(element->buff, "PIT", ¤tPidProfile->pid[PID_PITCH]);
+}
+
+static void osdElementPidsRoll(osdElementParms_t *element)
+{
+ osdFormatPID(element->buff, "ROL", ¤tPidProfile->pid[PID_ROLL]);
+}
+
+static void osdElementPidsYaw(osdElementParms_t *element)
+{
+ osdFormatPID(element->buff, "YAW", ¤tPidProfile->pid[PID_YAW]);
+}
+
+static void osdElementPower(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%4dW", getAmperage() * getBatteryVoltage() / 10000);
+}
+
+static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
+{
+ const int mAhDrawn = getMAhDrawn();
+
+ if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) { // also handles the mAhDrawn == 0 condition
+ tfp_sprintf(element->buff, "--:--");
+ } else if (mAhDrawn > osdConfig()->cap_alarm) {
+ tfp_sprintf(element->buff, "00:00");
+ } else {
+ const int remaining_time = (int)((osdConfig()->cap_alarm - mAhDrawn) * ((float)osdFlyTime) / mAhDrawn);
+ osdFormatTime(element->buff, OSD_TIMER_PREC_SECOND, remaining_time);
+ }
+}
+
+static void osdElementRssi(osdElementParms_t *element)
+{
+ uint16_t osdRssi = getRssi() * 100 / 1024; // change range
+ if (osdRssi >= 100) {
+ osdRssi = 99;
+ }
+
+ tfp_sprintf(element->buff, "%c%2d", SYM_RSSI, osdRssi);
+}
+
+#ifdef USE_RTC_TIME
+static void osdElementRtcTime(osdElementParms_t *element)
+{
+ osdFormatRtcDateTime(&element->buff[0]);
+}
+#endif // USE_RTC_TIME
+
+#ifdef USE_OSD_STICK_OVERLAY
+static void osdElementStickOverlay(osdElementParms_t *element)
+{
+ const uint8_t xpos = element->elemPosX;
+ const uint8_t ypos = element->elemPosY;
+
+ // Draw the axis first
+ for (unsigned x = 0; x < OSD_STICK_OVERLAY_WIDTH; x++) {
+ for (unsigned y = 0; y < OSD_STICK_OVERLAY_HEIGHT; y++) {
+ // draw the axes, vertical and horizonal
+ if ((x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) && (y == (OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
+ displayWriteChar(element->osdDisplayPort, xpos + x, ypos + y, SYM_STICK_OVERLAY_CENTER);
+ } else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
+ displayWriteChar(element->osdDisplayPort, xpos + x, ypos + y, SYM_STICK_OVERLAY_VERTICAL);
+ } else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
+ displayWriteChar(element->osdDisplayPort, xpos + x, ypos + y, SYM_STICK_OVERLAY_HORIZONTAL);
+ }
+ }
+ }
+
+ // Now draw the cursor
+ rc_alias_e vertical_channel, horizontal_channel;
+
+ if (element->item == OSD_STICK_OVERLAY_LEFT) {
+ vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_vertical;
+ horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].left_horizontal;
+ } else {
+ vertical_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_vertical;
+ horizontal_channel = radioModes[osdConfig()->overlay_radio_mode-1].right_horizontal;
+ }
+
+ const uint8_t cursorX = scaleRange(constrain(rcData[horizontal_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_WIDTH);
+ const uint8_t cursorY = OSD_STICK_OVERLAY_VERTICAL_POSITIONS - 1 - scaleRange(constrain(rcData[vertical_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_VERTICAL_POSITIONS);
+ const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (cursorY % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
+
+ displayWriteChar(element->osdDisplayPort, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, cursor);
+
+ element->drawElement = false; // element already drawn
+}
+#endif // USE_OSD_STICK_OVERLAY
+
+static void osdElementThrottlePosition(osdElementParms_t *element)
+{
+ element->buff[0] = SYM_THR;
+ element->buff[1] = SYM_THR1;
+ tfp_sprintf(element->buff + 2, "%3d", calculateThrottlePercent());
+}
+
+static void osdElementTimer(osdElementParms_t *element)
+{
+ osdFormatTimer(element->buff, true, true, element->item - OSD_ITEM_TIMER_1);
+}
+
+#ifdef USE_VTX_COMMON
+static void osdElementVtxChannel(osdElementParms_t *element)
+{
+ const vtxDevice_t *vtxDevice = vtxCommonDevice();
+ const char vtxBandLetter = vtxCommonLookupBandLetter(vtxDevice, vtxSettingsConfig()->band);
+ const char *vtxChannelName = vtxCommonLookupChannelName(vtxDevice, vtxSettingsConfig()->channel);
+ uint8_t vtxPower = vtxSettingsConfig()->power;
+ if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
+ vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
+ }
+ tfp_sprintf(element->buff, "%c:%s:%1d", vtxBandLetter, vtxChannelName, vtxPower);
+}
+#endif // USE_VTX_COMMON
+
+static void osdElementWarnings(osdElementParms_t *element)
+{
+#define OSD_WARNINGS_MAX_SIZE 11
+#define OSD_FORMAT_MESSAGE_BUFFER_SIZE (OSD_WARNINGS_MAX_SIZE + 1)
+
+ STATIC_ASSERT(OSD_FORMAT_MESSAGE_BUFFER_SIZE <= OSD_ELEMENT_BUFFER_LENGTH, osd_warnings_size_exceeds_buffer_size);
+
+ const batteryState_e batteryState = getBatteryState();
+ const timeUs_t currentTimeUs = micros();
+
+ static timeUs_t armingDisabledUpdateTimeUs;
+ static unsigned armingDisabledDisplayIndex;
+
+ CLR_BLINK(OSD_WARNINGS);
+
+ // Cycle through the arming disabled reasons
+ if (osdWarnGetState(OSD_WARNING_ARMING_DISABLE)) {
+ if (IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) {
+ const armingDisableFlags_e armSwitchOnlyFlag = 1 << (ARMING_DISABLE_FLAGS_COUNT - 1);
+ armingDisableFlags_e flags = getArmingDisableFlags();
+
+ // Remove the ARMSWITCH flag unless it's the only one
+ if ((flags & armSwitchOnlyFlag) && (flags != armSwitchOnlyFlag)) {
+ flags -= armSwitchOnlyFlag;
+ }
+
+ // Rotate to the next arming disabled reason after a 0.5 second time delay
+ // or if the current flag is no longer set
+ if ((currentTimeUs - armingDisabledUpdateTimeUs > 5e5) || !(flags & (1 << armingDisabledDisplayIndex))) {
+ if (armingDisabledUpdateTimeUs == 0) {
+ armingDisabledDisplayIndex = ARMING_DISABLE_FLAGS_COUNT - 1;
+ }
+ armingDisabledUpdateTimeUs = currentTimeUs;
+
+ do {
+ if (++armingDisabledDisplayIndex >= ARMING_DISABLE_FLAGS_COUNT) {
+ armingDisabledDisplayIndex = 0;
+ }
+ } while (!(flags & (1 << armingDisabledDisplayIndex)));
+ }
+
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDisableFlagNames[armingDisabledDisplayIndex]);
+ return;
+ } else {
+ armingDisabledUpdateTimeUs = 0;
+ }
+ }
+
+#ifdef USE_DSHOT
+ if (isTryingToArm() && !ARMING_FLAG(ARMED)) {
+ int armingDelayTime = (getLastDshotBeaconCommandTimeUs() + DSHOT_BEACON_GUARD_DELAY_US - currentTimeUs) / 1e5;
+ if (armingDelayTime < 0) {
+ armingDelayTime = 0;
+ }
+ if (armingDelayTime >= (DSHOT_BEACON_GUARD_DELAY_US / 1e5 - 5)) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " BEACON ON"); // Display this message for the first 0.5 seconds
+ } else {
+ char armingDelayMessage[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
+ tfp_sprintf(armingDelayMessage, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, armingDelayMessage);
+ }
+ return;
+ }
+#endif // USE_DSHOT
+ if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "FAIL SAFE");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+
+ // Warn when in flip over after crash mode
+ if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashActive()) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "CRASH FLIP");
+ return;
+ }
+
+#ifdef USE_LAUNCH_CONTROL
+ // Warn when in launch control mode
+ if (osdWarnGetState(OSD_WARNING_LAUNCH_CONTROL) && isLaunchControlActive()) {
+#ifdef USE_ACC
+ if (sensors(SENSOR_ACC)) {
+ char launchControlMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
+ const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
+ tfp_sprintf(launchControlMsg, "LAUNCH %d", pitchAngle);
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, launchControlMsg);
+ } else
+#endif // USE_ACC
+ {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LAUNCH");
+ }
+ return;
+ }
+#endif // USE_LAUNCH_CONTROL
+
+ if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " LAND NOW");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+
+#ifdef USE_GPS_RESCUE
+ if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
+ ARMING_FLAG(ARMED) &&
+ gpsRescueIsConfigured() &&
+ !gpsRescueIsDisabled() &&
+ !gpsRescueIsAvailable()) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+
+ if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_DISABLED) &&
+ ARMING_FLAG(ARMED) &&
+ gpsRescueIsConfigured() &&
+ gpsRescueIsDisabled()) {
+
+ statistic_t *stats = osdGetStats();
+ if (!cmpTimeUs(stats->armed_time, OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US)) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RESC OFF!!!");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+ }
+
+#endif // USE_GPS_RESCUE
+
+ // Show warning if in HEADFREE flight mode
+ if (FLIGHT_MODE(HEADFREE_MODE)) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "HEADFREE");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+
+#ifdef USE_ADC_INTERNAL
+ const int16_t coreTemperature = getCoreTemperatureCelsius();
+ if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
+ char coreTemperatureWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
+ tfp_sprintf(coreTemperatureWarningMsg, "CORE: %3d%c", osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
+
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, coreTemperatureWarningMsg);
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+#endif // USE_ADC_INTERNAL
+
+#ifdef USE_ESC_SENSOR
+ // Show warning if we lose motor output, the ESC is overheating or excessive current draw
+ if (featureIsEnabled(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
+ char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
+ unsigned pos = 0;
+
+ const char *title = "ESC";
+
+ // center justify message
+ while (pos < (OSD_WARNINGS_MAX_SIZE - (strlen(title) + getMotorCount())) / 2) {
+ escWarningMsg[pos++] = ' ';
+ }
+
+ strcpy(escWarningMsg + pos, title);
+ pos += strlen(title);
+
+ unsigned i = 0;
+ unsigned escWarningCount = 0;
+ while (i < getMotorCount() && pos < OSD_FORMAT_MESSAGE_BUFFER_SIZE - 1) {
+ escSensorData_t *escData = getEscSensorData(i);
+ const char motorNumber = '1' + i;
+ // if everything is OK just display motor number else R, T or C
+ char warnFlag = motorNumber;
+ if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
+ warnFlag = 'R';
+ }
+ if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
+ warnFlag = 'T';
+ }
+ if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF && escData->current >= osdConfig()->esc_current_alarm) {
+ warnFlag = 'C';
+ }
+
+ escWarningMsg[pos++] = warnFlag;
+
+ if (warnFlag != motorNumber) {
+ escWarningCount++;
+ }
+
+ i++;
+ }
+
+ escWarningMsg[pos] = '\0';
+
+ if (escWarningCount > 0) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, escWarningMsg);
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+ }
+#endif // USE_ESC_SENSOR
+
+ if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+
+#ifdef USE_RC_SMOOTHING_FILTER
+ // Show warning if rc smoothing hasn't initialized the filters
+ if (osdWarnGetState(OSD_WARNING_RC_SMOOTHING) && ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "RCSMOOTHING");
+ SET_BLINK(OSD_WARNINGS);
+ return;
+ }
+#endif // USE_RC_SMOOTHING_FILTER
+
+ // Show warning if battery is not fresh
+ if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK)
+ && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "BATT < FULL");
+ return;
+ }
+
+ // Visual beeper
+ if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && osdGetVisualBeeperState()) {
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, " * * * *");
+ return;
+ }
+
+ osdFormatMessage(element->buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, NULL);
+}
+
+// Define the order in which the elements are drawn.
+// Elements positioned later in the list will overlay the earlier
+// ones if their character positions overlap
+// Elements that need special runtime conditional processing should be added
+// to osdAnalyzeActiveElements()
+
+static const uint8_t osdElementDisplayOrder[] = {
+ OSD_MAIN_BATT_VOLTAGE,
+ OSD_RSSI_VALUE,
+ OSD_CROSSHAIRS,
+ OSD_HORIZON_SIDEBARS,
+ OSD_ITEM_TIMER_1,
+ OSD_ITEM_TIMER_2,
+ OSD_REMAINING_TIME_ESTIMATE,
+ OSD_FLYMODE,
+ OSD_THROTTLE_POS,
+ OSD_VTX_CHANNEL,
+ OSD_CURRENT_DRAW,
+ OSD_MAH_DRAWN,
+ OSD_CRAFT_NAME,
+ OSD_ALTITUDE,
+ OSD_ROLL_PIDS,
+ OSD_PITCH_PIDS,
+ OSD_YAW_PIDS,
+ OSD_POWER,
+ OSD_PIDRATE_PROFILE,
+ OSD_WARNINGS,
+ OSD_AVG_CELL_VOLTAGE,
+ OSD_DEBUG,
+ OSD_PITCH_ANGLE,
+ OSD_ROLL_ANGLE,
+ OSD_MAIN_BATT_USAGE,
+ OSD_DISARMED,
+ OSD_NUMERICAL_HEADING,
+ OSD_NUMERICAL_VARIO,
+ OSD_COMPASS_BAR,
+ OSD_ANTI_GRAVITY,
+ OSD_MOTOR_DIAG,
+#ifdef USE_ACC
+ OSD_FLIP_ARROW,
+#endif
+ OSD_DISPLAY_NAME,
+#ifdef USE_RTC_TIME
+ OSD_RTC_DATETIME,
+#endif
+#ifdef USE_OSD_ADJUSTMENTS
+ OSD_ADJUSTMENT_RANGE,
+#endif
+#ifdef USE_ADC_INTERNAL
+ OSD_CORE_TEMPERATURE,
+#endif
+#ifdef USE_RX_LINK_QUALITY_INFO
+ OSD_LINK_QUALITY,
+#endif
+#ifdef USE_OSD_STICK_OVERLAY
+ OSD_STICK_OVERLAY_LEFT,
+ OSD_STICK_OVERLAY_RIGHT,
+#endif
+};
+
+// Define the mapping between the OSD element id and the function to draw it
+
+const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
+ [OSD_RSSI_VALUE] = osdElementRssi,
+ [OSD_MAIN_BATT_VOLTAGE] = osdElementMainBatteryVoltage,
+ [OSD_CROSSHAIRS] = osdElementCrosshairs,
+#ifdef USE_ACC
+ [OSD_ARTIFICIAL_HORIZON] = osdElementArtificialHorizon,
+#endif
+ [OSD_HORIZON_SIDEBARS] = osdElementHorizonSidebars,
+ [OSD_ITEM_TIMER_1] = osdElementTimer,
+ [OSD_ITEM_TIMER_2] = osdElementTimer,
+ [OSD_FLYMODE] = osdElementFlymode,
+ [OSD_CRAFT_NAME] = osdElementCraftName,
+ [OSD_THROTTLE_POS] = osdElementThrottlePosition,
+#ifdef USE_VTX_COMMON
+ [OSD_VTX_CHANNEL] = osdElementVtxChannel,
+#endif
+ [OSD_CURRENT_DRAW] = osdElementCurrentDraw,
+ [OSD_MAH_DRAWN] = osdElementMahDrawn,
+#ifdef USE_GPS
+ [OSD_GPS_SPEED] = osdElementGpsSpeed,
+ [OSD_GPS_SATS] = osdElementGpsSats,
+#endif
+ [OSD_ALTITUDE] = osdElementAltitude,
+ [OSD_ROLL_PIDS] = osdElementPidsRoll,
+ [OSD_PITCH_PIDS] = osdElementPidsPitch,
+ [OSD_YAW_PIDS] = osdElementPidsYaw,
+ [OSD_POWER] = osdElementPower,
+ [OSD_PIDRATE_PROFILE] = osdElementPidRateProfile,
+ [OSD_WARNINGS] = osdElementWarnings,
+ [OSD_AVG_CELL_VOLTAGE] = osdElementAverageCellVoltage,
+#ifdef USE_GPS
+ [OSD_GPS_LON] = osdElementGpsLongitude,
+ [OSD_GPS_LAT] = osdElementGpsLatitude,
+#endif
+ [OSD_DEBUG] = osdElementDebug,
+#ifdef USE_ACC
+ [OSD_PITCH_ANGLE] = osdElementAngleRollPitch,
+ [OSD_ROLL_ANGLE] = osdElementAngleRollPitch,
+#endif
+ [OSD_MAIN_BATT_USAGE] = osdElementMainBatteryUsage,
+ [OSD_DISARMED] = osdElementDisarmed,
+#ifdef USE_GPS
+ [OSD_HOME_DIR] = osdElementGpsHomeDirection,
+ [OSD_HOME_DIST] = osdElementGpsHomeDistance,
+#endif
+ [OSD_NUMERICAL_HEADING] = osdElementNumericalHeading,
+#ifdef USE_VARIO
+ [OSD_NUMERICAL_VARIO] = osdElementNumericalVario,
+#endif
+ [OSD_COMPASS_BAR] = osdElementCompassBar,
+#ifdef USE_ESC_SENSOR
+ [OSD_ESC_TMP] = osdElementEscTemperature,
+ [OSD_ESC_RPM] = osdElementEscRpm,
+#endif
+ [OSD_REMAINING_TIME_ESTIMATE] = osdElementRemainingTimeEstimate,
+#ifdef USE_RTC_TIME
+ [OSD_RTC_DATETIME] = osdElementRtcTime,
+#endif
+#ifdef USE_OSD_ADJUSTMENTS
+ [OSD_ADJUSTMENT_RANGE] = osdElementAdjustmentRange,
+#endif
+#ifdef USE_ADC_INTERNAL
+ [OSD_CORE_TEMPERATURE] = osdElementCoreTemperature,
+#endif
+ [OSD_ANTI_GRAVITY] = osdElementAntiGravity,
+#ifdef USE_ACC
+ [OSD_G_FORCE] = osdElementGForce,
+#endif
+ [OSD_MOTOR_DIAG] = osdElementMotorDiagnostics,
+#ifdef USE_BLACKBOX
+ [OSD_LOG_STATUS] = osdElementLogStatus,
+#endif
+#ifdef USE_ACC
+ [OSD_FLIP_ARROW] = osdElementCrashFlipArrow,
+#endif
+#ifdef USE_RX_LINK_QUALITY_INFO
+ [OSD_LINK_QUALITY] = osdElementLinkQuality,
+#endif
+#ifdef USE_GPS
+ [OSD_FLIGHT_DIST] = osdElementGpsFlightDistance,
+#endif
+#ifdef USE_OSD_STICK_OVERLAY
+ [OSD_STICK_OVERLAY_LEFT] = osdElementStickOverlay,
+ [OSD_STICK_OVERLAY_RIGHT] = osdElementStickOverlay,
+#endif
+ [OSD_DISPLAY_NAME] = osdElementDisplayName,
+};
+
+void osdFormatAltitudeString(char * buff, int32_t altitudeCm)
+{
+ const int alt = osdGetMetersToSelectedUnit(altitudeCm) / 10;
+
+ tfp_sprintf(buff, "%5d %c", alt, osdGetMetersToSelectedUnitSymbol());
+ buff[5] = buff[4];
+ buff[4] = '.';
+}
+
+static void osdAddActiveElement(osd_items_e element)
+{
+ if (VISIBLE(osdConfig()->item_pos[element])) {
+ activeOsdElementArray[activeOsdElementCount++] = element;
+ }
+}
+
+// Examine the elements and build a list of only the active (enabled)
+// ones to speed up rendering.
+
+void osdAnalyzeActiveElements(void)
+{
+ activeOsdElementCount = 0;
+
+#ifdef USE_ACC
+ if (sensors(SENSOR_ACC)) {
+ osdAddActiveElement(OSD_ARTIFICIAL_HORIZON);
+ osdAddActiveElement(OSD_G_FORCE);
+ }
+#endif
+
+ for (unsigned i = 0; i < sizeof(osdElementDisplayOrder); i++) {
+ osdAddActiveElement(osdElementDisplayOrder[i]);
+ }
+
+#ifdef USE_GPS
+ if (sensors(SENSOR_GPS)) {
+ osdAddActiveElement(OSD_GPS_SATS);
+ osdAddActiveElement(OSD_GPS_SPEED);
+ osdAddActiveElement(OSD_GPS_LAT);
+ osdAddActiveElement(OSD_GPS_LON);
+ osdAddActiveElement(OSD_HOME_DIST);
+ osdAddActiveElement(OSD_HOME_DIR);
+ osdAddActiveElement(OSD_FLIGHT_DIST);
+ }
+#endif // GPS
+
+#ifdef USE_ESC_SENSOR
+ if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
+ osdAddActiveElement(OSD_ESC_TMP);
+ osdAddActiveElement(OSD_ESC_RPM);
+ }
+#endif
+
+#ifdef USE_BLACKBOX
+ if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
+ osdAddActiveElement(OSD_LOG_STATUS);
+ }
+#endif
+}
+
+static bool osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
+{
+ if (BLINK(item)) {
+ return false;
+ }
+
+ uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
+ uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
+ char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
+
+ osdElementParms_t element;
+ element.item = item;
+ element.elemPosX = elemPosX;
+ element.elemPosY = elemPosY;
+ element.buff = (char *)&buff;
+ element.osdDisplayPort = osdDisplayPort;
+ element.drawElement = true;
+
+ // Call the element drawing function
+ osdElementDrawFunction[item](&element);
+ if (element.drawElement) {
+ displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
+ }
+
+ return true;
+}
+
+void osdDrawActiveElements(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs)
+{
+ blinkState = (currentTimeUs / 200000) % 2;
+
+ for (unsigned i = 0; i < activeOsdElementCount; i++) {
+ osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[i]);
+ }
+}
+
+void osdResetAlarms(void)
+{
+ memset(blinkBits, 0, sizeof(blinkBits));
+}
+
+void osdUpdateAlarms(void)
+{
+ // This is overdone?
+
+ int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
+
+ if (getRssiPercent() < osdConfig()->rssi_alarm) {
+ SET_BLINK(OSD_RSSI_VALUE);
+ } else {
+ CLR_BLINK(OSD_RSSI_VALUE);
+ }
+
+ if (getBatteryState() == BATTERY_OK) {
+ CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
+ CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
+ } else {
+ SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
+ SET_BLINK(OSD_AVG_CELL_VOLTAGE);
+ }
+
+#ifdef USE_GPS
+ if ((STATE(GPS_FIX) == 0) || (gpsSol.numSat < 5)
+#ifdef USE_GPS_RESCUE
+ || ((gpsSol.numSat < gpsRescueConfig()->minSats) && gpsRescueIsConfigured())
+#endif
+ ) {
+ SET_BLINK(OSD_GPS_SATS);
+ } else {
+ CLR_BLINK(OSD_GPS_SATS);
+ }
+#endif //USE_GPS
+
+ for (int i = 0; i < OSD_TIMER_COUNT; i++) {
+ const uint16_t timer = osdConfig()->timers[i];
+ const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer));
+ const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us
+ if (alarmTime != 0 && time >= alarmTime) {
+ SET_BLINK(OSD_ITEM_TIMER_1 + i);
+ } else {
+ CLR_BLINK(OSD_ITEM_TIMER_1 + i);
+ }
+ }
+
+ if (getMAhDrawn() >= osdConfig()->cap_alarm) {
+ SET_BLINK(OSD_MAH_DRAWN);
+ SET_BLINK(OSD_MAIN_BATT_USAGE);
+ SET_BLINK(OSD_REMAINING_TIME_ESTIMATE);
+ } else {
+ CLR_BLINK(OSD_MAH_DRAWN);
+ CLR_BLINK(OSD_MAIN_BATT_USAGE);
+ CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE);
+ }
+
+ if (alt >= osdConfig()->alt_alarm) {
+ SET_BLINK(OSD_ALTITUDE);
+ } else {
+ CLR_BLINK(OSD_ALTITUDE);
+ }
+
+#ifdef USE_ESC_SENSOR
+ if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
+ // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs
+ if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm) {
+ SET_BLINK(OSD_ESC_TMP);
+ } else {
+ CLR_BLINK(OSD_ESC_TMP);
+ }
+ }
+#endif
+}
+
+#endif // USE_OSD
diff --git a/src/main/osd/osd_elements.h b/src/main/osd/osd_elements.h
new file mode 100644
index 0000000000..8f2650c848
--- /dev/null
+++ b/src/main/osd/osd_elements.h
@@ -0,0 +1,49 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "drivers/display.h"
+
+#include "osd/osd.h"
+
+typedef struct osdElementParms_s {
+ uint8_t item;
+ uint8_t elemPosX;
+ uint8_t elemPosY;
+ char *buff;
+ displayPort_t *osdDisplayPort;
+ bool drawElement;
+} osdElementParms_t;
+
+typedef void (*osdElementDrawFn)(osdElementParms_t *element);
+
+int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius);
+void osdFormatAltitudeString(char * buff, int32_t altitudeCm);
+bool osdFormatRtcDateTime(char *buffer);
+void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time);
+void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex);
+int32_t osdGetMetersToSelectedUnit(int32_t meters);
+char osdGetMetersToSelectedUnitSymbol(void);
+char osdGetTemperatureSymbolForSelectedUnit(void);
+void osdAnalyzeActiveElements(void);
+void osdDrawActiveElements(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs);
+void osdResetAlarms(void);
+void osdUpdateAlarms(void);
diff --git a/src/main/target/ALIENWHOOP/target.mk b/src/main/target/ALIENWHOOP/target.mk
index f23a6d92bf..9e8c676cfd 100644
--- a/src/main/target/ALIENWHOOP/target.mk
+++ b/src/main/target/ALIENWHOOP/target.mk
@@ -24,5 +24,4 @@ TARGET_SRC = \
drivers/compass/compass_ak8963.c \
drivers/compass/compass_lis3mdl.c \
drivers/flash_m25p16.c \
- drivers/max7456.c \
- io/osd.c
+ drivers/max7456.c
diff --git a/src/main/target/BEEBRAIN_V2F/config.c b/src/main/target/BEEBRAIN_V2F/config.c
index 13ed08efe1..80976b7cfa 100644
--- a/src/main/target/BEEBRAIN_V2F/config.c
+++ b/src/main/target/BEEBRAIN_V2F/config.c
@@ -48,7 +48,8 @@
#include "rx/rx.h"
#include "io/serial.h"
-#include "io/osd.h"
+
+#include "osd/osd.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
diff --git a/src/main/target/BETAFLIGHTF3/target.mk b/src/main/target/BETAFLIGHTF3/target.mk
index e4fa3617cc..e430e1719c 100644
--- a/src/main/target/BETAFLIGHTF3/target.mk
+++ b/src/main/target/BETAFLIGHTF3/target.mk
@@ -11,5 +11,4 @@ TARGET_SRC = \
drivers/compass/compass_hmc5883l.c \
drivers/display_ug2864hsweg01.h \
drivers/flash_m25p16.c \
- drivers/max7456.c \
- io/osd.c
+ drivers/max7456.c
diff --git a/src/main/target/KIWIF4/target.mk b/src/main/target/KIWIF4/target.mk
index 0fb93c0b75..6964b0822b 100644
--- a/src/main/target/KIWIF4/target.mk
+++ b/src/main/target/KIWIF4/target.mk
@@ -3,5 +3,4 @@ FEATURES += VCP ONBOARDFLASH SDCARD_SPI
TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
- drivers/max7456.c \
- io/osd.c
+ drivers/max7456.c
diff --git a/src/main/target/KROOZX/config.c b/src/main/target/KROOZX/config.c
index 0111f38400..f0d25cf783 100644
--- a/src/main/target/KROOZX/config.c
+++ b/src/main/target/KROOZX/config.c
@@ -25,7 +25,7 @@
#include "common/axis.h"
#include "drivers/io.h"
-#include "io/osd.h"
+#include "osd/osd.h"
#include "pg/pinio.h"
#include "sensors/battery.h"
#include "sensors/barometer.h"
diff --git a/src/main/target/RACEBASE/target.mk b/src/main/target/RACEBASE/target.mk
index c52ecf7936..fd5eb47b3d 100644
--- a/src/main/target/RACEBASE/target.mk
+++ b/src/main/target/RACEBASE/target.mk
@@ -8,5 +8,4 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu.c \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/flash_m25p16.c \
- drivers/max7456.c \
- io/osd.c
+ drivers/max7456.c
diff --git a/src/main/target/SPRACINGF4NEO/target.mk b/src/main/target/SPRACINGF4NEO/target.mk
index 11b2828f79..0020ee27dd 100644
--- a/src/main/target/SPRACINGF4NEO/target.mk
+++ b/src/main/target/SPRACINGF4NEO/target.mk
@@ -10,5 +10,4 @@ TARGET_SRC = \
drivers/compass/compass_hmc5883l.c \
drivers/compass/compass_qmc5883l.c \
drivers/max7456.c \
- drivers/vtx_rtc6705.c \
- io/osd.c \
+ drivers/vtx_rtc6705.c
diff --git a/src/main/target/WORMFC/target.mk b/src/main/target/WORMFC/target.mk
index 4fa1a426ed..705233bf90 100644
--- a/src/main/target/WORMFC/target.mk
+++ b/src/main/target/WORMFC/target.mk
@@ -6,5 +6,4 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/barometer/barometer_lps.c \
- drivers/max7456.c \
- io/osd.c
+ drivers/max7456.c
\ No newline at end of file
diff --git a/src/main/target/YUPIF4/target.mk b/src/main/target/YUPIF4/target.mk
index 341ae64ee0..2191e10c79 100644
--- a/src/main/target/YUPIF4/target.mk
+++ b/src/main/target/YUPIF4/target.mk
@@ -5,5 +5,4 @@ TARGET_SRC = \
drivers/accgyro/accgyro_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_icm20689.c\
- drivers/max7456.c \
- io/osd.c
+ drivers/max7456.c
diff --git a/src/test/Makefile b/src/test/Makefile
index 9b408adfd6..dd974d5325 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -166,7 +166,8 @@ maths_unittest_SRC := \
osd_unittest_SRC := \
- $(USER_DIR)/io/osd.c \
+ $(USER_DIR)/osd/osd.c \
+ $(USER_DIR)/osd/osd_elements.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/common/maths.c \
diff --git a/src/test/unit/cli_unittest.cc b/src/test/unit/cli_unittest.cc
index f32616685c..318ae05aab 100644
--- a/src/test/unit/cli_unittest.cc
+++ b/src/test/unit/cli_unittest.cc
@@ -40,11 +40,11 @@ extern "C" {
#include "flight/servos.h"
#include "io/beeper.h"
#include "io/ledstrip.h"
- #include "io/osd.h"
#include "io/serial.h"
#include "io/vtx.h"
#include "msp/msp.h"
#include "msp/msp_box.h"
+ #include "osd/osd.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/beeper.h"
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index 225c987806..a0db2cc9ea 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -52,7 +52,9 @@ extern "C" {
#include "io/beeper.h"
#include "io/gps.h"
- #include "io/osd.h"
+
+ #include "osd/osd.h"
+ #include "osd/osd_elements.h"
#include "sensors/acceleration.h"
#include "sensors/battery.h"
@@ -62,7 +64,6 @@ extern "C" {
void osdRefresh(timeUs_t currentTimeUs);
void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time);
- void osdFormatTimer(char *buff, bool showSymbol, int timerIndex);
int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius);
uint16_t rssi;
@@ -481,6 +482,8 @@ TEST(OsdTest, TestAlarms)
osdConfigMutable()->cap_alarm = 2200;
osdConfigMutable()->alt_alarm = 100; // meters
+ osdAnalyzeActiveElements();
+
// and
// this timer 1 configuration
osdConfigMutable()->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_HUNDREDTHS, 2);
@@ -562,6 +565,8 @@ TEST(OsdTest, TestElementRssi)
osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
osdConfigMutable()->rssi_alarm = 0;
+ osdAnalyzeActiveElements();
+
// when
rssi = 1024;
displayClearScreen(&testDisplayPort);
@@ -595,6 +600,8 @@ TEST(OsdTest, TestElementAmperage)
// given
osdConfigMutable()->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 12) | OSD_PROFILE_1_FLAG;
+ osdAnalyzeActiveElements();
+
// when
simulationBatteryAmperage = 0;
displayClearScreen(&testDisplayPort);
@@ -628,6 +635,8 @@ TEST(OsdTest, TestElementMahDrawn)
// given
osdConfigMutable()->item_pos[OSD_MAH_DRAWN] = OSD_POS(1, 11) | OSD_PROFILE_1_FLAG;
+ osdAnalyzeActiveElements();
+
// when
simulationMahDrawn = 0;
displayClearScreen(&testDisplayPort);
@@ -677,6 +686,8 @@ TEST(OsdTest, TestElementPower)
// given
osdConfigMutable()->item_pos[OSD_POWER] = OSD_POS(1, 10) | OSD_PROFILE_1_FLAG;
+ osdAnalyzeActiveElements();
+
// and
simulationBatteryVoltage = 1000; // 10V
@@ -739,6 +750,8 @@ TEST(OsdTest, TestElementAltitude)
// given
osdConfigMutable()->item_pos[OSD_ALTITUDE] = OSD_POS(23, 7) | OSD_PROFILE_1_FLAG;
+ osdAnalyzeActiveElements();
+
// and
osdConfigMutable()->units = OSD_UNIT_METRIC;
sensorsClear(SENSOR_GPS);
@@ -800,6 +813,8 @@ TEST(OsdTest, TestElementCoreTemperature)
// given
osdConfigMutable()->item_pos[OSD_CORE_TEMPERATURE] = OSD_POS(1, 8) | OSD_PROFILE_1_FLAG;
+ osdAnalyzeActiveElements();
+
// and
osdConfigMutable()->units = OSD_UNIT_METRIC;
@@ -846,6 +861,8 @@ TEST(OsdTest, TestElementWarningsBattery)
osdWarnSetState(OSD_WARNING_BATTERY_CRITICAL, true);
osdWarnSetState(OSD_WARNING_BATTERY_NOT_FULL, true);
+ osdAnalyzeActiveElements();
+
// and
batteryConfigMutable()->vbatfullcellvoltage = 410;
diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc
index b38390acb5..8669640d54 100644
--- a/src/test/unit/rcdevice_unittest.cc
+++ b/src/test/unit/rcdevice_unittest.cc
@@ -40,9 +40,10 @@ extern "C" {
#include "scheduler/scheduler.h"
#include "io/rcdevice_cam.h"
- #include "io/osd.h"
#include "io/rcdevice.h"
+ #include "osd/osd.h"
+
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/vcd.h"