diff --git a/make/source.mk b/make/source.mk
index a70ff9564d..cef0117e76 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -177,6 +177,7 @@ COMMON_SRC = \
io/pidaudio.c \
osd/osd.c \
osd/osd_elements.c \
+ osd/osd_warnings.c \
sensors/barometer.c \
sensors/rangefinder.c \
telemetry/telemetry.c \
@@ -355,6 +356,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
io/spektrum_vtx_control.c \
osd/osd.c \
osd/osd_elements.c \
+ osd/osd_warnings.c \
rx/rx_bind.c
# Gyro driver files that only contain initialization and configuration code - not runtime code
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 12f7893896..352a692fd8 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -112,6 +112,7 @@
#include "osd/osd.h"
#include "osd/osd_elements.h"
+#include "osd/osd_warnings.h"
#include "pg/beeper.h"
#include "pg/board.h"
@@ -1211,6 +1212,28 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
break;
#endif
+#ifdef USE_OSD
+ case MSP2_GET_OSD_WARNINGS:
+ {
+ bool isBlinking;
+ uint8_t displayAttr;
+ char warningsBuffer[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
+
+ renderOsdWarning(warningsBuffer, &isBlinking, &displayAttr);
+ const uint8_t warningsLen = strlen(warningsBuffer);
+
+ if (isBlinking) {
+ displayAttr |= DISPLAYPORT_ATTR_BLINK;
+ }
+ sbufWriteU8(dst, displayAttr); // see displayPortAttr_e
+ sbufWriteU8(dst, warningsLen); // length byte followed by the actual characters
+ for (unsigned i = 0; i < warningsLen; i++) {
+ sbufWriteU8(dst, warningsBuffer[i]);
+ }
+ break;
+ }
+#endif
+
case MSP_RC:
for (int i = 0; i < rxRuntimeState.channelCount; i++) {
sbufWriteU16(dst, rcData[i]);
diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h
index 9c1d938f35..f4dceb6ffd 100644
--- a/src/main/msp/msp_protocol_v2_betaflight.h
+++ b/src/main/msp/msp_protocol_v2_betaflight.h
@@ -23,3 +23,4 @@
#define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002
#define MSP2_SEND_DSHOT_COMMAND 0x3003
#define MSP2_GET_VTX_DEVICE_STATUS 0x3004
+#define MSP2_GET_OSD_WARNINGS 0x3005 // returns active OSD warning message text
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 0745436eee..f5b14ccd7e 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -96,30 +96,26 @@
#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 "osd/osd_warnings.h"
#include "pg/motor.h"
#include "pg/stats.h"
#include "rx/rx.h"
-#include "sensors/acceleration.h"
#include "sensors/adcinternal.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
@@ -1348,278 +1344,13 @@ static void osdElementVtxChannel(osdElementParms_t *element)
static void osdElementWarnings(osdElementParms_t *element)
{
-#define OSD_WARNINGS_MAX_SIZE 12
-#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)));
- }
-
- tfp_sprintf(element->buff, "%s", armingDisableFlagNames[armingDisabledDisplayIndex]);
- element->attr = DISPLAYPORT_ATTR_WARNING;
- 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)) {
- tfp_sprintf(element->buff, " BEACON ON"); // Display this message for the first 0.5 seconds
- } else {
- tfp_sprintf(element->buff, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
- }
- element->attr = DISPLAYPORT_ATTR_INFO;
- return;
- }
-#endif // USE_DSHOT
- if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
- tfp_sprintf(element->buff, "FAIL SAFE");
- element->attr = DISPLAYPORT_ATTR_CRITICAL;
+ bool elementBlinking = false;
+ renderOsdWarning(element->buff, &elementBlinking, &element->attr);
+ if (elementBlinking) {
SET_BLINK(OSD_WARNINGS);
- return;
+ } else {
+ CLR_BLINK(OSD_WARNINGS);
}
-
- // Warn when in flip over after crash mode
- if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashActive()) {
- tfp_sprintf(element->buff, "CRASH FLIP");
- element->attr = DISPLAYPORT_ATTR_INFO;
- 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)) {
- const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
- tfp_sprintf(element->buff, "LAUNCH %d", pitchAngle);
- } else
-#endif // USE_ACC
- {
- tfp_sprintf(element->buff, "LAUNCH");
- }
-
- // Blink the message if the throttle is within 10% of the launch setting
- if ( calculateThrottlePercent() >= MAX(currentPidProfile->launchControlThrottlePercent - 10, 0)) {
- SET_BLINK(OSD_WARNINGS);
- }
-
- element->attr = DISPLAYPORT_ATTR_INFO;
- return;
- }
-#endif // USE_LAUNCH_CONTROL
-
- // RSSI
- if (osdWarnGetState(OSD_WARNING_RSSI) && (getRssiPercent() < osdConfig()->rssi_alarm)) {
- tfp_sprintf(element->buff, "RSSI LOW");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
-#ifdef USE_RX_RSSI_DBM
- // rssi dbm
- if (osdWarnGetState(OSD_WARNING_RSSI_DBM) && (getRssiDbm() < osdConfig()->rssi_dbm_alarm)) {
- tfp_sprintf(element->buff, "RSSI DBM");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
-#endif // USE_RX_RSSI_DBM
-
-#ifdef USE_RX_LINK_QUALITY_INFO
- // Link Quality
- if (osdWarnGetState(OSD_WARNING_LINK_QUALITY) && (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm)) {
- tfp_sprintf(element->buff, "LINK QUALITY");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
-#endif // USE_RX_LINK_QUALITY_INFO
-
- if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
- tfp_sprintf(element->buff, " LAND NOW");
- element->attr = DISPLAYPORT_ATTR_CRITICAL;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
-
-#ifdef USE_GPS_RESCUE
- if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
- ARMING_FLAG(ARMED) &&
- gpsRescueIsConfigured() &&
- !gpsRescueIsDisabled() &&
- !gpsRescueIsAvailable()) {
- tfp_sprintf(element->buff, "RESCUE N/A");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- 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) < 0) {
- tfp_sprintf(element->buff, "RESCUE OFF");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
- }
-
-#endif // USE_GPS_RESCUE
-
- // Show warning if in HEADFREE flight mode
- if (FLIGHT_MODE(HEADFREE_MODE)) {
- tfp_sprintf(element->buff, "HEADFREE");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- 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) {
- tfp_sprintf(element->buff, "CORE %c: %3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
- element->attr = DISPLAYPORT_ATTR_WARNING;
- 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) {
- tfp_sprintf(element->buff, "%s", escWarningMsg);
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
- }
-#endif // USE_ESC_SENSOR
-
- if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
- tfp_sprintf(element->buff, "LOW BATTERY");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- 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()) {
- tfp_sprintf(element->buff, "RCSMOOTHING");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
-#endif // USE_RC_SMOOTHING_FILTER
-
- // Show warning if mah consumed is over the configured limit
- if (osdWarnGetState(OSD_WARNING_OVER_CAP) && ARMING_FLAG(ARMED) && osdConfig()->cap_alarm > 0 && getMAhDrawn() >= osdConfig()->cap_alarm) {
- tfp_sprintf(element->buff, "OVER CAP");
- element->attr = DISPLAYPORT_ATTR_WARNING;
- SET_BLINK(OSD_WARNINGS);
- return;
- }
-
- // Show warning if battery is not fresh
- if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !(ARMING_FLAG(ARMED) || ARMING_FLAG(WAS_EVER_ARMED)) && (getBatteryState() == BATTERY_OK)
- && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
- tfp_sprintf(element->buff, "BATT < FULL");
- element->attr = DISPLAYPORT_ATTR_INFO;
- return;
- }
-
- // Visual beeper
- if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && osdGetVisualBeeperState()) {
- tfp_sprintf(element->buff, " * * * *");
- element->attr = DISPLAYPORT_ATTR_INFO;
- return;
- }
-
}
// Define the order in which the elements are drawn.
diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c
new file mode 100644
index 0000000000..4ee43ef614
--- /dev/null
+++ b/src/main/osd/osd_warnings.c
@@ -0,0 +1,339 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_OSD
+
+#include "config/config.h"
+#include "config/feature.h"
+
+#include "common/maths.h"
+#include "common/printf.h"
+
+#include "drivers/display.h"
+#include "drivers/osd_symbols.h"
+#include "drivers/time.h"
+
+#include "fc/core.h"
+#include "fc/rc.h"
+#include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
+
+#include "flight/failsafe.h"
+#include "flight/gps_rescue.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "io/beeper.h"
+
+#include "osd/osd.h"
+#include "osd/osd_elements.h"
+#include "osd/osd_warnings.h"
+
+#include "rx/rx.h"
+
+#include "sensors/acceleration.h"
+#include "sensors/adcinternal.h"
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
+{
+ const batteryState_e batteryState = getBatteryState();
+ const timeUs_t currentTimeUs = micros();
+
+ static timeUs_t armingDisabledUpdateTimeUs;
+ static unsigned armingDisabledDisplayIndex;
+
+ warningText[0] = '\0';
+ *displayAttr = DISPLAYPORT_ATTR_NONE;
+ *blinking = false;
+
+ // 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)));
+ }
+
+ tfp_sprintf(warningText, "%s", armingDisableFlagNames[armingDisabledDisplayIndex]);
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ 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)) {
+ tfp_sprintf(warningText, " BEACON ON"); // Display this message for the first 0.5 seconds
+ } else {
+ tfp_sprintf(warningText, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
+ }
+ *displayAttr = DISPLAYPORT_ATTR_INFO;
+ return;
+ }
+#endif // USE_DSHOT
+ if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
+ tfp_sprintf(warningText, "FAIL SAFE");
+ *displayAttr = DISPLAYPORT_ATTR_CRITICAL;
+ *blinking = true;;
+ return;
+ }
+
+ // Warn when in flip over after crash mode
+ if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && isFlipOverAfterCrashActive()) {
+ tfp_sprintf(warningText, "CRASH FLIP");
+ *displayAttr = DISPLAYPORT_ATTR_INFO;
+ 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)) {
+ const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
+ tfp_sprintf(warningText, "LAUNCH %d", pitchAngle);
+ } else
+#endif // USE_ACC
+ {
+ tfp_sprintf(warningText, "LAUNCH");
+ }
+
+ // Blink the message if the throttle is within 10% of the launch setting
+ if ( calculateThrottlePercent() >= MAX(currentPidProfile->launchControlThrottlePercent - 10, 0)) {
+ *blinking = true;;
+ }
+
+ *displayAttr = DISPLAYPORT_ATTR_INFO;
+ return;
+ }
+#endif // USE_LAUNCH_CONTROL
+
+ // RSSI
+ if (osdWarnGetState(OSD_WARNING_RSSI) && (getRssiPercent() < osdConfig()->rssi_alarm)) {
+ tfp_sprintf(warningText, "RSSI LOW");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+#ifdef USE_RX_RSSI_DBM
+ // rssi dbm
+ if (osdWarnGetState(OSD_WARNING_RSSI_DBM) && (getRssiDbm() < osdConfig()->rssi_dbm_alarm)) {
+ tfp_sprintf(warningText, "RSSI DBM");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+#endif // USE_RX_RSSI_DBM
+
+#ifdef USE_RX_LINK_QUALITY_INFO
+ // Link Quality
+ if (osdWarnGetState(OSD_WARNING_LINK_QUALITY) && (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm)) {
+ tfp_sprintf(warningText, "LINK QUALITY");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+#endif // USE_RX_LINK_QUALITY_INFO
+
+ if (osdWarnGetState(OSD_WARNING_BATTERY_CRITICAL) && batteryState == BATTERY_CRITICAL) {
+ tfp_sprintf(warningText, " LAND NOW");
+ *displayAttr = DISPLAYPORT_ATTR_CRITICAL;
+ *blinking = true;;
+ return;
+ }
+
+#ifdef USE_GPS_RESCUE
+ if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
+ ARMING_FLAG(ARMED) &&
+ gpsRescueIsConfigured() &&
+ !gpsRescueIsDisabled() &&
+ !gpsRescueIsAvailable()) {
+ tfp_sprintf(warningText, "RESCUE N/A");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ 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) < 0) {
+ tfp_sprintf(warningText, "RESCUE OFF");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+ }
+
+#endif // USE_GPS_RESCUE
+
+ // Show warning if in HEADFREE flight mode
+ if (FLIGHT_MODE(HEADFREE_MODE)) {
+ tfp_sprintf(warningText, "HEADFREE");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+
+#ifdef USE_ADC_INTERNAL
+ const int16_t coreTemperature = getCoreTemperatureCelsius();
+ if (osdWarnGetState(OSD_WARNING_CORE_TEMPERATURE) && coreTemperature >= osdConfig()->core_temp_alarm) {
+ tfp_sprintf(warningText, "CORE %c: %3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(coreTemperature), osdGetTemperatureSymbolForSelectedUnit());
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ 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) {
+ tfp_sprintf(warningText, "%s", escWarningMsg);
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+ }
+#endif // USE_ESC_SENSOR
+
+ if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
+ tfp_sprintf(warningText, "LOW BATTERY");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ 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()) {
+ tfp_sprintf(warningText, "RCSMOOTHING");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+#endif // USE_RC_SMOOTHING_FILTER
+
+ // Show warning if mah consumed is over the configured limit
+ if (osdWarnGetState(OSD_WARNING_OVER_CAP) && ARMING_FLAG(ARMED) && osdConfig()->cap_alarm > 0 && getMAhDrawn() >= osdConfig()->cap_alarm) {
+ tfp_sprintf(warningText, "OVER CAP");
+ *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *blinking = true;;
+ return;
+ }
+
+ // Show warning if battery is not fresh
+ if (osdWarnGetState(OSD_WARNING_BATTERY_NOT_FULL) && !(ARMING_FLAG(ARMED) || ARMING_FLAG(WAS_EVER_ARMED)) && (getBatteryState() == BATTERY_OK)
+ && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) {
+ tfp_sprintf(warningText, "BATT < FULL");
+ *displayAttr = DISPLAYPORT_ATTR_INFO;
+ return;
+ }
+
+ // Visual beeper
+ if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && osdGetVisualBeeperState()) {
+ tfp_sprintf(warningText, " * * * *");
+ *displayAttr = DISPLAYPORT_ATTR_INFO;
+ return;
+ }
+
+}
+
+#endif // USE_OSD
diff --git a/src/main/osd/osd_warnings.h b/src/main/osd/osd_warnings.h
new file mode 100644
index 0000000000..58e0062faa
--- /dev/null
+++ b/src/main/osd/osd_warnings.h
@@ -0,0 +1,28 @@
+/*
+ * 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
+
+#define OSD_WARNINGS_MAX_SIZE 12
+#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);
+
+void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr);
diff --git a/src/test/Makefile b/src/test/Makefile
index dff77dab49..3aa017faaa 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -186,6 +186,7 @@ motor_output_unittest_SRC := \
osd_unittest_SRC := \
$(USER_DIR)/osd/osd.c \
$(USER_DIR)/osd/osd_elements.c \
+ $(USER_DIR)/osd/osd_warnings.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/common/maths.c \
@@ -202,6 +203,7 @@ osd_unittest_DEFINES := \
link_quality_unittest_SRC := \
$(USER_DIR)/osd/osd.c \
$(USER_DIR)/osd/osd_elements.c \
+ $(USER_DIR)/osd/osd_warnings.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/drivers/display.c \
$(USER_DIR)/drivers/serial.c \
diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc
index e26c3bc7ce..18d4bc5116 100644
--- a/src/test/unit/link_quality_unittest.cc
+++ b/src/test/unit/link_quality_unittest.cc
@@ -27,26 +27,27 @@ extern "C" {
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_io.h"
- #include "common/time.h"
#include "common/crc.h"
- #include "common/utils.h"
#include "common/printf.h"
#include "common/streambuf.h"
+ #include "common/time.h"
+ #include "common/utils.h"
+
+ #include "config/config.h"
#include "drivers/osd_symbols.h"
#include "drivers/persistent.h"
#include "drivers/serial.h"
#include "drivers/system.h"
- #include "config/config.h"
#include "fc/core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
+ #include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
- #include "flight/imu.h"
#include "io/beeper.h"
#include "io/gps.h"
@@ -54,6 +55,7 @@ extern "C" {
#include "osd/osd.h"
#include "osd/osd_elements.h"
+ #include "osd/osd_warnings.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index e448f38974..7bdbc33b8f 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -28,39 +28,40 @@ extern "C" {
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_io.h"
- #include "config/feature.h"
-
- #include "pg/pg.h"
- #include "pg/pg_ids.h"
- #include "pg/rx.h"
-
#include "common/time.h"
+ #include "config/config.h"
+ #include "config/feature.h"
+
#include "drivers/osd_symbols.h"
#include "drivers/persistent.h"
#include "drivers/serial.h"
- #include "config/config.h"
#include "fc/core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/gps_rescue.h"
- #include "flight/pid.h"
#include "flight/imu.h"
+ #include "flight/mixer.h"
+ #include "flight/pid.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "osd/osd.h"
#include "osd/osd_elements.h"
+ #include "osd/osd_warnings.h"
+
+ #include "pg/pg.h"
+ #include "pg/pg_ids.h"
+ #include "pg/rx.h"
#include "sensors/acceleration.h"
#include "sensors/battery.h"
#include "rx/rx.h"
- #include "flight/mixer.h"
void osdRefresh(timeUs_t currentTimeUs);
void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time);