diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index cd3cfad20c..5fb91a8575 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -88,6 +88,7 @@
#include "pg/max7456.h"
#include "pg/mco.h"
#include "pg/motor.h"
+#include "pg/msp.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/pinio.h"
@@ -229,9 +230,9 @@ static const char * const lookupTableBlackboxSampleRate[] = {
};
#endif
-#ifdef USE_SERIAL_RX
+#ifdef USE_SERIALRX
static const char * const lookupTableSerialRX[] = {
- "SPEK1024",
+ "NONE",
"SPEK2048",
"SBUS",
"SUMD",
@@ -246,6 +247,8 @@ static const char * const lookupTableSerialRX[] = {
"FPORT",
"SRXL2",
"GHST",
+ "SPEK1024",
+
};
#endif
@@ -387,9 +390,9 @@ const char * const lookupTableRescueAltitudeMode[] = {
};
#endif
-#if defined(USE_MAX7456) || defined(USE_FRSKYOSD)
+#if defined(USE_VIDEO_SYSTEM)
static const char * const lookupTableVideoSystem[] = {
- "AUTO", "PAL", "NTSC"
+ "AUTO", "PAL", "NTSC", "HD"
};
#endif
@@ -524,7 +527,7 @@ static const char* const lookupTableFreqDomain[] = {
};
static const char* const lookupTableSwitchMode[] = {
- "HYBRID", "WIDE",
+ "WIDE", "HYBRID",
};
#endif
@@ -553,7 +556,7 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_SERVOS
LOOKUP_TABLE_ENTRY(lookupTableGimbalMode),
#endif
-#ifdef USE_SERIAL_RX
+#ifdef USE_SERIALRX
LOOKUP_TABLE_ENTRY(lookupTableSerialRX),
#endif
#ifdef USE_RX_SPI
@@ -602,7 +605,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableGyro),
#endif
LOOKUP_TABLE_ENTRY(lookupTableThrottleLimitType),
-#if defined(USE_MAX7456) || defined(USE_FRSKYOSD)
+#if defined(USE_VIDEO_SYSTEM)
LOOKUP_TABLE_ENTRY(lookupTableVideoSystem),
#endif
#if defined(USE_ITERM_RELAX)
@@ -770,7 +773,7 @@ const clivalue_t valueTable[] = {
{ "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },
{ "max_aux_channels", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, MAX_AUX_CHANNEL_COUNT }, PG_RX_CONFIG, offsetof(rxConfig_t, max_aux_channel) },
-#ifdef USE_SERIAL_RX
+#ifdef USE_SERIALRX
{ PARAM_NAME_SERIAL_RX_PROVIDER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) },
{ "serialrx_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_inverted) },
#endif
@@ -785,11 +788,8 @@ const clivalue_t valueTable[] = {
#if defined(USE_SERIALRX_SBUS)
{ "sbus_baud_fast", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_baud_fast) },
#endif
-#if defined(USE_SERIALRX_CRSF)
- { "crsf_use_rx_snr", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, crsf_use_rx_snr) },
#if defined(USE_CRSF_V3)
{ "crsf_use_negotiated_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, crsf_use_negotiated_baud) },
-#endif
#endif
{ "airmode_start_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) },
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) },
@@ -813,7 +813,7 @@ const clivalue_t valueTable[] = {
#endif
// PG_PWM_CONFIG
-#if defined(USE_PWM)
+#if defined(USE_RX_PWM)
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PWM_CONFIG, offsetof(pwmConfig_t, inputFilteringMode) },
#endif
@@ -1307,49 +1307,23 @@ const clivalue_t valueTable[] = {
// PG_OSD_CONFIG
#ifdef USE_OSD
{ "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_UNIT }, PG_OSD_CONFIG, offsetof(osdConfig_t, units) },
-
-// Please try to keep the OSD warnings in the same order as presented in the Configurator.
-// This makes it easier for the user to relate the CLI output as warnings are in the same relative
-// position as displayed in the GUI.
- { "osd_warn_arming_disable", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_ARMING_DISABLE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_batt_not_full", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_BATTERY_NOT_FULL, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_batt_warning", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_BATTERY_WARNING, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_batt_critical", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_BATTERY_CRITICAL, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_visual_beeper", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_VISUAL_BEEPER, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_crash_flip", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CRASH_FLIP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_esc_fail", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_ESC_FAIL, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#ifdef USE_ADC_INTERNAL
- { "osd_warn_core_temp", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CORE_TEMPERATURE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#endif
-#ifdef USE_RC_SMOOTHING_FILTER
- { "osd_warn_rc_smoothing", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_RC_SMOOTHING, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#endif
- { "osd_warn_fail_safe", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_FAIL_SAFE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#ifdef USE_LAUNCH_CONTROL
- { "osd_warn_launch_control", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_LAUNCH_CONTROL, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#endif
- { "osd_warn_no_gps_rescue", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_GPS_RESCUE_UNAVAILABLE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_gps_rescue_disabled", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_GPS_RESCUE_DISABLED, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
- { "osd_warn_rssi", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_RSSI, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#ifdef USE_RX_LINK_QUALITY_INFO
- { "osd_warn_link_quality", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_LINK_QUALITY, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#endif
-#if defined(USE_RX_RSSI_DBM)
- { "osd_warn_rssi_dbm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_RSSI_DBM, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-#endif
- { "osd_warn_over_cap", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_OVER_CAP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
+ // Enabled OSD warning flags are stored as bitmapped values inside a 32bit parameter
+ { "osd_warn_bitmask", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
{ "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_alarm) },
#ifdef USE_RX_LINK_QUALITY_INFO
{ "osd_link_quality_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, link_quality_alarm) },
#endif
#ifdef USE_RX_RSSI_DBM
- { "osd_rssi_dbm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { CRSF_RSSI_MIN, CRSF_SNR_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_dbm_alarm) },
+ { "osd_rssi_dbm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { CRSF_RSSI_MIN, CRSF_RSSI_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_dbm_alarm) },
+#endif
+#ifdef USE_RX_RSNR
+ { "osd_rsnr_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { CRSF_SNR_MIN, CRSF_SNR_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rsnr_alarm) },
#endif
{ "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) },
{ "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) },
{ "osd_distance_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, distance_alarm) },
- { "osd_esc_temp_alarm", VAR_INT8 | MASTER_VALUE, .config.minmax = { INT8_MIN, INT8_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_temp_alarm) },
+ { "osd_esc_temp_alarm", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, UINT8_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_temp_alarm) },
{ "osd_esc_rpm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { ESC_RPM_ALARM_OFF, INT16_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_rpm_alarm) },
{ "osd_esc_current_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { ESC_CURRENT_ALARM_OFF, INT16_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, esc_current_alarm) },
#ifdef USE_ADC_INTERNAL
@@ -1375,6 +1349,9 @@ const clivalue_t valueTable[] = {
#endif
#ifdef USE_RX_RSSI_DBM
{ "osd_rssi_dbm_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_RSSI_DBM_VALUE]) },
+#endif
+#ifdef USE_RX_RSNR
+ { "osd_rsnr_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_RSNR_VALUE]) },
#endif
{ "osd_tim_1_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_ITEM_TIMER_1]) },
{ "osd_tim_2_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_ITEM_TIMER_2]) },
@@ -1389,9 +1366,10 @@ const clivalue_t valueTable[] = {
{ "osd_ah_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_ARTIFICIAL_HORIZON]) },
{ "osd_current_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_CURRENT_DRAW]) },
{ "osd_mah_drawn_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_MAH_DRAWN]) },
+ { "osd_wh_drawn_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_WATT_HOURS_DRAWN]) },
{ "osd_motor_diag_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_MOTOR_DIAG]) },
{ "osd_craft_name_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_CRAFT_NAME]) },
- { "osd_display_name_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_DISPLAY_NAME]) },
+ { "osd_pilot_name_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_PILOT_NAME]) },
{ "osd_gps_speed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_SPEED]) },
{ "osd_gps_lon_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LON]) },
{ "osd_gps_lat_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_GPS_LAT]) },
@@ -1415,6 +1393,7 @@ const clivalue_t valueTable[] = {
{ "osd_disarmed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_DISARMED]) },
{ "osd_nheading_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_NUMERICAL_HEADING]) },
{ "osd_up_down_reference_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_UP_DOWN_REFERENCE]) },
+ { "osd_ready_mode_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_READY_MODE]) },
#ifdef USE_VARIO
{ "osd_nvario_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_NUMERICAL_VARIO]) },
#endif
@@ -1451,40 +1430,25 @@ const clivalue_t valueTable[] = {
{ "osd_camera_frame_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_CAMERA_FRAME]) },
{ "osd_efficiency_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_EFFICIENCY]) },
{ "osd_total_flights_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_TOTAL_FLIGHTS]) },
+ { "osd_aux_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_AUX_VALUE]) },
- // OSD stats enabled flags are stored as bitmapped values inside a 32bit parameter
- // It is recommended to keep the settings order the same as the enumeration. This way the settings are displayed in the cli in the same order making it easier on the users
- { "osd_stat_rtc_date_time", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_RTC_DATE_TIME, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_tim_1", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_TIMER_1, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_tim_2", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_TIMER_2, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_spd", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_SPEED, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_dist", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_DISTANCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_min_batt", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_BATTERY, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_endbatt", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_END_BATTERY, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_battery", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_BATTERY, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_min_rssi", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_RSSI, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_curr", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_CURRENT, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_used_mah", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_USED_MAH, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_alt", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ALTITUDE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_bbox", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_BLACKBOX, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_bb_no", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_BLACKBOX_NUMBER, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_g_force", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_G_FORCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_esc_temp", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_TEMP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_max_esc_rpm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_RPM, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_min_link_quality", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_LINK_QUALITY,PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_flight_dist", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_FLIGHT_DISTANCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
-#ifdef USE_DYN_NOTCH_FILTER
- { "osd_stat_max_fft", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_FFT, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
-#endif
-#ifdef USE_PERSISTENT_STATS
- { "osd_stat_total_flights", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_TOTAL_FLIGHTS, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_total_time", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_TOTAL_TIME, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
- { "osd_stat_total_dist", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_TOTAL_DIST, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
-#endif
-#ifdef USE_RX_RSSI_DBM
- { "osd_stat_min_rssi_dbm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_RSSI_DBM, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
+#ifdef USE_MSP_DISPLAYPORT
+ { "osd_sys_goggle_voltage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_GOGGLE_VOLTAGE]) },
+ { "osd_sys_vtx_voltage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_VTX_VOLTAGE]) },
+ { "osd_sys_bitrate_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_BITRATE]) },
+ { "osd_sys_delay_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_DELAY]) },
+ { "osd_sys_distance_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_DISTANCE]) },
+ { "osd_sys_lq_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_LQ]) },
+ { "osd_sys_goggle_dvr_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_GOGGLE_DVR]) },
+ { "osd_sys_vtx_dvr_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_VTX_DVR]) },
+ { "osd_sys_warnings_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_WARNINGS]) },
+ { "osd_sys_vtx_temp_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_VTX_TEMP]) },
+ { "osd_sys_fan_speed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_SYS_FAN_SPEED]) },
#endif
+ // OSD stats enabled flags are stored as bitmapped values inside a 32bit parameter
+ { "osd_stat_bitmask", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)},
+
#ifdef USE_OSD_PROFILES
{ "osd_profile", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, OSD_PROFILE_COUNT }, PG_OSD_CONFIG, offsetof(osdConfig_t, osdProfileIndex) },
{ "osd_profile_1_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, OSD_PROFILE_NAME_LENGTH, STRING_FLAGS_NONE }, PG_OSD_CONFIG, offsetof(osdConfig_t, profile[0]) },
@@ -1539,7 +1503,7 @@ const clivalue_t valueTable[] = {
#endif
// PG_VCD_CONFIG
-#if defined(USE_MAX7456) || defined(USE_FRSKYOSD) || defined(USE_MSP_DISPLAYPORT)
+#if defined(USE_VIDEO_SYSTEM)
{ "vcd_video_system", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_VIDEO_SYSTEM }, PG_VCD_CONFIG, offsetof(vcdProfile_t, video_system) },
#endif
#if defined(USE_MAX7456)
@@ -1558,8 +1522,7 @@ const clivalue_t valueTable[] = {
#ifdef USE_MSP_DISPLAYPORT
{ "displayport_msp_col_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) },
{ "displayport_msp_row_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) },
- { "displayport_msp_serial", VAR_INT8 | MASTER_VALUE, .config.minmax = { SERIAL_PORT_NONE, SERIAL_PORT_IDENTIFIER_MAX }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, displayPortSerial) },
- { "displayport_msp_attrs", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, attrValues) },
+ { "displayport_msp_fonts", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, fontSelection) },
{ "displayport_msp_use_device_blink", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, useDeviceBlink) },
#endif
@@ -1695,6 +1658,7 @@ const clivalue_t valueTable[] = {
{ "scheduler_relax_rx", VAR_UINT16 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_SCHEDULER_CONFIG, PG_ARRAY_ELEMENT_OFFSET(schedulerConfig_t, 0, rxRelaxDeterminism) },
{ "scheduler_relax_osd", VAR_UINT16 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_SCHEDULER_CONFIG, PG_ARRAY_ELEMENT_OFFSET(schedulerConfig_t, 0, osdRelaxDeterminism) },
+ { "serialmsp_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MSP_CONFIG, offsetof(mspConfig_t, halfDuplex) },
// PG_TIMECONFIG
#ifdef USE_RTC_TIME
@@ -1721,11 +1685,12 @@ const clivalue_t valueTable[] = {
{ "stats_total_time_s", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_time_s) },
{ "stats_total_dist_m", VAR_UINT32 | MASTER_VALUE, .config.u32Max = UINT32_MAX, PG_STATS_CONFIG, offsetof(statsConfig_t, stats_total_dist_m) },
#endif
- { "name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, name) },
+ { "craft_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, craftName) },
#ifdef USE_OSD
- { "display_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, displayName) },
+ { "pilot_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, pilotName) },
#endif
+
// PG_POSITION
{ "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
{ "position_alt_gps_min_sats", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, 50 }, PG_POSITION, offsetof(positionConfig_t, altNumSatsGpsUse) },
diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h
index 3bf0e292a8..cddc4a27a6 100644
--- a/src/main/cli/settings.h
+++ b/src/main/cli/settings.h
@@ -48,7 +48,7 @@ typedef enum {
#ifdef USE_SERVOS
TABLE_GIMBAL_MODE,
#endif
-#ifdef USE_SERIAL_RX
+#ifdef USE_SERIALRX
TABLE_SERIAL_RX,
#endif
#ifdef USE_RX_SPI
@@ -97,7 +97,7 @@ typedef enum {
TABLE_GYRO,
#endif
TABLE_THROTTLE_LIMIT_TYPE,
-#if defined(USE_MAX7456) || defined(USE_FRSKYOSD)
+#if defined(USE_VIDEO_SYSTEM)
TABLE_VIDEO_SYSTEM,
#endif
#if defined(USE_ITERM_RELAX)
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index 6cc415f9d2..c4578ffd30 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -155,14 +155,16 @@ bool cmsDisplayPortSelect(displayPort_t *instance)
// 30 cols x 13 rows
// HoTT Telemetry Screen
// 21 cols x 8 rows
-//
+// HD
+// 53 cols x 20 rows
// Spektrum SRXL Telemtry Textgenerator
// 13 cols x 9 rows, top row printed as a Bold Heading
// Needs the "smallScreen" adaptions
-#define CMS_MAX_ROWS 16
+#define CMS_MAX_ROWS 31
#define NORMAL_SCREEN_MIN_COLS 18 // Less is a small screen
+#define NORMAL_SCREEN_MAX_COLS 30 // More is a large screen
static bool smallScreen;
static uint8_t leftMenuColumn;
static uint8_t rightMenuColumn;
@@ -388,7 +390,7 @@ static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row
#else
colpos = smallScreen ? rightMenuColumn - maxSize : rightMenuColumn;
#endif
- cnt = cmsDisplayWrite(pDisplay, colpos, row, DISPLAYPORT_ATTR_NONE, buff);
+ cnt = cmsDisplayWrite(pDisplay, colpos, row, DISPLAYPORT_SEVERITY_NORMAL, buff);
return cnt;
}
@@ -589,7 +591,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t
case OME_Label:
if (IS_PRINTVALUE(*flags) && p->data) {
// A label with optional string, immediately following text
- cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_ATTR_NONE, p->data);
+ cnt = cmsDisplayWrite(pDisplay, leftMenuColumn + 1 + (uint8_t)strlen(p->text), row, DISPLAYPORT_SEVERITY_NORMAL, p->data);
CLR_PRINTVALUE(*flags);
}
break;
@@ -605,9 +607,9 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t
#ifdef CMS_MENU_DEBUG
// Shouldn't happen. Notify creator of this menu content
#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
- cnt = cmsDisplayWrite(pDisplay, rightMenuColumn - 6, row, DISPLAYPORT_ATTR_NONE, "BADENT");
+ cnt = cmsDisplayWrite(pDisplay, rightMenuColumn - 6, row, DISPLAYPORT_SEVERITY_NORMAL, "BADENT");
#else
- cnt = cmsDisplayWrite(pDisplay, rightMenuColumn, row, DISPLAYPORT_ATTR_NONE, "BADENT");
+ cnt = cmsDisplayWrite(pDisplay, rightMenuColumn, row, DISPLAYPORT_SEVERITY_NORMAL, "BADENT");
#endif
#endif
break;
@@ -746,7 +748,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
#endif
if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
- room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, " ");
+ room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, " ");
}
if (room < 30) {
@@ -754,7 +756,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
}
if (pDisplay->cursorRow != currentCtx.cursorRow) {
- room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, ">");
+ room -= cmsDisplayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, ">");
pDisplay->cursorRow = currentCtx.cursorRow;
}
@@ -776,7 +778,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
if (IS_PRINTLABEL(runtimeEntryFlags[i])) {
uint8_t coloff = leftMenuColumn;
coloff += ((p->flags & OSD_MENU_ELEMENT_MASK) == OME_Label) ? 0 : 1;
- room -= cmsDisplayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, p->text);
+ room -= cmsDisplayWrite(pDisplay, coloff, top + i * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, p->text);
CLR_PRINTLABEL(runtimeEntryFlags[i]);
if (room < 30) {
return;
@@ -786,7 +788,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
// Highlight values overridden by sliders
if (rowSliderOverride(p->flags)) {
- displayWriteChar(pDisplay, leftMenuColumn - 1, top + i * linesPerMenuItem, DISPLAYPORT_ATTR_NONE, 'S');
+ displayWriteChar(pDisplay, leftMenuColumn - 1, top + i * linesPerMenuItem, DISPLAYPORT_SEVERITY_NORMAL, 'S');
}
// Print values
@@ -809,12 +811,12 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
// simple text device and use the '^' (carat) and 'V' for arrow approximations.
if (displayWasCleared && leftMenuColumn > 0) { // make sure there's room to draw the symbol
if (currentCtx.page > 0) {
- const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_NORTH : '^';
- displayWriteChar(pDisplay, leftMenuColumn - 1, top, DISPLAYPORT_ATTR_NONE, symbol);
+ const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SMALL_UP : '^';
+ displayWriteChar(pDisplay, leftMenuColumn - 1, top, DISPLAYPORT_SEVERITY_NORMAL, symbol);
}
if (currentCtx.page < pageCount - 1) {
- const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SOUTH : 'V';
- displayWriteChar(pDisplay, leftMenuColumn - 1, top + pageMaxRow, DISPLAYPORT_ATTR_NONE, symbol);
+ const uint8_t symbol = displaySupportsOsdSymbols(pDisplay) ? SYM_ARROW_SMALL_DOWN : 'v';
+ displayWriteChar(pDisplay, leftMenuColumn - 1, top + pageMaxRow, DISPLAYPORT_SEVERITY_NORMAL, symbol);
}
}
@@ -897,9 +899,11 @@ void cmsMenuOpen(void)
menuStackIdx = 0;
setArmingDisabled(ARMING_DISABLED_CMS_MENU);
displayLayerSelect(pCurrentDisplay, DISPLAYPORT_LAYER_FOREGROUND); // make sure the foreground layer is active
+#ifdef USE_OSD
if (osdConfig()->cms_background_type != DISPLAY_BACKGROUND_TRANSPARENT) {
displaySetBackgroundType(pCurrentDisplay, (displayPortBackground_e)osdConfig()->cms_background_type); // set the background type if not transparent
}
+#endif
} else {
// Switch display
displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
@@ -911,7 +915,9 @@ void cmsMenuOpen(void)
displaySetBackgroundType(pCurrentDisplay, DISPLAY_BACKGROUND_TRANSPARENT); // reset previous displayPort to transparent
displayRelease(pCurrentDisplay);
pCurrentDisplay = pNextDisplay;
+#ifdef USE_OSD
displaySetBackgroundType(pCurrentDisplay, (displayPortBackground_e)osdConfig()->cms_background_type); // set the background type if not transparent
+#endif
} else {
return;
}
@@ -931,12 +937,21 @@ void cmsMenuOpen(void)
} else {
smallScreen = false;
linesPerMenuItem = 1;
- leftMenuColumn = 2;
+ if (pCurrentDisplay->cols <= NORMAL_SCREEN_MAX_COLS) {
+ leftMenuColumn = 2;
+#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
+ rightMenuColumn = pCurrentDisplay->cols - 2;
+#else
+ rightMenuColumn = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
+#endif
+ } else {
+ leftMenuColumn = (pCurrentDisplay->cols / 2) - 13;
#ifdef CMS_OSD_RIGHT_ALIGNED_VALUES
- rightMenuColumn = pCurrentDisplay->cols - 2;
+ rightMenuColumn = (pCurrentDisplay->cols / 2) + 13;
#else
- rightMenuColumn = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
+ rightMenuColumn = pCurrentDisplay->cols - CMS_DRAW_BUFFER_LEN;
#endif
+ }
maxMenuItems = pCurrentDisplay->rows - 2;
}
@@ -999,7 +1014,7 @@ const void *cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT) || (exitType == CMS_POPUP_EXITREBOOT)) {
displayClearScreen(pDisplay, DISPLAY_CLEAR_WAIT);
- cmsDisplayWrite(pDisplay, 5, 3, DISPLAYPORT_ATTR_NONE, "REBOOTING...");
+ cmsDisplayWrite(pDisplay, 5, 3, DISPLAYPORT_SEVERITY_NORMAL, "REBOOTING...");
// Flush display
displayRedraw(pDisplay);
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index 6548eca960..7f7e384679 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@@ -170,7 +170,7 @@ static const void *cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
}
displayClearScreen(pDisplay, DISPLAY_CLEAR_WAIT);
- displayWrite(pDisplay, 5, 3, DISPLAYPORT_ATTR_INFO, "ERASING FLASH...");
+ displayWrite(pDisplay, 5, 3, DISPLAYPORT_SEVERITY_INFO, "ERASING FLASH...");
displayRedraw(pDisplay);
flashfsEraseCompletely();
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index 86bf5f9ff5..0724c2e9b1 100644
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -80,6 +80,9 @@ const OSD_Entry menuOsdActiveElemsEntries[] =
{"RSSI", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_RSSI_VALUE]},
#ifdef USE_RX_RSSI_DBM
{"RSSI DBM", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_RSSI_DBM_VALUE]},
+#endif
+#ifdef USE_RX_RSNR
+ {"RSNR", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_RSNR_VALUE]},
#endif
{"BATTERY VOLTAGE", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_VOLTAGE]},
{"BATTERY USAGE", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_USAGE]},
@@ -105,7 +108,7 @@ const OSD_Entry menuOsdActiveElemsEntries[] =
#endif
{"ANTI GRAVITY", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_ANTI_GRAVITY]},
{"FLY MODE", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_FLYMODE]},
- {"NAME", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_CRAFT_NAME]},
+ {"CRAFT NAME", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_CRAFT_NAME]},
{"THROTTLE", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_THROTTLE_POS]},
#ifdef USE_VTX_CONTROL
{"VTX CHAN", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_VTX_CHANNEL]},
@@ -161,10 +164,11 @@ const OSD_Entry menuOsdActiveElemsEntries[] =
{"STICK OVERLAY LEFT", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_STICK_OVERLAY_LEFT]},
{"STICK OVERLAY RIGHT",OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_STICK_OVERLAY_RIGHT]},
#endif
- {"DISPLAY NAME", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_DISPLAY_NAME]},
+ {"PILOT NAME", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_PILOT_NAME]},
{"RC CHANNELS", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_RC_CHANNELS]},
{"CAMERA FRAME", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_CAMERA_FRAME]},
{"TOTAL FLIGHTS", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_TOTAL_FLIGHTS]},
+ {"AUX VALUE", OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_AUX_VALUE]},
{"BACK", OME_Back, NULL, NULL},
{NULL, OME_END, NULL, NULL}
};
@@ -183,6 +187,7 @@ static CMS_Menu menuOsdActiveElems = {
static uint8_t osdConfig_rssi_alarm;
static uint16_t osdConfig_link_quality_alarm;
static int16_t osdConfig_rssi_dbm_alarm;
+static int16_t osdConfig_rsnr_alarm;
static uint16_t osdConfig_cap_alarm;
static uint16_t osdConfig_alt_alarm;
static uint16_t osdConfig_distance_alarm;
@@ -196,6 +201,7 @@ static const void *menuAlarmsOnEnter(displayPort_t *pDisp)
osdConfig_rssi_alarm = osdConfig()->rssi_alarm;
osdConfig_link_quality_alarm = osdConfig()->link_quality_alarm;
osdConfig_rssi_dbm_alarm = osdConfig()->rssi_dbm_alarm;
+ osdConfig_rsnr_alarm = osdConfig()->rsnr_alarm;
osdConfig_cap_alarm = osdConfig()->cap_alarm;
osdConfig_alt_alarm = osdConfig()->alt_alarm;
osdConfig_distance_alarm = osdConfig()->distance_alarm;
@@ -213,6 +219,7 @@ static const void *menuAlarmsOnExit(displayPort_t *pDisp, const OSD_Entry *self)
osdConfigMutable()->rssi_alarm = osdConfig_rssi_alarm;
osdConfigMutable()->link_quality_alarm = osdConfig_link_quality_alarm;
osdConfigMutable()->rssi_dbm_alarm = osdConfig_rssi_dbm_alarm;
+ osdConfigMutable()->rsnr_alarm = osdConfig_rsnr_alarm;
osdConfigMutable()->cap_alarm = osdConfig_cap_alarm;
osdConfigMutable()->alt_alarm = osdConfig_alt_alarm;
osdConfigMutable()->distance_alarm = osdConfig_distance_alarm;
@@ -228,6 +235,7 @@ const OSD_Entry menuAlarmsEntries[] =
{"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}},
{"LINK QUALITY", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_link_quality_alarm, 5, 300, 5}},
{"RSSI DBM", OME_INT16, NULL, &(OSD_INT16_t){&osdConfig_rssi_dbm_alarm, CRSF_RSSI_MIN, CRSF_SNR_MAX, 5}},
+ {"RSNR", OME_INT16, NULL, &(OSD_INT16_t){&osdConfig_rsnr_alarm, CRSF_SNR_MIN, CRSF_SNR_MAX, 5}},
{"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}},
{"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}},
{"MAX DISTANCE", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_distance_alarm, 0, UINT16_MAX, 10}},
diff --git a/src/main/cms/cms_menu_vtx_msp.c b/src/main/cms/cms_menu_vtx_msp.c
new file mode 100644
index 0000000000..6f1cd42b63
--- /dev/null
+++ b/src/main/cms/cms_menu_vtx_msp.c
@@ -0,0 +1,285 @@
+/*
+ * 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 "platform.h"
+
+#if defined(USE_CMS) && defined(USE_VTX_MSP)
+
+#include "common/printf.h"
+#include "common/utils.h"
+
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+
+#include "drivers/vtx_common.h"
+#include "drivers/vtx_table.h"
+
+#include "config/config.h"
+
+#include "io/vtx_msp.h"
+#include "io/vtx.h"
+
+char mspCmsStatusString[31] = "- -- ---- ----";
+// m bc ffff tppp
+// 01234567890123
+
+void mspCmsUpdateStatusString(void)
+{
+ vtxDevice_t *vtxDevice = vtxCommonDevice();
+
+ if (vtxTableBandCount == 0 || vtxTablePowerLevels == 0) {
+ strncpy(mspCmsStatusString, "PLEASE CONFIGURE VTXTABLE", sizeof(mspCmsStatusString));
+ return;
+ }
+
+ mspCmsStatusString[0] = '*';
+ mspCmsStatusString[1] = ' ';
+ uint8_t band;
+ uint8_t chan;
+ if (!vtxCommonGetBandAndChannel(vtxDevice, &band, &chan) || (band == 0 && chan == 0)) {
+ mspCmsStatusString[2] = 'U';//user freq
+ mspCmsStatusString[3] = 'F';
+ } else {
+ mspCmsStatusString[2] = vtxCommonLookupBandLetter(vtxDevice, band);
+ mspCmsStatusString[3] = vtxCommonLookupChannelName(vtxDevice, chan)[0];
+ }
+ mspCmsStatusString[4] = ' ';
+
+ uint16_t freq;
+ if (!vtxCommonGetFrequency(vtxDevice, &freq) || (freq == 0)) {
+ tfp_sprintf(&mspCmsStatusString[5], "----");
+ } else {
+ tfp_sprintf(&mspCmsStatusString[5], "%4d", freq);
+ }
+
+ uint8_t powerIndex;
+ uint16_t powerValue;
+ if (vtxCommonGetPowerIndex(vtxDevice, &powerIndex) && vtxCommonLookupPowerValue(vtxDevice, powerIndex, &powerValue)) {
+ tfp_sprintf(&mspCmsStatusString[9], " *%3d", powerValue);
+ } else {
+ tfp_sprintf(&mspCmsStatusString[9], " ----");
+ }
+}
+
+uint8_t mspCmsPitMode = 0;
+uint8_t mspCmsBand = 1;
+uint8_t mspCmsChan = 1;
+uint16_t mspCmsFreqRef;
+
+static OSD_TAB_t mspCmsEntBand;
+static OSD_TAB_t mspCmsEntChan;
+
+static OSD_UINT16_t mspCmsEntFreqRef = { &mspCmsFreqRef, 5600, 5900, 0 };
+
+static uint8_t mspCmsPower = 1;
+
+static OSD_TAB_t mspCmsEntPower;
+
+static void mspCmsUpdateFreqRef(void)
+{
+ if (mspCmsBand > 0 && mspCmsChan > 0) {
+ mspCmsFreqRef = vtxCommonLookupFrequency(vtxCommonDevice(), mspCmsBand, mspCmsChan);
+ }
+}
+
+static const void *mspCmsConfigBand(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (mspCmsBand == 0) {
+ // Bounce back
+ mspCmsBand = 1;
+ } else {
+ mspCmsUpdateFreqRef();
+ }
+
+ return NULL;
+}
+
+static const void *mspCmsConfigChan(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (mspCmsChan == 0) {
+ // Bounce back
+ mspCmsChan = 1;
+ } else {
+ mspCmsUpdateFreqRef();
+ }
+
+ return NULL;
+}
+
+static const void *mspCmsConfigPower(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (mspCmsPower == 0) {
+ // Bounce back
+ mspCmsPower = 1;
+ }
+
+ return NULL;
+}
+
+#define MSP_PIT_STATUS_NA (0)
+#define MSP_PIT_STATUS_OFF (1)
+#define MSP_PIT_STATUS_ON (2)
+
+static const char * const mspCmsPitModeNames[] = {
+ "---", "OFF", "ON "
+};
+
+static OSD_TAB_t mspCmsEntPitMode = { &mspCmsPitMode, 2, mspCmsPitModeNames };
+
+static const void *mspCmsSetPitMode(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ if (mspCmsPitMode == MSP_PIT_STATUS_NA) {
+ // Bouce back
+ mspCmsPitMode = MSP_PIT_STATUS_OFF;
+ } else {
+ vtxCommonSetPitMode(vtxCommonDevice(), (mspCmsPitMode == MSP_PIT_STATUS_OFF) ? 0 : 1);
+ }
+
+ return NULL;
+}
+
+static const void *mspCmsCommence(displayPort_t *pDisp, const void *self)
+{
+ UNUSED(pDisp);
+ UNUSED(self);
+
+ vtxDevice_t *device = vtxCommonDevice();
+ vtxCommonSetBandAndChannel(device, mspCmsBand, mspCmsChan);
+ vtxCommonSetPowerByIndex(device, mspCmsPower);
+
+ // update'vtx_' settings
+ vtxSettingsConfigMutable()->band = mspCmsBand;
+ vtxSettingsConfigMutable()->channel = mspCmsChan;
+ vtxSettingsConfigMutable()->power = mspCmsPower;
+ vtxSettingsConfigMutable()->freq = vtxCommonLookupFrequency(vtxCommonDevice(), mspCmsBand, mspCmsChan);
+
+ saveConfigAndNotify();
+
+ return MENU_CHAIN_BACK;
+}
+
+static bool mspCmsInitSettings(void)
+{
+ vtxDevice_t *device = vtxCommonDevice();
+ unsigned vtxStatus;
+
+ if (!device) {
+ return false;
+ }
+
+ vtxCommonGetBandAndChannel(device, &mspCmsBand, &mspCmsChan);
+
+ mspCmsUpdateFreqRef();
+ if (vtxCommonGetStatus(device, &vtxStatus)) {
+ mspCmsPitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? MSP_PIT_STATUS_ON : MSP_PIT_STATUS_OFF;
+ } else {
+ mspCmsPitMode = MSP_PIT_STATUS_NA;
+ }
+
+ if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &mspCmsPower)) {
+ mspCmsPower = 1;
+ }
+
+ mspCmsEntBand.val = &mspCmsBand;
+ mspCmsEntBand.max = vtxTableBandCount;
+ mspCmsEntBand.names = vtxTableBandNames;
+
+ mspCmsEntChan.val = &mspCmsChan;
+ mspCmsEntChan.max = vtxTableChannelCount;
+ mspCmsEntChan.names = vtxTableChannelNames;
+
+ mspCmsEntPower.val = &mspCmsPower;
+ mspCmsEntPower.max = vtxTablePowerLevels;
+ mspCmsEntPower.names = vtxTablePowerLabels;
+
+ return true;
+}
+
+static const void *mspCmsOnEnter(displayPort_t *pDisp)
+{
+ UNUSED(pDisp);
+
+ if (!mspCmsInitSettings()) {
+ return MENU_CHAIN_BACK;
+ }
+
+ return NULL;
+}
+
+static const OSD_Entry mspCmsMenuCommenceEntries[] = {
+ { "CONFIRM", OME_Label, NULL, NULL },
+ { "YES", OME_Funcall, mspCmsCommence, NULL },
+ { "NO", OME_Back, NULL, NULL },
+ { NULL, OME_END, NULL, NULL }
+};
+
+static CMS_Menu mspCmsMenuCommence = {
+#ifdef CMS_MENU_DEBUG
+ .GUARD_text = "XVTXMSPC",
+ .GUARD_type = OME_MENU,
+#endif
+ .onEnter = NULL,
+ .onExit = NULL,
+ .onDisplayUpdate = NULL,
+ .entries = mspCmsMenuCommenceEntries,
+};
+
+static const OSD_Entry mspMenuEntries[] =
+{
+ { "- MSP -", OME_Label, NULL, NULL },
+ { "", OME_Label | DYNAMIC, NULL, mspCmsStatusString },
+ { "PIT", OME_TAB, mspCmsSetPitMode, &mspCmsEntPitMode },
+ { "BAND", OME_TAB, mspCmsConfigBand, &mspCmsEntBand },
+ { "CHAN", OME_TAB, mspCmsConfigChan, &mspCmsEntChan },
+ { "(FREQ)", OME_UINT16 | DYNAMIC, NULL, &mspCmsEntFreqRef },
+ { "POWER", OME_TAB, mspCmsConfigPower, &mspCmsEntPower },
+ { "SAVE", OME_Submenu, cmsMenuChange, &mspCmsMenuCommence },
+ { "BACK", OME_Back, NULL, NULL },
+ { NULL, OME_END, NULL, NULL }
+};
+
+CMS_Menu cmsx_menuVtxMsp = {
+#ifdef CMS_MENU_DEBUG
+ .GUARD_text = "XVTXMSP",
+ .GUARD_type = OME_MENU,
+#endif
+ .onEnter = mspCmsOnEnter,
+ .onExit = NULL,
+ .onDisplayUpdate = NULL,
+ .entries = mspMenuEntries,
+};
+#endif
diff --git a/src/main/cms/cms_menu_vtx_msp.h b/src/main/cms/cms_menu_vtx_msp.h
new file mode 100644
index 0000000000..1c255ebf72
--- /dev/null
+++ b/src/main/cms/cms_menu_vtx_msp.h
@@ -0,0 +1,27 @@
+/*
+ * 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 "cms/cms.h"
+#include "cms/cms_types.h"
+extern CMS_Menu cmsx_menuVtxMsp;
+
+void mspCmsUpdateStatusString(void);
diff --git a/src/main/config/feature.h b/src/main/config/feature.h
index 240346c5a6..312787b23e 100644
--- a/src/main/config/feature.h
+++ b/src/main/config/feature.h
@@ -25,10 +25,20 @@
#ifndef DEFAULT_FEATURES
#define DEFAULT_FEATURES 0
#endif
+
#ifndef DEFAULT_RX_FEATURE
-#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
+
+#if defined(USE_SERIALRX)
+#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
+#elif defined(USE_RX_MSP)
+#define DEFAULT_RX_FEATURE FEATURE_RX_MSP
+#elif defined(USE_RX_SPI)
+// need to test with FEATURE_RX_EXPRESSLRS
+#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
#endif
+#endif // DEFAULT_RX_FEATURE
+
typedef enum {
FEATURE_RX_PPM = 1 << 0,
FEATURE_INFLIGHT_ACC_CAL = 1 << 2,
@@ -49,6 +59,7 @@ typedef enum {
FEATURE_CHANNEL_FORWARDING = 1 << 20,
FEATURE_TRANSPONDER = 1 << 21,
FEATURE_AIRMODE = 1 << 22,
+ FEATURE_VTX = 1 << 24,
FEATURE_RX_SPI = 1 << 25,
//FEATURE_SOFTSPI = 1 << 26, (removed)
FEATURE_ESC_SENSOR = 1 << 27,
diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 1c8abf2da6..d9b90bf387 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -80,11 +80,20 @@ void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y)
instance->posY = y;
}
-int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *s)
+int displaySys(displayPort_t *instance, uint8_t x, uint8_t y, displayPortSystemElement_e systemElement)
{
- instance->posX = x + strlen(s);
+ if (instance->vTable->writeSys) {
+ return instance->vTable->writeSys(instance, x, y, systemElement);
+ }
+
+ return 0;
+}
+
+int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *text)
+{
+ instance->posX = x + strlen(text);
instance->posY = y;
- return instance->vTable->writeString(instance, x, y, attr, s);
+ return instance->vTable->writeString(instance, x, y, attr, text);
}
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, uint8_t c)
diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h
index 7c95d031c0..d20979fb43 100644
--- a/src/main/drivers/display.h
+++ b/src/main/drivers/display.h
@@ -20,6 +20,10 @@
#pragma once
+#define VIDEO_COLUMNS_SD 30
+#define VIDEO_LINES_NTSC 13
+#define VIDEO_LINES_PAL 16
+
typedef enum {
DISPLAYPORT_DEVICE_TYPE_MAX7456 = 0,
DISPLAYPORT_DEVICE_TYPE_OLED,
@@ -31,13 +35,30 @@ typedef enum {
} displayPortDeviceType_e;
typedef enum {
- DISPLAYPORT_ATTR_NONE = 0,
- DISPLAYPORT_ATTR_INFO,
- DISPLAYPORT_ATTR_WARNING,
- DISPLAYPORT_ATTR_CRITICAL,
-} displayPortAttr_e;
+ DISPLAYPORT_SEVERITY_NORMAL = 0,
+ DISPLAYPORT_SEVERITY_INFO,
+ DISPLAYPORT_SEVERITY_WARNING,
+ DISPLAYPORT_SEVERITY_CRITICAL,
+ DISPLAYPORT_SEVERITY_COUNT,
+} displayPortSeverity_e;
+
+#define DISPLAYPORT_BLINK 0x80 // Device local blink bit or'ed into displayPortSeverity_e
-#define DISPLAYPORT_ATTR_BLINK 0x80 // Device local blink bit or'ed into displayPortAttr_e
+// System elements rendered by VTX or Goggles
+typedef enum {
+ DISPLAYPORT_SYS_GOGGLE_VOLTAGE = 0,
+ DISPLAYPORT_SYS_VTX_VOLTAGE = 1,
+ DISPLAYPORT_SYS_BITRATE = 2,
+ DISPLAYPORT_SYS_DELAY = 3,
+ DISPLAYPORT_SYS_DISTANCE = 4,
+ DISPLAYPORT_SYS_LQ = 5,
+ DISPLAYPORT_SYS_GOGGLE_DVR = 6,
+ DISPLAYPORT_SYS_VTX_DVR = 7,
+ DISPLAYPORT_SYS_WARNINGS = 8,
+ DISPLAYPORT_SYS_VTX_TEMP = 9,
+ DISPLAYPORT_SYS_FAN_SPEED = 10,
+ DISPLAYPORT_SYS_COUNT,
+} displayPortSystemElement_e;
typedef enum {
DISPLAYPORT_LAYER_FOREGROUND,
@@ -103,6 +124,7 @@ typedef struct displayPortVTable_s {
int (*clearScreen)(displayPort_t *displayPort, displayClearOption_e options);
bool (*drawScreen)(displayPort_t *displayPort);
int (*screenSize)(const displayPort_t *displayPort);
+ int (*writeSys)(displayPort_t *displayPort, uint8_t x, uint8_t y, displayPortSystemElement_e systemElement);
int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t attr, const char *text);
int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t attr, uint8_t c);
bool (*isTransferInProgress)(const displayPort_t *displayPort);
@@ -129,7 +151,8 @@ void displayClearScreen(displayPort_t *instance, displayClearOption_e options);
bool displayDrawScreen(displayPort_t *instance);
int displayScreenSize(const displayPort_t *instance);
void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y);
-int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *s);
+int displaySys(displayPort_t *instance, uint8_t x, uint8_t y, displayPortSystemElement_e systemElement);
+int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, const char *text);
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t attr, uint8_t c);
bool displayIsTransferInProgress(const displayPort_t *instance);
bool displayHeartbeat(displayPort_t *instance);
diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c
index 7a76e458b5..8b163f537e 100644
--- a/src/main/io/displayport_hott.c
+++ b/src/main/io/displayport_hott.c
@@ -23,7 +23,8 @@
#include
#include "platform.h"
-#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
+
+#if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
#include "common/utils.h"
#include "cms/cms.h"
@@ -56,7 +57,7 @@ static int hottWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row,
UNUSED(attr);
while (*s) {
- hottWriteChar(displayPort, col++, row, DISPLAYPORT_ATTR_NONE, *(s++));
+ hottWriteChar(displayPort, col++, row, DISPLAYPORT_SEVERITY_NORMAL, *(s++));
}
return 0;
}
@@ -67,7 +68,7 @@ static int hottClearScreen(displayPort_t *displayPort, displayClearOption_e opti
for (int row = 0; row < displayPort->rows; row++) {
for (int col= 0; col < displayPort->cols; col++) {
- hottWriteChar(displayPort, col, row, DISPLAYPORT_ATTR_NONE, ' ');
+ hottWriteChar(displayPort, col, row, DISPLAYPORT_SEVERITY_NORMAL, ' ');
}
}
return 0;
@@ -130,7 +131,7 @@ static const displayPortVTable_t hottVTable = {
.layerCopy = NULL,
};
-static displayPort_t *displayPortHottInit()
+static displayPort_t *displayPortHottInit(void)
{
hottDisplayPort.device = NULL;
displayInit(&hottDisplayPort, &hottVTable, DISPLAYPORT_DEVICE_TYPE_HOTT);
@@ -140,12 +141,12 @@ static displayPort_t *displayPortHottInit()
return &hottDisplayPort;
}
-void hottDisplayportRegister()
+void hottDisplayportRegister(void)
{
cmsDisplayPortRegister(displayPortHottInit());
}
-void hottCmsOpen()
+void hottCmsOpen(void)
{
if (!cmsInMenu) {
cmsDisplayPortSelect(&hottDisplayPort);
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index bc6c564beb..52dff222f4 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -40,9 +40,12 @@
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
+#include "osd/osd.h"
+
#include "pg/vcd.h"
static displayPort_t mspDisplayPort;
+static serialPortIdentifier_e displayPortSerial;
static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len)
{
@@ -54,12 +57,12 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len
return 0;
}
#endif
- return mspSerialPush(displayPortProfileMsp()->displayPortSerial, cmd, buf, len, MSP_DIRECTION_REPLY);
+ return mspSerialPush(displayPortSerial, cmd, buf, len, MSP_DIRECTION_REPLY, MSP_V1);
}
static int heartbeat(displayPort_t *displayPort)
{
- uint8_t subcmd[] = { 0 };
+ uint8_t subcmd[] = { MSP_DP_HEARTBEAT };
// heartbeat is used to:
// a) ensure display is not released by MW OSD software
@@ -76,7 +79,7 @@ static int grab(displayPort_t *displayPort)
static int release(displayPort_t *displayPort)
{
- uint8_t subcmd[] = { 1 };
+ uint8_t subcmd[] = { MSP_DP_RELEASE };
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
@@ -85,14 +88,14 @@ static int clearScreen(displayPort_t *displayPort, displayClearOption_e options)
{
UNUSED(options);
- uint8_t subcmd[] = { 2 };
+ uint8_t subcmd[] = { MSP_DP_CLEAR_SCREEN };
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
static bool drawScreen(displayPort_t *displayPort)
{
- uint8_t subcmd[] = { 4 };
+ uint8_t subcmd[] = { MSP_DP_DRAW_SCREEN };
output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
return 0;
@@ -113,12 +116,12 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, uin
len = MSP_OSD_MAX_STRING_LENGTH;
}
- buf[0] = 3;
+ buf[0] = MSP_DP_WRITE_STRING;
buf[1] = row;
buf[2] = col;
- buf[3] = displayPortProfileMsp()->attrValues[attr] & ~DISPLAYPORT_MSP_ATTR_BLINK & DISPLAYPORT_MSP_ATTR_MASK;
+ buf[3] = displayPortProfileMsp()->fontSelection[attr & (DISPLAYPORT_SEVERITY_COUNT - 1)] & DISPLAYPORT_MSP_ATTR_FONT;
- if (attr & DISPLAYPORT_ATTR_BLINK) {
+ if (attr & DISPLAYPORT_BLINK) {
buf[3] |= DISPLAYPORT_MSP_ATTR_BLINK;
}
@@ -127,13 +130,25 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, uin
return output(displayPort, MSP_DISPLAYPORT, buf, len + 4);
}
+static int writeSys(displayPort_t *displayPort, uint8_t col, uint8_t row, displayPortSystemElement_e systemElement)
+{
+ uint8_t syscmd[4];
+
+ syscmd[0] = MSP_DP_SYS;
+ syscmd[1] = row;
+ syscmd[2] = col;
+ syscmd[3] = systemElement;
+
+ return output(displayPort, MSP_DISPLAYPORT, syscmd, sizeof(syscmd));
+}
+
static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8_t attr, uint8_t c)
{
char buf[2];
buf[0] = c;
buf[1] = 0;
- return writeString(displayPort, col, row, attr, buf); //!!TODO - check if there is a direct MSP command to do this
+ return writeString(displayPort, col, row, attr, buf);
}
static bool isTransferInProgress(const displayPort_t *displayPort)
@@ -150,9 +165,6 @@ static bool isSynced(const displayPort_t *displayPort)
static void redraw(displayPort_t *displayPort)
{
- const uint8_t displayRows = (vcdProfile()->video_system == VIDEO_SYSTEM_PAL) ? 16 : 13;
- displayPort->rows = displayRows + displayPortProfileMsp()->rowAdjust;
- displayPort->cols = 30 + displayPortProfileMsp()->colAdjust;
drawScreen(displayPort);
}
@@ -168,6 +180,7 @@ static const displayPortVTable_t mspDisplayPortVTable = {
.clearScreen = clearScreen,
.drawScreen = drawScreen,
.screenSize = screenSize,
+ .writeSys = writeSys,
.writeString = writeString,
.writeChar = writeChar,
.isTransferInProgress = isTransferInProgress,
@@ -188,7 +201,32 @@ displayPort_t *displayPortMspInit(void)
mspDisplayPort.useDeviceBlink = true;
}
+#ifndef USE_OSD_SD
+ if (vcdProfile()->video_system != VIDEO_SYSTEM_HD) {
+ vcdProfileMutable()->video_system = VIDEO_SYSTEM_HD;
+ }
+#endif
+#ifndef USE_OSD_HD
+ if (vcdProfile()->video_system == VIDEO_SYSTEM_HD) {
+ vcdProfileMutable()->video_system = VIDEO_SYSTEM_AUTO;
+ }
+#endif
+
+ if (vcdProfile()->video_system == VIDEO_SYSTEM_HD) {
+ mspDisplayPort.rows = osdConfig()->canvas_rows;
+ mspDisplayPort.cols = osdConfig()->canvas_cols;
+ } else {
+ const uint8_t displayRows = (vcdProfile()->video_system == VIDEO_SYSTEM_PAL) ? VIDEO_LINES_PAL : VIDEO_LINES_NTSC;
+ mspDisplayPort.rows = displayRows + displayPortProfileMsp()->rowAdjust;
+ mspDisplayPort.cols = OSD_SD_COLS + displayPortProfileMsp()->colAdjust;
+ }
+
redraw(&mspDisplayPort);
+
return &mspDisplayPort;
}
+
+void displayPortMspSetSerial(serialPortIdentifier_e serialPort) {
+ displayPortSerial = serialPort;
+}
#endif // USE_MSP_DISPLAYPORT
diff --git a/src/main/io/displayport_msp.h b/src/main/io/displayport_msp.h
index fac120576c..2d89616ad8 100644
--- a/src/main/io/displayport_msp.h
+++ b/src/main/io/displayport_msp.h
@@ -22,11 +22,28 @@
#include "drivers/display.h"
+#include "io/serial.h"
+
#include "pg/displayport_profiles.h"
+// MSP Display Port commands
+typedef enum {
+ MSP_DP_HEARTBEAT = 0, // Release the display after clearing and updating
+ MSP_DP_RELEASE = 1, // Release the display after clearing and updating
+ MSP_DP_CLEAR_SCREEN = 2, // Clear the display
+ MSP_DP_WRITE_STRING = 3, // Write a string at given coordinates
+ MSP_DP_DRAW_SCREEN = 4, // Trigger a screen draw
+ MSP_DP_OPTIONS = 5, // Not used by Betaflight. Reserved by Ardupilot and INAV
+ MSP_DP_SYS = 6, // Display system element displayportSystemElement_e at given coordinates
+ MSP_DP_COUNT,
+} displayportMspCommand_e;
+
// MSP displayport V2 attribute byte bit functions
#define DISPLAYPORT_MSP_ATTR_VERSION BIT(7) // Format indicator; must be zero for V2 (and V1)
#define DISPLAYPORT_MSP_ATTR_BLINK BIT(6) // Device local blink
-#define DISPLAYPORT_MSP_ATTR_MASK (~(DISPLAYPORT_MSP_ATTR_VERSION|DISPLAYPORT_MSP_ATTR_BLINK))
+#define DISPLAYPORT_MSP_ATTR_FONT (BIT(0) | BIT(1)) // Select bank of 256 characters as per displayPortSeverity_e
+#define DISPLAYPORT_MSP_ATTR_MASK (DISPLAYPORT_MSP_ATTR_VERSION | DISPLAYPORT_MSP_ATTR_BLINK | DISPLAYPORT_MSP_ATTR_FONT)
struct displayPort_s *displayPortMspInit(void);
+void displayPortMspSetSerial(serialPortIdentifier_e serialPort);
+
diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c
index 33a04dc989..d12767d479 100644
--- a/src/main/io/displayport_srxl.c
+++ b/src/main/io/displayport_srxl.c
@@ -71,15 +71,15 @@ static int srxlClearScreen(displayPort_t *displayPort, displayClearOption_e opti
UNUSED(options);
for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
- srxlWriteChar(displayPort, col, row, DISPLAYPORT_ATTR_NONE, ' ');
+ srxlWriteChar(displayPort, col, row, DISPLAYPORT_SEVERITY_NORMAL, ' ');
}
}
- srxlWriteString(displayPort, 1, 0, DISPLAYPORT_ATTR_NONE, "BETAFLIGHT");
+ srxlWriteString(displayPort, 1, 0, DISPLAYPORT_SEVERITY_NORMAL, "BETAFLIGHT");
if (displayPort->grabCount == 0) {
- srxlWriteString(displayPort, 0, 2, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT1);
- srxlWriteString(displayPort, 2, 3, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT2);
- srxlWriteString(displayPort, 2, 4, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT3);
+ srxlWriteString(displayPort, 0, 2, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT1);
+ srxlWriteString(displayPort, 2, 3, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT2);
+ srxlWriteString(displayPort, 2, 4, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT3);
}
return 0;
}
@@ -143,7 +143,7 @@ static const displayPortVTable_t srxlVTable = {
.layerCopy = NULL,
};
-static displayPort_t *displayPortSrxlInit()
+static displayPort_t *displayPortSrxlInit(void)
{
srxlDisplayPort.device = NULL;
displayInit(&srxlDisplayPort, &srxlVTable, DISPLAYPORT_DEVICE_TYPE_SRXL);
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index b38e7f86c9..3df1e74c9b 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -91,7 +91,7 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = {
SERIAL_PORT_USART7,
#endif
#ifdef USE_UART8
- SERIAL_PORT_USART8,
+ SERIAL_PORT_USART8,
#endif
#ifdef USE_UART9
SERIAL_PORT_UART9,
@@ -115,7 +115,7 @@ static uint8_t serialPortCount;
const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000,
400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000}; // see baudRate_e
-#define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0]))
+#define BAUD_RATE_COUNT ARRAYLEN(baudRates)
serialPortConfig_t *serialFindPortConfigurationMutable(serialPortIdentifier_e identifier)
{
@@ -158,12 +158,10 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig)
}
#endif
-#if defined(USE_VCP) && defined(USE_MSP_UART)
- if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) {
- serialPortConfig_t * uart1Config = serialFindPortConfigurationMutable(SERIAL_PORT_USART1);
- if (uart1Config) {
- uart1Config->functionMask = FUNCTION_MSP;
- }
+#if defined(USE_MSP_UART)
+ serialPortConfig_t * uart1Config = serialFindPortConfigurationMutable(USE_MSP_UART);
+ if (uart1Config) {
+ uart1Config->functionMask = FUNCTION_MSP;
}
#endif
@@ -205,7 +203,8 @@ serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e identi
return NULL;
}
-serialPortUsage_t *findSerialPortUsageByPort(serialPort_t *serialPort) {
+serialPortUsage_t *findSerialPortUsageByPort(serialPort_t *serialPort)
+{
uint8_t index;
for (index = 0; index < SERIAL_PORT_COUNT; index++) {
serialPortUsage_t *candidate = &serialPortUsageList[index];
@@ -271,9 +270,9 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s
}
#ifdef USE_TELEMETRY
-#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | TELEMETRY_PORT_FUNCTIONS_MASK)
+#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | TELEMETRY_PORT_FUNCTIONS_MASK | FUNCTION_VTX_MSP)
#else
-#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX)
+#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | FUNCTION_VTX_MSP)
#endif
bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck)
@@ -301,9 +300,16 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck)
}
uint8_t bitCount = BITCOUNT(portConfig->functionMask);
+
+#ifdef USE_VTX_MSP
+ if ((portConfig->functionMask & FUNCTION_VTX_MSP) && bitCount == 1) { // VTX MSP has to be shared with RX or MSP serial
+ return false;
+ }
+#endif
+
if (bitCount > 1) {
// shared
- if (bitCount > 2) {
+ if (bitCount > (BITCOUNT(FUNCTION_MSP | ALL_FUNCTIONS_SHARABLE_WITH_MSP))) {
return false;
}
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index ffcda088ea..a3de3fba52 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -50,7 +50,8 @@ typedef enum {
FUNCTION_RCDEVICE = (1 << 14), // 16384
FUNCTION_LIDAR_TF = (1 << 15), // 32768
FUNCTION_FRSKY_OSD = (1 << 16), // 65536
- FUNCTION_CO_PROCESSOR = (1 << 17),
+ FUNCTION_VTX_MSP = (1 << 17), // 131072
+ FUNCTION_CO_PROCESSOR = (1 << 18),
} serialPortFunction_e;
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
@@ -102,7 +103,6 @@ typedef enum {
extern const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT];
#define SERIAL_PORT_IDENTIFIER_TO_INDEX(x) (((x) < RESOURCE_SOFT_OFFSET) ? (x) : (RESOURCE_SOFT_OFFSET + ((x) - SERIAL_PORT_SOFTSERIAL1)))
-
#define SERIAL_PORT_IDENTIFIER_TO_UARTDEV(x) ((x) - SERIAL_PORT_USART1 + UARTDEV_1)
//
diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c
new file mode 100644
index 0000000000..96d5be031c
--- /dev/null
+++ b/src/main/io/vtx_msp.c
@@ -0,0 +1,391 @@
+/*
+ * 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 phobos- */
+
+#include
+#include
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
+
+#include "build/debug.h"
+
+#include "cms/cms_menu_vtx_msp.h"
+#include "common/crc.h"
+#include "config/feature.h"
+
+#include "drivers/vtx_common.h"
+#include "drivers/vtx_table.h"
+
+#include "fc/runtime_config.h"
+#include "flight/failsafe.h"
+
+#include "io/serial.h"
+#include "io/vtx_msp.h"
+#include "io/vtx_control.h"
+#include "io/vtx.h"
+
+#include "msp/msp_protocol.h"
+#include "msp/msp_serial.h"
+
+#include "pg/vtx_table.h"
+
+#include "rx/crsf.h"
+#include "rx/crsf_protocol.h"
+#include "rx/rx.h"
+
+#include "telemetry/msp_shared.h"
+
+static uint16_t mspConfFreq = 0;
+static uint8_t mspConfBand = 0;
+static uint8_t mspConfChannel = 0;
+static uint16_t mspConfPower = 0;
+static uint8_t mspConfPitMode = 0;
+static bool mspVtxConfigChanged = false;
+static timeUs_t mspVtxLastTimeUs = 0;
+static bool prevLowPowerDisarmedState = false;
+
+static const vtxVTable_t mspVTable; // forward
+static vtxDevice_t vtxMsp = {
+ .vTable = &mspVTable,
+};
+
+STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
+static uint8_t mspVtxPortIdentifier = 255;
+
+#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
+
+static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
+{
+ return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxRuntimeState.serialrxProvider == SERIALRX_CRSF;
+}
+
+static bool isLowPowerDisarmed(void)
+{
+ return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
+ (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
+ (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
+}
+
+void setMspVtxDeviceStatusReady(const int descriptor)
+{
+ if (mspVtxStatus != MSP_VTX_STATUS_READY && vtxTableConfig()->bands && vtxTableConfig()->channels && vtxTableConfig()->powerLevels) {
+#if defined(USE_MSP_OVER_TELEMETRY)
+ if (getMspSerialPortDescriptor(mspVtxPortIdentifier) == descriptor || getMspTelemetryDescriptor() == descriptor) {
+#else
+ if (getMspSerialPortDescriptor(mspVtxPortIdentifier) == descriptor) {
+#endif
+ mspVtxStatus = MSP_VTX_STATUS_READY;
+ }
+ }
+}
+
+void prepareMspFrame(uint8_t *mspFrame)
+{
+ mspFrame[0] = VTXDEV_MSP;
+ mspFrame[1] = vtxSettingsConfig()->band;
+ mspFrame[2] = vtxSettingsConfig()->channel;
+ mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
+ mspFrame[4] = mspConfPitMode;
+ mspFrame[5] = vtxSettingsConfig()->freq & 0xFF;
+ mspFrame[6] = (vtxSettingsConfig()->freq >> 8) & 0xFF;
+ mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
+ mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm;
+ mspFrame[9] = vtxSettingsConfig()->pitModeFreq & 0xFF;
+ mspFrame[10] = (vtxSettingsConfig()->pitModeFreq >> 8) & 0xFF;
+#ifdef USE_VTX_TABLE
+ mspFrame[11] = 1;
+ mspFrame[12] = vtxTableConfig()->bands;
+ mspFrame[13] = vtxTableConfig()->channels;
+ mspFrame[14] = vtxTableConfig()->powerLevels;
+#else
+ mspFrame[11] = 0;
+ mspFrame[12] = 0;
+ mspFrame[13] = 0;
+ mspFrame[14] = 0;
+#endif
+}
+
+static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
+{
+#ifndef USE_TELEMETRY_CRSF
+ UNUSED(mspCommand);
+ UNUSED(mspFrame);
+ UNUSED(mspFrameSize);
+#else
+ sbuf_t crsfPayloadBuf;
+ sbuf_t *dst = &crsfPayloadBuf;
+
+ uint8_t mspHeader[6] = {0x50, 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 over CRSF header
+ uint8_t mspChecksum;
+
+ mspChecksum = crc8_dvb_s2_update(0, &mspHeader[1], 5); // first character is not checksummable
+ mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize);
+
+ uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1; // add 1 for msp checksum
+ uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize;
+
+ uint8_t crsfFrame[crsfFrameSize];
+
+ dst->ptr = crsfFrame;
+ dst->end = ARRAYEND(crsfFrame);
+
+ sbufWriteU8(dst, CRSF_SYNC_BYTE);
+ sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
+ sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
+ sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection
+ sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
+ sbufWriteData(dst, mspHeader, sizeof(mspHeader));
+ sbufWriteData(dst, mspFrame, mspFrameSize);
+ sbufWriteU8(dst, mspChecksum);
+ crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
+ sbufSwitchToReader(dst, crsfFrame);
+
+ crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
+ crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
+ crsfRxSendTelemetryData();
+#endif
+}
+
+static uint16_t packetCounter = 0;
+
+static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
+{
+ UNUSED(vtxDevice);
+
+ const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_MSP);
+ uint8_t frame[15];
+
+ switch (mspVtxStatus) {
+ case MSP_VTX_STATUS_OFFLINE:
+ // wait for MSP communication from the VTX
+#ifdef USE_CMS
+ mspCmsUpdateStatusString();
+#endif
+ break;
+ case MSP_VTX_STATUS_READY:
+ if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
+ mspVtxConfigChanged = true;
+ prevLowPowerDisarmedState = isLowPowerDisarmed();
+ }
+
+ // send an update if stuff has changed with 200ms period
+ if (mspVtxConfigChanged && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
+
+ prepareMspFrame(frame);
+
+ if (isCrsfPortConfig(portConfig)) {
+ mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
+ } else {
+ mspSerialPush((serialPortIdentifier_e) portConfig->identifier, MSP_VTX_CONFIG, frame, sizeof(frame), MSP_DIRECTION_REPLY, MSP_V2_NATIVE);
+ }
+ packetCounter++;
+ mspVtxLastTimeUs = currentTimeUs;
+ mspVtxConfigChanged = false;
+
+#ifdef USE_CMS
+ mspCmsUpdateStatusString();
+#endif
+ }
+ break;
+ default:
+ mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
+ break;
+ }
+
+ DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
+ DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
+ DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
+#if defined(USE_MSP_OVER_TELEMETRY)
+ DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
+#else
+ DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
+#endif
+}
+
+static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
+{
+ UNUSED(vtxDevice);
+ return VTXDEV_MSP;
+}
+
+static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
+{
+ return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
+}
+
+static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
+{
+ UNUSED(vtxDevice);
+ if (band != mspConfBand || channel != mspConfChannel) {
+ mspVtxConfigChanged = true;
+ }
+ mspConfBand = band;
+ mspConfChannel = channel;
+}
+
+static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
+{
+ uint16_t powerValue;
+
+ if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) {
+ if (powerValue != mspConfPower) {
+ mspVtxConfigChanged = true;
+ }
+ mspConfPower = powerValue;
+ }
+}
+
+static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
+{
+ UNUSED(vtxDevice);
+ if (onoff != mspConfPitMode) {
+ mspVtxConfigChanged = true;
+ }
+ mspConfPitMode = onoff;
+}
+
+static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
+{
+ UNUSED(vtxDevice);
+ if (freq != mspConfFreq) {
+ mspVtxConfigChanged = true;
+ }
+ mspConfFreq = freq;
+}
+
+static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
+{
+ if (!vtxMspIsReady(vtxDevice)) {
+ return false;
+ }
+
+ *pBand = mspConfBand;
+ *pChannel = mspConfChannel;
+ return true;
+}
+
+static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
+{
+ if (!vtxMspIsReady(vtxDevice)) {
+ return false;
+ }
+
+ // Special case, power not set
+ if (mspConfPower == 0) {
+ *pIndex = 0;
+ return true;
+ }
+
+ // Lookup value in table
+ for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
+ // Find value that matches current configured power level
+ if (mspConfPower == vtxTablePowerValues[i]) {
+ // Value found, return index
+ *pIndex = i + 1;
+ return true;
+ }
+ }
+
+ // Value not found in table
+ return false;
+}
+
+static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+{
+ if (!vtxMspIsReady(vtxDevice)) {
+ return false;
+ }
+
+ *pFreq = vtxCommonLookupFrequency(vtxDevice, mspConfBand, mspConfChannel);
+ return true;
+}
+
+static bool vtxMspGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
+{
+ if (!vtxMspIsReady(vtxDevice)) {
+ return false;
+ }
+
+ // Mirror configued pit mode state rather than use current pitmode as we
+ // should, otherwise the logic in vtxProcessPitMode may not get us to the
+ // correct state if pitmode is toggled quickly
+ *status = (mspConfPitMode ? VTX_STATUS_PIT_MODE : 0);
+
+ return true;
+}
+
+static uint8_t vtxMspGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers)
+{
+ if (!vtxMspIsReady(vtxDevice)) {
+ return 0;
+ }
+
+ for (uint8_t i = 0; i < vtxTablePowerLevels; i++) {
+ levels[i] = vtxTablePowerValues[i];
+ uint16_t power = (uint16_t)powf(10.0f, levels[i] / 10.0f);
+ powers[i] = power;
+ }
+
+ return vtxTablePowerLevels;
+}
+
+static const vtxVTable_t mspVTable = {
+ .process = vtxMspProcess,
+ .getDeviceType = vtxMspGetDeviceType,
+ .isReady = vtxMspIsReady,
+ .setBandAndChannel = vtxMspSetBandAndChannel,
+ .setPowerByIndex = vtxMspSetPowerByIndex,
+ .setPitMode = vtxMspSetPitMode,
+ .setFrequency = vtxMspSetFreq,
+ .getBandAndChannel = vtxMspGetBandAndChannel,
+ .getPowerIndex = vtxMspGetPowerIndex,
+ .getFrequency = vtxMspGetFreq,
+ .getStatus = vtxMspGetStatus,
+ .getPowerLevels = vtxMspGetPowerLevels,
+ .serializeCustomDeviceStatus = NULL,
+};
+
+bool vtxMspInit(void)
+{
+ // don't bother setting up this device if we don't have MSP vtx enabled
+ const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_MSP);
+ if (!portConfig) {
+ return false;
+ }
+
+ mspVtxPortIdentifier = portConfig->identifier;
+
+ // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
+#if defined(USE_VTX_COMMON)
+ vtxCommonSetDevice(&vtxMsp);
+#endif
+
+ vtxInit();
+
+ return true;
+}
+
+#endif
diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h
new file mode 100644
index 0000000000..c769b181ed
--- /dev/null
+++ b/src/main/io/vtx_msp.h
@@ -0,0 +1,35 @@
+/*
+ * 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
+
+#include "build/build_config.h"
+
+typedef enum {
+ // Offline - device hasn't responded yet
+ MSP_VTX_STATUS_OFFLINE = 0,
+ MSP_VTX_STATUS_READY,
+} mspVtxStatus_e;
+
+bool vtxMspInit(void);
+void setMspVtxDeviceStatusReady(const int descriptor);
+void prepareMspFrame(uint8_t *mspFrame);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index d2b60a254d..a4abb8ba56 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -105,6 +105,7 @@
#include "io/usb_msc.h"
#include "io/vtx_control.h"
#include "io/vtx.h"
+#include "io/vtx_msp.h"
#include "msp/msp_box.h"
#include "msp/msp_protocol.h"
@@ -123,6 +124,9 @@
#include "pg/motor.h"
#include "pg/rx.h"
#include "pg/rx_spi.h"
+#ifdef USE_RX_EXPRESSLRS
+#include "pg/rx_spi_expresslrs.h"
+#endif
#include "pg/usb.h"
#include "pg/vcd.h"
#include "pg/vtx_table.h"
@@ -134,11 +138,11 @@
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
+#include "sensors/adcinternal.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
-#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/gyro_init.h"
#include "sensors/rangefinder.h"
@@ -218,10 +222,12 @@ mspDescriptor_t mspDescriptorAlloc(void)
static uint32_t mspArmingDisableFlags = 0;
+#ifndef SIMULATOR_BUILD
static void mspArmingDisableByDescriptor(mspDescriptor_t desc)
{
mspArmingDisableFlags |= (1 << desc);
}
+#endif
static void mspArmingEnableByDescriptor(mspDescriptor_t desc)
{
@@ -238,7 +244,7 @@ static bool mspIsMspArmingEnabled(void)
static uint8_t mspPassthroughMode;
static uint8_t mspPassthroughArgument;
-#ifdef USE_ESCSERIAL
+#if defined(USE_ESCSERIAL) && defined(USE_SERIAL_4WAY_BLHELI_INTERFACE)
static void mspEscPassthroughFn(serialPort_t *serialPort)
{
escEnablePassthrough(serialPort, &motorConfig()->dev, mspPassthroughArgument, mspPassthroughMode);
@@ -327,7 +333,7 @@ static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessF
}
FALLTHROUGH;
#endif // USE_ESCSERIAL
-#endif //USE_SERIAL_4WAY_BLHELI_INTERFACE
+#endif // USE_SERIAL_4WAY_BLHELI_INTERFACE
default:
sbufWriteU8(dst, 0);
}
@@ -663,18 +669,11 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
#if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
targetCapabilities |= BIT(TARGET_HAS_SOFTSERIAL);
#endif
-#if defined(USE_UNIFIED_TARGET)
targetCapabilities |= BIT(TARGET_IS_UNIFIED);
-#endif
#if defined(USE_FLASH_BOOT_LOADER)
targetCapabilities |= BIT(TARGET_HAS_FLASH_BOOTLOADER);
#endif
-#if defined(USE_CUSTOM_DEFAULTS)
- targetCapabilities |= BIT(TARGET_SUPPORTS_CUSTOM_DEFAULTS);
- if (hasCustomDefaults()) {
- targetCapabilities |= BIT(TARGET_HAS_CUSTOM_DEFAULTS);
- }
-#endif
+
#if defined(USE_RX_BIND)
if (getRxBindSupported()) {
targetCapabilities |= BIT(TARGET_SUPPORTS_RX_BIND);
@@ -937,6 +936,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
break;
}
+#if defined(USE_OSD)
case MSP_OSD_CONFIG: {
#define OSD_FLAGS_OSD_FEATURE (1 << 0)
//#define OSD_FLAGS_OSD_SLAVE (1 << 1)
@@ -944,9 +944,10 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
#define OSD_FLAGS_OSD_HARDWARE_FRSKYOSD (1 << 3)
#define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
#define OSD_FLAGS_OSD_DEVICE_DETECTED (1 << 5)
+#define OSD_FLAGS_OSD_MSP_DEVICE (1 << 6)
uint8_t osdFlags = 0;
-#if defined(USE_OSD)
+
osdFlags |= OSD_FLAGS_OSD_FEATURE;
osdDisplayPortDevice_e deviceType;
@@ -966,21 +967,27 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
}
+ break;
+ case OSD_DISPLAYPORT_DEVICE_MSP:
+ osdFlags |= OSD_FLAGS_OSD_MSP_DEVICE;
+ if (displayIsReady) {
+ osdFlags |= OSD_FLAGS_OSD_DEVICE_DETECTED;
+ }
+
break;
default:
break;
}
-#endif
+
sbufWriteU8(dst, osdFlags);
-#ifdef USE_MAX7456
- // send video system (AUTO/PAL/NTSC)
+#ifdef USE_OSD_SD
+ // send video system (AUTO/PAL/NTSC/HD)
sbufWriteU8(dst, vcdProfile()->video_system);
#else
- sbufWriteU8(dst, 0);
-#endif
+ sbufWriteU8(dst, VIDEO_SYSTEM_HD);
+#endif // USE_OSD_SD
-#ifdef USE_OSD
// OSD specific, not applicable to OSD slaves.
// Configuration
@@ -1043,7 +1050,15 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
sbufWriteU8(dst, osdConfig()->camera_frame_width);
sbufWriteU8(dst, osdConfig()->camera_frame_height);
+ break;
+ }
#endif // USE_OSD
+
+ case MSP_OSD_CANVAS: {
+#ifdef USE_OSD
+ sbufWriteU8(dst, osdConfig()->canvas_cols);
+ sbufWriteU8(dst, osdConfig()->canvas_rows);
+#endif
break;
}
@@ -1053,10 +1068,14 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
return true;
}
-static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
+static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *dst)
{
bool unsupportedCommand = false;
+#if !defined(USE_VTX_COMMON) || !defined(USE_VTX_MSP)
+ UNUSED(srcDesc);
+#endif
+
switch (cmdMSP) {
case MSP_STATUS_EX:
case MSP_STATUS:
@@ -1098,6 +1117,14 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
// config state flags - bits to indicate the state of the configuration, reboot required, etc.
// other flags can be added as needed
sbufWriteU8(dst, (getRebootRequired() << 0));
+
+ // Added in API version 1.46
+ // Write CPU temp
+#ifdef USE_ADC_INTERNAL
+ sbufWriteU16(dst, getCoreTemperatureCelsius());
+#else
+ sbufWriteU16(dst, 0);
+#endif
}
break;
@@ -1138,11 +1165,11 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
}
break;
- case MSP_NAME:
+case MSP_NAME:
{
- const int nameLen = strlen(pilotConfig()->name);
+ const int nameLen = strlen(pilotConfig()->craftName);
for (int i = 0; i < nameLen; i++) {
- sbufWriteU8(dst, pilotConfig()->name[i]);
+ sbufWriteU8(dst, pilotConfig()->craftName[i]);
}
}
break;
@@ -1206,14 +1233,35 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
- rpm = (int)getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount;
+ rpm = erpmToRpm(getDshotTelemetry(i));
rpmDataAvailable = true;
invalidPct = 10000; // 100.00%
+
+
#ifdef USE_DSHOT_TELEMETRY_STATS
if (isDshotMotorTelemetryActive(i)) {
invalidPct = getDshotTelemetryMotorInvalidPercent(i);
}
#endif
+
+
+ // Provide extended dshot telemetry
+ if ((dshotTelemetryState.motorState[i].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0) {
+ // Temperature Celsius [0, 1, ..., 255] in degree Celsius, just like Blheli_32 and KISS
+ if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) {
+ escTemperature = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE];
+ }
+
+ // Current -> 0-255A step 1A
+ if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) != 0) {
+ escCurrent = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT];
+ }
+
+ // Voltage -> 0-63,75V step 0,25V
+ if ((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) != 0) {
+ escVoltage = dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] >> 2;
+ }
+ }
}
#endif
@@ -1221,7 +1269,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
escSensorData_t *escData = getEscSensorData(i);
if (!rpmDataAvailable) { // We want DSHOT telemetry RPM data (if available) to have precedence
- rpm = calcEscRpm(escData->rpm);
+ rpm = erpmToRpm(escData->rpm);
rpmDataAvailable = true;
}
escTemperature = escData->temperature;
@@ -1270,9 +1318,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
const uint8_t warningsLen = strlen(warningsBuffer);
if (isBlinking) {
- displayAttr |= DISPLAYPORT_ATTR_BLINK;
+ displayAttr |= DISPLAYPORT_BLINK;
}
- sbufWriteU8(dst, displayAttr); // see displayPortAttr_e
+ sbufWriteU8(dst, displayAttr); // see displayPortSeverity_e
sbufWriteU8(dst, warningsLen); // length byte followed by the actual characters
for (unsigned i = 0; i < warningsLen; i++) {
sbufWriteU8(dst, warningsBuffer[i]);
@@ -1328,10 +1376,10 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
for (int i = 0 ; i < 3; i++) {
sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
}
- sbufWriteU8(dst, currentControlRateProfile->tpa_rate);
+ sbufWriteU8(dst, 0); // was currentControlRateProfile->tpa_rate
sbufWriteU8(dst, currentControlRateProfile->thrMid8);
sbufWriteU8(dst, currentControlRateProfile->thrExpo8);
- sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint);
+ sbufWriteU16(dst, 0); // was currentControlRateProfile->tpa_breakpoint
sbufWriteU8(dst, currentControlRateProfile->rcExpo[FD_YAW]);
sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_YAW]);
sbufWriteU8(dst, currentControlRateProfile->rcRates[FD_PITCH]);
@@ -1426,9 +1474,10 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#endif
break;
-#if defined(USE_ESC_SENSOR)
// Deprecated in favor of MSP_MOTOR_TELEMETY as of API version 1.42
+ // Used by DJI FPV
case MSP_ESC_SENSOR_DATA:
+#if defined(USE_ESC_SENSOR)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
sbufWriteU8(dst, getMotorCount());
for (int i = 0; i < getMotorCount(); i++) {
@@ -1436,12 +1485,23 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, escData->temperature);
sbufWriteU16(dst, escData->rpm);
}
- } else {
+ } else
+#endif
+#if defined(USE_DSHOT_TELEMETRY)
+ if (motorConfig()->dev.useDshotTelemetry) {
+ sbufWriteU8(dst, getMotorCount());
+ for (int i = 0; i < getMotorCount(); i++) {
+ sbufWriteU8(dst, dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]);
+ sbufWriteU16(dst, getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount);
+ }
+ }
+ else
+#endif
+ {
unsupportedCommand = true;
}
break;
-#endif
#ifdef USE_GPS
case MSP_GPS_CONFIG:
@@ -1462,13 +1522,13 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, (uint16_t)constrain(gpsSol.llh.altCm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again.
sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, gpsSol.groundCourse);
- // Added in API version 1.44
+ // Added in API version 1.44
sbufWriteU16(dst, gpsSol.hdop);
break;
case MSP_COMP_GPS:
sbufWriteU16(dst, GPS_distanceToHome);
- sbufWriteU16(dst, GPS_directionToHome);
+ sbufWriteU16(dst, GPS_directionToHome / 10); // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD
sbufWriteU8(dst, GPS_update & 1);
break;
@@ -1493,6 +1553,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
sbufWriteU8(dst, gpsRescueConfig()->sanityChecks);
sbufWriteU8(dst, gpsRescueConfig()->minSats);
+
// Added in API version 1.43
sbufWriteU16(dst, gpsRescueConfig()->ascendRate);
sbufWriteU16(dst, gpsRescueConfig()->descendRate);
@@ -1578,6 +1639,15 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#else
sbufWriteU8(dst, 0);
#endif
+
+ // Added in MSP API 1.45
+#ifdef USE_RX_EXPRESSLRS
+ sbufWriteData(dst, rxExpressLrsSpiConfig()->UID, sizeof(rxExpressLrsSpiConfig()->UID));
+#else
+ uint8_t emptyUid[6];
+ memset(emptyUid, 0, sizeof(emptyUid));
+ sbufWriteData(dst, &emptyUid, sizeof(emptyUid));
+#endif
break;
case MSP_FAILSAFE_CONFIG:
sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
@@ -1706,6 +1776,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, blackboxGetRateDenom());
sbufWriteU16(dst, blackboxGetPRatio());
sbufWriteU8(dst, blackboxConfig()->sample_rate);
+ // Added in MSP API 1.45
+ sbufWriteU32(dst, blackboxConfig()->fields_disabled_mask);
#else
sbufWriteU8(dst, 0); // Blackbox not supported
sbufWriteU8(dst, 0);
@@ -1713,6 +1785,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU8(dst, 0);
+ // Added in MSP API 1.45
+ sbufWriteU32(dst, 0);
#endif
break;
@@ -1880,7 +1954,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit);
sbufWriteU8(dst, currentPidProfile->levelAngleLimit);
sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
- sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
+ sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold
sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight
sbufWriteU8(dst, currentPidProfile->iterm_rotation);
@@ -1910,8 +1984,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F);
sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F);
sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F);
-
- sbufWriteU8(dst, currentPidProfile->antiGravityMode);
+ sbufWriteU8(dst, 0); // was currentPidProfile->antiGravityMode
#if defined(USE_D_MIN)
sbufWriteU8(dst, currentPidProfile->d_min[PID_ROLL]);
sbufWriteU8(dst, currentPidProfile->d_min[PID_PITCH]);
@@ -1973,19 +2046,28 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
break;
case MSP_SENSOR_CONFIG:
#if defined(USE_ACC)
- sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
+ // Changed with API 1.46
+ sbufWriteU8(dst, accelerometerConfig()->acc_hardware == ACC_DEFAULT ? detectedSensors[1] : accelerometerConfig()->acc_hardware);
#else
sbufWriteU8(dst, 0);
#endif
#ifdef USE_BARO
- sbufWriteU8(dst, barometerConfig()->baro_hardware);
+ // Changed with API 1.46
+ sbufWriteU8(dst, barometerConfig()->baro_hardware == BARO_DEFAULT ? detectedSensors[2] : barometerConfig()->baro_hardware);
#else
sbufWriteU8(dst, BARO_NONE);
#endif
#ifdef USE_MAG
- sbufWriteU8(dst, compassConfig()->mag_hardware);
+ // Changed with API 1.46
+ sbufWriteU8(dst, compassConfig()->mag_hardware == MAG_DEFAULT ? detectedSensors[3] : compassConfig()->mag_hardware);
#else
sbufWriteU8(dst, MAG_NONE);
+#endif
+ // Added in MSP API 1.46
+#ifdef USE_RANGEFINDER
+ sbufWriteU8(dst, rangefinderConfig()->rangefinder_hardware); // no RANGEFINDER_DEFAULT value
+#else
+ sbufWriteU8(dst, RANGEFINDER_NONE);
#endif
break;
@@ -2023,7 +2105,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
-
+#ifdef USE_VTX_MSP
+ setMspVtxDeviceStatusReady(srcDesc);
+#endif
}
break;
#endif
@@ -2301,6 +2385,9 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
} else {
return MSP_RESULT_ERROR;
}
+#ifdef USE_VTX_MSP
+ setMspVtxDeviceStatusReady(srcDesc);
+#endif
}
break;
@@ -2317,6 +2404,9 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
} else {
return MSP_RESULT_ERROR;
}
+#ifdef USE_VTX_MSP
+ setMspVtxDeviceStatusReady(srcDesc);
+#endif
}
break;
#endif // USE_VTX_TABLE
@@ -2407,25 +2497,14 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
case MSP_RESET_CONF:
{
-#if defined(USE_CUSTOM_DEFAULTS)
- defaultsType_e defaultsType = DEFAULTS_TYPE_CUSTOM;
-#endif
if (sbufBytesRemaining(src) >= 1) {
// Added in MSP API 1.42
-#if defined(USE_CUSTOM_DEFAULTS)
- defaultsType = sbufReadU8(src);
-#else
sbufReadU8(src);
-#endif
}
bool success = false;
if (!ARMING_FLAG(ARMED)) {
-#if defined(USE_CUSTOM_DEFAULTS)
- success = resetEEPROM(defaultsType == DEFAULTS_TYPE_CUSTOM);
-#else
- success = resetEEPROM(false);
-#endif
+ success = resetEEPROM();
if (success && mspPostProcessFn) {
rebootMode = MSP_REBOOT_FIRMWARE;
@@ -2438,6 +2517,50 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_
}
break;
+
+ case MSP2_GET_TEXT:
+ {
+ // type byte, then length byte followed by the actual characters
+ const uint8_t textType = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
+
+ const char *textVar;
+
+ switch (textType) {
+ case MSP2TEXT_PILOT_NAME:
+ textVar = pilotConfigMutable()->pilotName;
+ break;
+
+ case MSP2TEXT_CRAFT_NAME:
+ textVar = pilotConfigMutable()->craftName;
+ break;
+
+ case MSP2TEXT_PID_PROFILE_NAME:
+ textVar = currentPidProfile->profileName;
+ break;
+
+ case MSP2TEXT_RATE_PROFILE_NAME:
+ textVar = currentControlRateProfile->profileName;
+ break;
+
+ case MSP2TEXT_BUILDKEY:
+ textVar = "";
+ break;
+
+ default:
+ return MSP_RESULT_ERROR;
+ }
+
+ const uint8_t textLength = strlen(textVar);
+
+ // type byte, then length byte followed by the actual characters
+ sbufWriteU8(dst, textType);
+ sbufWriteU8(dst, textLength);
+ for (unsigned int i = 0; i < textLength; i++) {
+ sbufWriteU8(dst, textVar[i]);
+ }
+ }
+ break;
+
default:
return MSP_RESULT_CMD_UNKNOWN;
}
@@ -2614,11 +2737,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
currentControlRateProfile->rates[i] = sbufReadU8(src);
}
- value = sbufReadU8(src);
+ value = sbufReadU8(src);
currentControlRateProfile->tpa_rate = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX);
- currentControlRateProfile->thrMid8 = sbufReadU8(src);
+ currentControlRateProfile->thrMid8 = sbufReadU8(src);
currentControlRateProfile->thrExpo8 = sbufReadU8(src);
- currentControlRateProfile->tpa_breakpoint = sbufReadU16(src);
+ currentControlRateProfile->tpa_breakpoint = sbufReadU16(src); // tpa_breakpoint is moved to PID profile
if (sbufBytesRemaining(src) >= 1) {
currentControlRateProfile->rcExpo[FD_YAW] = sbufReadU8(src);
@@ -2971,7 +3094,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src); // was pidProfile.levelSensitivity
}
if (sbufBytesRemaining(src) >= 4) {
- currentPidProfile->itermThrottleThreshold = sbufReadU16(src);
+ currentPidProfile->itermThrottleThreshold = sbufReadU16(src);// was currentPidProfile->itermThrottleThreshold
currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
}
if (sbufBytesRemaining(src) >= 2) {
@@ -3008,7 +3131,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src);
currentPidProfile->pid[PID_YAW].F = sbufReadU16(src);
- currentPidProfile->antiGravityMode = sbufReadU8(src);
+ currentPidProfile->antiGravityMode=sbufReadU8(src); // was currentPidProfile->antiGravityMode
}
if (sbufBytesRemaining(src) >= 7) {
// Added in MSP API 1.41
@@ -3078,6 +3201,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src);
#endif
}
+ if (sbufBytesRemaining(src) >= 4) {
+ // Added in API 1.45
+ sbufReadU8(src);
+ sbufReadU8(src);
+ sbufReadU16(src);
+ }
+
pidInitConfig(currentPidProfile);
initEscEndpoints();
mixerInitProfile();
@@ -3159,6 +3289,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
// sample_rate not specified in MSP, so calculate it from old p_ratio
blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(pRatio);
}
+
+ // Added in MSP API 1.45
+ if (sbufBytesRemaining(src) >= 4) {
+ blackboxConfigMutable()->fields_disabled_mask = sbufReadU32(src);
+ }
}
break;
#endif
@@ -3254,6 +3389,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src);
#endif
}
+#ifdef USE_VTX_MSP
+ setMspVtxDeviceStatusReady(srcDesc);
+#endif
}
break;
#endif
@@ -3301,6 +3439,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
} else {
return MSP_RESULT_ERROR;
}
+#ifdef USE_VTX_MSP
+ setMspVtxDeviceStatusReady(srcDesc);
+#endif
}
break;
@@ -3325,6 +3466,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
} else {
return MSP_RESULT_ERROR;
}
+#ifdef USE_VTX_MSP
+ setMspVtxDeviceStatusReady(srcDesc);
+#endif
}
break;
#endif
@@ -3408,11 +3552,13 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
disableRunawayTakeoff = sbufReadU8(src);
}
if (command) {
+#ifndef SIMULATOR_BUILD // In simulator mode we can safely arm with MSP link.
mspArmingDisableByDescriptor(srcDesc);
setArmingDisabled(ARMING_DISABLED_MSP);
if (ARMING_FLAG(ARMED)) {
disarm(DISARM_REASON_ARMING_DISABLED);
}
+#endif
#ifdef USE_RUNAWAY_TAKEOFF
runawayTakeoffTemporaryDisable(false);
#endif
@@ -3428,7 +3574,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
}
break;
-#ifdef USE_FLASHFS
+#if defined(USE_FLASHFS) && defined(USE_BLACKBOX)
case MSP_DATAFLASH_ERASE:
blackboxEraseAll();
@@ -3552,6 +3698,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_mode, sbufReadU8(src));
#else
sbufReadU8(src);
+#endif
+ }
+ if (sbufBytesRemaining(src) >= 6) {
+ // Added in MSP API 1.45
+#ifdef USE_RX_EXPRESSLRS
+ sbufReadData(src, rxExpressLrsSpiConfigMutable()->UID, 6);
+#else
+ uint8_t emptyUid[6];
+ sbufReadData(src, emptyUid, 6);
#endif
}
break;
@@ -3693,9 +3848,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#endif
case MSP_SET_NAME:
- memset(pilotConfigMutable()->name, 0, ARRAYLEN(pilotConfig()->name));
+ memset(pilotConfigMutable()->craftName, 0, ARRAYLEN(pilotConfig()->craftName));
for (unsigned int i = 0; i < MIN(MAX_NAME_LENGTH, dataSize); i++) {
- pilotConfigMutable()->name[i] = sbufReadU8(src);
+ pilotConfigMutable()->craftName[i] = sbufReadU8(src);
}
#ifdef USE_OSD
osdAnalyzeActiveElements();
@@ -3772,6 +3927,48 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
break;
#endif
+
+ case MSP2_SET_TEXT:
+ {
+ // type byte, then length byte followed by the actual characters
+ const uint8_t textType = sbufReadU8(src);
+
+ char* textVar;
+ const uint8_t textLength = MIN(MAX_NAME_LENGTH, sbufReadU8(src));
+ switch (textType) {
+ case MSP2TEXT_PILOT_NAME:
+ textVar = pilotConfigMutable()->pilotName;
+ break;
+
+ case MSP2TEXT_CRAFT_NAME:
+ textVar = pilotConfigMutable()->craftName;
+ break;
+
+ case MSP2TEXT_PID_PROFILE_NAME:
+ textVar = currentPidProfile->profileName;
+ break;
+
+ case MSP2TEXT_RATE_PROFILE_NAME:
+ textVar = currentControlRateProfile->profileName;
+ break;
+
+ default:
+ return MSP_RESULT_ERROR;
+ }
+
+ memset(textVar, 0, strlen(textVar));
+ for (unsigned int i = 0; i < textLength; i++) {
+ textVar[i] = sbufReadU8(src);
+ }
+
+#ifdef USE_OSD
+ if (textType == MSP2TEXT_PILOT_NAME || textType == MSP2TEXT_CRAFT_NAME) {
+ osdAnalyzeActiveElements();
+ }
+#endif
+ }
+ break;
+
default:
// we do not know how to handle the (valid) message, indicate error MSP $M!
return MSP_RESULT_ERROR;
@@ -3893,12 +4090,22 @@ static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cm
if ((int8_t)addr == -1) {
/* Set general OSD settings */
-#ifdef USE_MAX7456
- vcdProfileMutable()->video_system = sbufReadU8(src);
-#else
- sbufReadU8(src); // Skip video system
+ videoSystem_e video_system = sbufReadU8(src);
+#ifndef USE_OSD_HD
+ if (video_system == VIDEO_SYSTEM_HD) {
+ video_system = VIDEO_SYSTEM_AUTO;
+ }
#endif
-#if defined(USE_OSD)
+
+ if ((video_system == VIDEO_SYSTEM_HD) && (vcdProfile()->video_system != VIDEO_SYSTEM_HD)) {
+ // If switching to HD, don't wait for the VTX to communicate the correct resolution, just
+ // increase the canvas size to the HD default as that is what the user will expect
+ osdConfigMutable()->canvas_cols = OSD_HD_COLS;
+ osdConfigMutable()->canvas_rows = OSD_HD_ROWS;
+ }
+
+ vcdProfileMutable()->video_system = video_system;
+
osdConfigMutable()->units = sbufReadU8(src);
// Alarms
@@ -3946,19 +4153,16 @@ static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cm
osdConfigMutable()->camera_frame_width = sbufReadU8(src);
osdConfigMutable()->camera_frame_height = sbufReadU8(src);
}
-#endif
} else if ((int8_t)addr == -2) {
-#if defined(USE_OSD)
// Timers
uint8_t index = sbufReadU8(src);
if (index > OSD_TIMER_COUNT) {
return MSP_RESULT_ERROR;
}
osdConfigMutable()->timers[index] = sbufReadU16(src);
-#endif
+
return MSP_RESULT_ERROR;
} else {
-#if defined(USE_OSD)
const uint16_t value = sbufReadU16(src);
/* Get screen index, 0 is post flight statistics, 1 and above are in flight OSD screens */
@@ -3974,9 +4178,6 @@ static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cm
} else {
return MSP_RESULT_ERROR;
}
-#else
- return MSP_RESULT_ERROR;
-#endif
}
}
break;
@@ -4018,6 +4219,27 @@ static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cm
}
}
break;
+
+#ifdef USE_OSD_HD
+ case MSP_SET_OSD_CANVAS:
+ {
+ osdConfigMutable()->canvas_cols = sbufReadU8(src);
+ osdConfigMutable()->canvas_rows = sbufReadU8(src);
+
+ if ((vcdProfile()->video_system != VIDEO_SYSTEM_HD) ||
+ (osdConfig()->displayPortDevice != OSD_DISPLAYPORT_DEVICE_MSP)) {
+ // An HD VTX has communicated it's canvas size, so we must be in HD mode
+ vcdProfileMutable()->video_system = VIDEO_SYSTEM_HD;
+ // And using MSP displayport
+ osdConfigMutable()->displayPortDevice = OSD_DISPLAYPORT_DEVICE_MSP;
+
+ // Save settings and reboot or the user won't see the effect and will have to manually save
+ writeEEPROM();
+ systemReset();
+ }
+ }
+ break;
+#endif //USE_OSD_HD
#endif // OSD
default:
@@ -4040,7 +4262,7 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa
if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
ret = MSP_RESULT_ACK;
- } else if (mspProcessOutCommand(cmdMSP, dst)) {
+ } else if (mspProcessOutCommand(srcDesc, cmdMSP, dst)) {
ret = MSP_RESULT_ACK;
} else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
/* ret */;
diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c
index 7471d99c7e..88af1fa5cd 100644
--- a/src/main/msp/msp_box.c
+++ b/src/main/msp/msp_box.c
@@ -100,6 +100,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOXMSPOVERRIDE, "MSP OVERRIDE", 50},
{ BOXSTICKCOMMANDDISABLE, "STICK COMMANDS DISABLE", 51},
{ BOXBEEPERMUTE, "BEEPER MUTE", 52},
+ { BOXREADY, "READY", 53},
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -290,7 +291,7 @@ void initActiveBoxIds(void)
BME(BOXCAMERA3);
#endif
-#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
BME(BOXVTXPITMODE);
BME(BOXVTXCONTROLDISABLE);
#endif
@@ -340,6 +341,7 @@ void initActiveBoxIds(void)
#endif
BME(BOXSTICKCOMMANDDISABLE);
+ BME(BOXREADY);
#undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index 7cd59b94a2..dac513b187 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -62,7 +62,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
-#define API_VERSION_MINOR 44 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+#define API_VERSION_MINOR 46 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_LENGTH 2
@@ -235,6 +235,9 @@
#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware
#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware
+#define MSP_SET_OSD_CANVAS 188 // in message Set osd canvas size COLSxROWS
+#define MSP_OSD_CANVAS 189 // out message Get osd canvas size COLSxROWS
+
//
// Multwii original MSP commands
//
@@ -317,7 +320,7 @@
#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
-#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats
+#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed and sanityChecks
#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P
#define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time)
#define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time)
@@ -333,7 +336,7 @@
#define MSP_V2_FRAME 255 //MSPv2 payload indicator
// Additional commands that are not compatible with MultiWii
-#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc
+#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, CPU temperature, sensor present etc
#define MSP_UID 160 //out message Unique device ID
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data
diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h
index f4dceb6ffd..a2906c95a4 100644
--- a/src/main/msp/msp_protocol_v2_betaflight.h
+++ b/src/main/msp/msp_protocol_v2_betaflight.h
@@ -24,3 +24,12 @@
#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
+#define MSP2_GET_TEXT 0x3006
+#define MSP2_SET_TEXT 0x3007
+
+// MSP2_SET_TEXT and MSP2_GET_TEXT variable types
+#define MSP2TEXT_PILOT_NAME 1
+#define MSP2TEXT_CRAFT_NAME 2
+#define MSP2TEXT_PID_PROFILE_NAME 3
+#define MSP2TEXT_RATE_PROFILE_NAME 4
+#define MSP2TEXT_BUILDKEY 5
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 86e82d9ea3..08b1c0ba6d 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -40,6 +40,8 @@
#include "msp_serial.h"
+#include "pg/msp.h"
+
static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, bool sharedWithTelemetry)
@@ -63,7 +65,13 @@ void mspSerialAllocatePorts(void)
continue;
}
- serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+ portOptions_e options = SERIAL_NOT_INVERTED;
+
+ if (mspConfig()->halfDuplex) {
+ options |= SERIAL_BIDIR;
+ }
+
+ serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, options);
if (serialPort) {
bool sharedWithTelemetry = isSerialPortShared(portConfig, FUNCTION_MSP, TELEMETRY_PORT_FUNCTIONS_MASK);
resetMspPort(mspPort, serialPort, sharedWithTelemetry);
@@ -86,8 +94,20 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
}
}
+mspDescriptor_t getMspSerialPortDescriptor(const uint8_t portIdentifier)
+{
+ for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+ mspPort_t *candidateMspPort = &mspPorts[portIndex];
+ if (candidateMspPort->port->identifier == portIdentifier) {
+ return candidateMspPort->descriptor;
+ }
+ }
+ return -1;
+}
+
#if defined(USE_TELEMETRY)
-void mspSerialReleaseSharedTelemetryPorts(void) {
+void mspSerialReleaseSharedTelemetryPorts(void)
+{
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
mspPort_t *candidateMspPort = &mspPorts[portIndex];
if (candidateMspPort->sharedWithTelemetry) {
@@ -554,7 +574,7 @@ void mspSerialInit(void)
mspSerialAllocatePorts();
}
-int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction)
+int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction, mspVersion_e mspVersion)
{
int ret = 0;
@@ -577,7 +597,7 @@ int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int d
.direction = direction,
};
- ret = mspSerialEncode(mspPort, &push, MSP_V1);
+ ret = mspSerialEncode(mspPort, &push, mspVersion);
}
return ret; // return the number of bytes written
}
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index 22802ddff2..466b25befd 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -71,11 +71,7 @@ typedef enum {
#define MSP_PORT_OUTBUF_SIZE_MIN 320
#ifdef USE_FLASHFS
-#ifdef STM32F1
-#define MSP_PORT_DATAFLASH_BUFFER_SIZE 1024
-#else
#define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096
-#endif
#define MSP_PORT_DATAFLASH_INFO_SIZE 16
#define MSP_PORT_OUTBUF_SIZE (MSP_PORT_DATAFLASH_BUFFER_SIZE + MSP_PORT_DATAFLASH_INFO_SIZE)
#else
@@ -124,5 +120,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
void mspSerialAllocatePorts(void);
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
void mspSerialReleaseSharedTelemetryPorts(void);
-int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction);
+mspDescriptor_t getMspSerialPortDescriptor(const uint8_t portIdentifier);
+int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction, mspVersion_e mspVersion);
uint32_t mspSerialTxBytesFree(void);
diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c
index a1086dc361..57c3ceeb2e 100644
--- a/src/main/osd/osd.c
+++ b/src/main/osd/osd.c
@@ -94,7 +94,6 @@
#include "sensors/acceleration.h"
#include "sensors/battery.h"
-#include "sensors/esc_sensor.h"
#include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
@@ -114,6 +113,9 @@ const char * const osdTimerSourceNames[] = {
"ON/ARM "
};
+#define OSD_LOGO_ROWS 4
+#define OSD_LOGO_COLS 24
+
// Things in both OSD and CMS
#define IS_HI(X) (rcData[X] > 1750)
@@ -124,6 +126,7 @@ timeUs_t osdFlyTime = 0;
#if defined(USE_ACC)
float osdGForce = 0;
#endif
+uint16_t osdAuxValue = 0;
static bool showVisualBeeper = false;
@@ -147,11 +150,11 @@ static bool backgroundLayerSupported = false;
escSensorData_t *osdEscDataCombined;
#endif
-STATIC_ASSERT(OSD_POS_MAX == OSD_POS(31,31), OSD_POS_MAX_incorrect);
+STATIC_ASSERT(OSD_POS_MAX == OSD_POS(63,31), OSD_POS_MAX_incorrect);
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
-PG_REGISTER_WITH_RESET_FN(osdElementConfig_t, osdElementConfig, PG_OSD_ELEMENT_CONFIG, 0);
+PG_REGISTER_WITH_RESET_FN(osdElementConfig_t, osdElementConfig, PG_OSD_ELEMENT_CONFIG, 1);
// Controls the display order of the OSD post-flight statistics.
// Adjust the ordering here to control how the post-flight stats are presented.
@@ -185,9 +188,11 @@ const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
OSD_STAT_MIN_LINK_QUALITY,
OSD_STAT_MAX_FFT,
OSD_STAT_MIN_RSSI_DBM,
+ OSD_STAT_MIN_RSNR,
OSD_STAT_TOTAL_FLIGHTS,
OSD_STAT_TOTAL_TIME,
OSD_STAT_TOTAL_DIST,
+ OSD_STAT_WATT_HOURS_DRAWN,
};
// Group elements in a number of groups to reduce task scheduling overhead
@@ -219,7 +224,7 @@ int osdPrintFloat(char *buffer, char leadingSymbol, float value, char *formatStr
}
value *= multiplier;
- const int scaledValueAbs = ABS(round ? lrintf(value) : value);
+ const int scaledValueAbs = abs(round ? (int)lrintf(value) : (int)value);
const int integerPart = scaledValueAbs / multiplier;
const int fractionalPart = scaledValueAbs % multiplier;
@@ -343,6 +348,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdWarnSetState(OSD_WARNING_RSSI, false);
osdWarnSetState(OSD_WARNING_LINK_QUALITY, false);
osdWarnSetState(OSD_WARNING_RSSI_DBM, false);
+ osdWarnSetState(OSD_WARNING_RSNR, false);
// turn off the over mah capacity warning
osdWarnSetState(OSD_WARNING_OVER_CAP, false);
@@ -369,6 +375,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->profile[i][0] = '\0';
}
osdConfig->rssi_dbm_alarm = -60;
+ osdConfig->rsnr_alarm = 4;
osdConfig->gps_sats_show_hdop = false;
for (int i = 0; i < OSD_RCCHANNELS_COUNT; i++) {
@@ -387,13 +394,37 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->stat_show_cell_value = false;
osdConfig->framerate_hz = OSD_FRAMERATE_DEFAULT_HZ;
osdConfig->cms_background_type = DISPLAY_BACKGROUND_TRANSPARENT;
+ #ifdef USE_CRAFTNAME_MSGS
+ osdConfig->osd_craftname_msgs = false; // Insert LQ/RSSI-dBm and warnings into CraftName
+ #endif //USE_CRAFTNAME_MSGS
+
+ osdConfig->aux_channel = 1;
+ osdConfig->aux_scale = 200;
+ osdConfig->aux_symbol = 'A';
+
+ // Make it obvious on the configurator that the FC doesn't support HD
+#ifdef USE_OSD_HD
+ osdConfig->canvas_cols = OSD_HD_COLS;
+ osdConfig->canvas_rows = OSD_HD_ROWS;
+#else
+ osdConfig->canvas_cols = OSD_SD_COLS;
+ osdConfig->canvas_rows = OSD_SD_ROWS;
+#endif
}
void pgResetFn_osdElementConfig(osdElementConfig_t *osdElementConfig)
{
+#ifdef USE_OSD_SD
+ uint8_t midRow = 7;
+ uint8_t midCol = 15;
+#else
+ uint8_t midRow = 10;
+ uint8_t midCol = 26;
+#endif
+
// Position elements near centre of screen and disabled by default
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
- osdElementConfig->item_pos[i] = OSD_POS(10, 7);
+ osdElementConfig->item_pos[i] = OSD_POS((midCol - 5), midRow);
}
// Always enable warnings elements by default
@@ -401,30 +432,33 @@ void pgResetFn_osdElementConfig(osdElementConfig_t *osdElementConfig)
for (unsigned i = 1; i <= OSD_PROFILE_COUNT; i++) {
profileFlags |= OSD_PROFILE_FLAG(i);
}
- osdElementConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | profileFlags;
+ osdElementConfig->item_pos[OSD_WARNINGS] = OSD_POS((midCol - 6), (midRow + 3)) | profileFlags;
// Default to old fixed positions for these elements
- osdElementConfig->item_pos[OSD_CROSSHAIRS] = OSD_POS(13, 6);
- osdElementConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2);
- osdElementConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(14, 6);
- osdElementConfig->item_pos[OSD_CAMERA_FRAME] = OSD_POS(3, 1);
- osdElementConfig->item_pos[OSD_UP_DOWN_REFERENCE] = OSD_POS(13, 6);
+ osdElementConfig->item_pos[OSD_CROSSHAIRS] = OSD_POS((midCol - 2), (midRow - 1));
+ osdElementConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS((midCol - 1), (midRow - 5));
+ osdElementConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS((midCol - 1), (midRow - 1));
+ osdElementConfig->item_pos[OSD_CAMERA_FRAME] = OSD_POS((midCol - 12), (midRow - 6));
+ osdElementConfig->item_pos[OSD_UP_DOWN_REFERENCE] = OSD_POS((midCol - 2), (midRow - 1));
}
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++) {
+ for (int row = 0; row < OSD_LOGO_ROWS; row++) {
+ for (int column = 0; column < OSD_LOGO_COLS; column++) {
if (fontOffset <= SYM_END_OF_FONT)
- displayWriteChar(osdDisplayPort, x + column, y + row, DISPLAYPORT_ATTR_NONE, fontOffset++);
+ displayWriteChar(osdDisplayPort, x + column, y + row, DISPLAYPORT_SEVERITY_NORMAL, fontOffset++);
}
}
}
static void osdCompleteInitialization(void)
{
+ uint8_t midRow = osdDisplayPort->rows / 2;
+ uint8_t midCol = osdDisplayPort->cols / 2;
+
armState = ARMING_FLAG(ARMED);
osdResetAlarms();
@@ -435,23 +469,21 @@ static void osdCompleteInitialization(void)
displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_WAIT);
- osdDrawLogo(3, 1);
+ osdDrawLogo(midCol - (OSD_LOGO_COLS) / 2, midRow - 5);
char string_buffer[30];
tfp_sprintf(string_buffer, "V%s", FC_VERSION_STRING);
- displayWrite(osdDisplayPort, 20, 6, DISPLAYPORT_ATTR_NONE, string_buffer);
- tfp_sprintf(string_buffer, "AT32 BY %s", "EMSR");
- displayWrite(osdDisplayPort, 10, 7, DISPLAYPORT_ATTR_NONE, string_buffer);
+ displayWrite(osdDisplayPort, midCol + 5, midRow, DISPLAYPORT_SEVERITY_NORMAL, string_buffer);
#ifdef USE_CMS
- displayWrite(osdDisplayPort, 7, 8, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT1);
- displayWrite(osdDisplayPort, 11, 9, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT2);
- displayWrite(osdDisplayPort, 11, 10, DISPLAYPORT_ATTR_NONE, CMS_STARTUP_HELP_TEXT3);
+ displayWrite(osdDisplayPort, midCol - 8, midRow + 2, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT1);
+ displayWrite(osdDisplayPort, midCol - 4, midRow + 3, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT2);
+ displayWrite(osdDisplayPort, midCol - 4, midRow + 4, DISPLAYPORT_SEVERITY_NORMAL, CMS_STARTUP_HELP_TEXT3);
#endif
#ifdef USE_RTC_TIME
char dateTimeBuffer[FORMATTED_DATE_TIME_BUFSIZE];
if (osdFormatRtcDateTime(&dateTimeBuffer[0])) {
- displayWrite(osdDisplayPort, 5, 12, DISPLAYPORT_ATTR_NONE, dateTimeBuffer);
+ displayWrite(osdDisplayPort, midCol - 10, midRow + 6, DISPLAYPORT_SEVERITY_NORMAL, dateTimeBuffer);
}
#endif
@@ -478,41 +510,71 @@ void osdInit(displayPort_t *osdDisplayPortToUse, osdDisplayPortDevice_e displayP
#ifdef USE_CMS
cmsDisplayPortRegister(osdDisplayPort);
#endif
+
+ if (osdDisplayPort->cols && osdDisplayPort->rows) {
+ // Ensure that osd_canvas_width/height are correct
+ if (osdConfig()->canvas_cols != osdDisplayPort->cols) {
+ osdConfigMutable()->canvas_cols = osdDisplayPort->cols;
+ }
+ if (osdConfig()->canvas_rows != osdDisplayPort->rows) {
+ osdConfigMutable()->canvas_rows = osdDisplayPort->rows;
+ }
+
+ // Ensure that all OSD elements are on the canvas once number of row/columns is known
+ for (int i = 0; i < OSD_ITEM_COUNT; i++) {
+ uint16_t itemPos = osdElementConfig()->item_pos[i];
+ uint8_t elemPosX = OSD_X(itemPos);
+ uint8_t elemPosY = OSD_Y(itemPos);
+ uint16_t elemProfileType = itemPos & (OSD_PROFILE_MASK | OSD_TYPE_MASK);
+ bool pos_reset = false;
+
+ if (elemPosX >= osdDisplayPort->cols) {
+ elemPosX = osdDisplayPort->cols - 1;
+ pos_reset = true;
+ }
+
+ if (elemPosY >= osdDisplayPort->rows) {
+ elemPosY = osdDisplayPort->rows - 1;
+ pos_reset = true;
+ }
+
+ if (pos_reset) {
+ osdElementConfigMutable()->item_pos[i] = elemProfileType | OSD_POS(elemPosX, elemPosY);
+ }
+ }
+ }
}
static void osdResetStats(void)
{
- stats.max_current = 0;
- stats.max_speed = 0;
- stats.min_voltage = 5000;
- stats.end_voltage = 0;
- 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.max_current = 0;
+ stats.max_speed = 0;
+ stats.min_voltage = 5000;
+ stats.end_voltage = 0;
+ 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_ix = 0;
+ stats.max_esc_temp = 0;
+ stats.max_esc_rpm = 0;
stats.min_link_quality = (linkQualitySource == LQ_SOURCE_NONE) ? 99 : 100; // percent
- stats.min_rssi_dbm = CRSF_SNR_MAX;
+ stats.min_rssi_dbm = CRSF_RSSI_MAX;
+ stats.min_rsnr = CRSF_SNR_MAX;
}
#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
static int32_t getAverageEscRpm(void)
{
-#ifdef USE_DSHOT_TELEMETRY
- if (motorConfig()->dev.useDshotTelemetry) {
- uint32_t rpm = 0;
- for (int i = 0; i < getMotorCount(); i++) {
- rpm += getDshotTelemetry(i);
- }
- rpm = rpm / getMotorCount();
- return rpm * 100 * 2 / motorConfig()->motorPoleCount;
- }
-#endif
#ifdef USE_ESC_SENSOR
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- return calcEscRpm(osdEscDataCombined->rpm);
+ return erpmToRpm(osdEscDataCombined->rpm);
+ }
+#endif
+#ifdef USE_DSHOT_TELEMETRY
+ if (motorConfig()->dev.useDshotTelemetry) {
+ return getDshotAverageRpm();
}
#endif
return 0;
@@ -579,6 +641,13 @@ static void osdUpdateStats(void)
}
#endif
+#ifdef USE_RX_RSNR
+ value = getRsnr();
+ if (stats.min_rsnr > value) {
+ stats.min_rsnr = value;
+ }
+#endif
+
#ifdef USE_GPS
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
if (stats.max_distance < GPS_distanceToHome) {
@@ -587,13 +656,26 @@ static void osdUpdateStats(void)
}
#endif
-#ifdef USE_ESC_SENSOR
+#if defined(USE_ESC_SENSOR)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
value = osdEscDataCombined->temperature;
if (stats.max_esc_temp < value) {
stats.max_esc_temp = value;
}
+ } else
+#endif
+#if defined(USE_DSHOT_TELEMETRY)
+ {
+ // Take max temp from dshot telemetry
+ for (uint8_t k = 0; k < getMotorCount(); k++) {
+ if (dshotTelemetryState.motorState[k].maxTemp > stats.max_esc_temp) {
+ stats.max_esc_temp_ix = k + 1;
+ stats.max_esc_temp = dshotTelemetryState.motorState[k].maxTemp;
+ }
+ }
}
+#else
+ {}
#endif
#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
@@ -648,11 +730,11 @@ static void osdGetBlackboxStatusString(char * buff)
}
#endif
-static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * value)
+static void osdDisplayStatisticLabel(uint8_t x, uint8_t y, const char * text, const char * value)
{
- displayWrite(osdDisplayPort, 2, y, DISPLAYPORT_ATTR_NONE, text);
- displayWrite(osdDisplayPort, 20, y, DISPLAYPORT_ATTR_NONE, ":");
- displayWrite(osdDisplayPort, 22, y, DISPLAYPORT_ATTR_NONE, value);
+ displayWrite(osdDisplayPort, x - 13, y, DISPLAYPORT_SEVERITY_NORMAL, text);
+ displayWrite(osdDisplayPort, x + 5, y, DISPLAYPORT_SEVERITY_NORMAL, ":");
+ displayWrite(osdDisplayPort, x + 7, y, DISPLAYPORT_SEVERITY_NORMAL, value);
}
/*
@@ -672,6 +754,7 @@ static bool isSomeStatEnabled(void)
static bool osdDisplayStat(int statistic, uint8_t displayRow)
{
+ uint8_t midCol = osdDisplayPort->cols / 2;
char buff[OSD_ELEMENT_BUFFER_LENGTH];
switch (statistic) {
@@ -684,23 +767,23 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
tfp_sprintf(buff, "NO RTC");
}
- displayWrite(osdDisplayPort, 2, displayRow, DISPLAYPORT_ATTR_NONE, buff);
+ displayWrite(osdDisplayPort, midCol - 13, displayRow, DISPLAYPORT_SEVERITY_NORMAL, buff);
return true;
}
case 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(displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
+ osdDisplayStatisticLabel(midCol, displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_1])], buff);
return true;
case 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(displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
+ osdDisplayStatisticLabel(midCol, displayRow, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff);
return true;
case OSD_STAT_MAX_ALTITUDE: {
osdPrintFloat(buff, SYM_NONE, osdGetMetersToSelectedUnit(stats.max_altitude) / 100.0f, "", 1, true, osdGetMetersToSelectedUnitSymbol());
- osdDisplayStatisticLabel(displayRow, "MAX ALTITUDE", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX ALTITUDE", buff);
return true;
}
@@ -708,7 +791,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_MAX_SPEED:
if (featureIsEnabled(FEATURE_GPS)) {
tfp_sprintf(buff, "%d%c", osdGetSpeedToSelectedUnit(stats.max_speed), osdGetSpeedToSelectedUnitSymbol());
- osdDisplayStatisticLabel(displayRow, "MAX SPEED", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX SPEED", buff);
return true;
}
break;
@@ -716,7 +799,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_MAX_DISTANCE:
if (featureIsEnabled(FEATURE_GPS)) {
osdFormatDistanceString(buff, stats.max_distance, SYM_NONE);
- osdDisplayStatisticLabel(displayRow, "MAX DISTANCE", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX DISTANCE", buff);
return true;
}
break;
@@ -725,7 +808,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
if (featureIsEnabled(FEATURE_GPS)) {
const int distanceFlown = GPS_distanceFlownInCm / 100;
osdFormatDistanceString(buff, distanceFlown, SYM_NONE);
- osdDisplayStatisticLabel(displayRow, "FLIGHT DISTANCE", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "FLIGHT DISTANCE", buff);
return true;
}
break;
@@ -733,19 +816,19 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_MIN_BATTERY:
osdPrintFloat(buff, SYM_NONE, stats.min_voltage / 100.0f, "", 2, true, SYM_VOLT);
- osdDisplayStatisticLabel(displayRow, osdConfig()->stat_show_cell_value? "MIN AVG CELL" : "MIN BATTERY", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value? "MIN AVG CELL" : "MIN BATTERY", buff);
return true;
case OSD_STAT_END_BATTERY:
osdPrintFloat(buff, SYM_NONE, stats.end_voltage / 100.0f, "", 2, true, SYM_VOLT);
- osdDisplayStatisticLabel(displayRow, osdConfig()->stat_show_cell_value ? "END AVG CELL" : "END BATTERY", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value ? "END AVG CELL" : "END BATTERY", buff);
return true;
case OSD_STAT_BATTERY:
{
const uint16_t statsVoltage = getStatsVoltage();
osdPrintFloat(buff, SYM_NONE, statsVoltage / 100.0f, "", 2, true, SYM_VOLT);
- osdDisplayStatisticLabel(displayRow, osdConfig()->stat_show_cell_value ? "AVG BATT CELL" : "BATTERY", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, osdConfig()->stat_show_cell_value ? "AVG BATT CELL" : "BATTERY", buff);
return true;
}
break;
@@ -753,13 +836,13 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_MIN_RSSI:
itoa(stats.min_rssi, buff, 10);
strcat(buff, "%");
- osdDisplayStatisticLabel(displayRow, "MIN RSSI", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MIN RSSI", buff);
return true;
case OSD_STAT_MAX_CURRENT:
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
tfp_sprintf(buff, "%d%c", stats.max_current, SYM_AMP);
- osdDisplayStatisticLabel(displayRow, "MAX CURRENT", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX CURRENT", buff);
return true;
}
break;
@@ -767,7 +850,15 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_USED_MAH:
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
- osdDisplayStatisticLabel(displayRow, "USED MAH", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "USED MAH", buff);
+ return true;
+ }
+ break;
+
+ case OSD_STAT_WATT_HOURS_DRAWN:
+ if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
+ osdPrintFloat(buff, SYM_NONE, getWhDrawn(), "", 2, true, SYM_NONE);
+ osdDisplayStatisticLabel(midCol, displayRow, "USED WATT HOURS", buff);
return true;
}
break;
@@ -776,7 +867,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_BLACKBOX:
if (blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
osdGetBlackboxStatusString(buff);
- osdDisplayStatisticLabel(displayRow, "BLACKBOX", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "BLACKBOX", buff);
return true;
}
break;
@@ -786,7 +877,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
int32_t logNumber = blackboxGetLogNumber();
if (logNumber >= 0) {
itoa(logNumber, buff, 10);
- osdDisplayStatisticLabel(displayRow, "BB LOG NUM", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "BB LOG NUM", buff);
return true;
}
}
@@ -797,7 +888,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_MAX_G_FORCE:
if (sensors(SENSOR_ACC)) {
osdPrintFloat(buff, SYM_NONE, stats.max_g_force, "", 1, true, 'G');
- osdDisplayStatisticLabel(displayRow, "MAX G-FORCE", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX G-FORCE", buff);
return true;
}
break;
@@ -805,15 +896,21 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
#ifdef USE_ESC_SENSOR
case OSD_STAT_MAX_ESC_TEMP:
- tfp_sprintf(buff, "%d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
- osdDisplayStatisticLabel(displayRow, "MAX ESC TEMP", buff);
+ {
+ uint16_t ix = 0;
+ if (stats.max_esc_temp_ix > 0) {
+ ix = tfp_sprintf(buff, "%d ", stats.max_esc_temp_ix);
+ }
+ tfp_sprintf(buff + ix, "%d%c", osdConvertTemperatureToSelectedUnit(stats.max_esc_temp), osdGetTemperatureSymbolForSelectedUnit());
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX ESC TEMP", buff);
return true;
+ }
#endif
#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
case OSD_STAT_MAX_ESC_RPM:
itoa(stats.max_esc_rpm, buff, 10);
- osdDisplayStatisticLabel(displayRow, "MAX ESC RPM", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MAX ESC RPM", buff);
return true;
#endif
@@ -821,7 +918,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
case OSD_STAT_MIN_LINK_QUALITY:
tfp_sprintf(buff, "%d", stats.min_link_quality);
strcat(buff, "%");
- osdDisplayStatisticLabel(displayRow, "MIN LINK", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MIN LINK", buff);
return true;
#endif
@@ -831,9 +928,9 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
int value = getMaxFFT();
if (value > 0) {
tfp_sprintf(buff, "%dHZ", value);
- osdDisplayStatisticLabel(displayRow, "PEAK FFT", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "PEAK FFT", buff);
} else {
- osdDisplayStatisticLabel(displayRow, "PEAK FFT", "THRT<20%");
+ osdDisplayStatisticLabel(midCol, displayRow, "PEAK FFT", "THRT<20%");
}
return true;
}
@@ -843,20 +940,27 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
#ifdef USE_RX_RSSI_DBM
case OSD_STAT_MIN_RSSI_DBM:
tfp_sprintf(buff, "%3d", stats.min_rssi_dbm);
- osdDisplayStatisticLabel(displayRow, "MIN RSSI DBM", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "MIN RSSI DBM", buff);
+ return true;
+#endif
+
+#ifdef USE_RX_RSNR
+ case OSD_STAT_MIN_RSNR:
+ tfp_sprintf(buff, "%3d", stats.min_rsnr);
+ osdDisplayStatisticLabel(midCol, displayRow, "MIN RSNR", buff);
return true;
#endif
#ifdef USE_PERSISTENT_STATS
case OSD_STAT_TOTAL_FLIGHTS:
itoa(statsConfig()->stats_total_flights, buff, 10);
- osdDisplayStatisticLabel(displayRow, "TOTAL FLIGHTS", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "TOTAL FLIGHTS", buff);
return true;
case OSD_STAT_TOTAL_TIME: {
int minutes = statsConfig()->stats_total_time_s / 60;
tfp_sprintf(buff, "%d:%02dH", minutes / 60, minutes % 60);
- osdDisplayStatisticLabel(displayRow, "TOTAL FLIGHT TIME", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "TOTAL FLIGHT TIME", buff);
return true;
}
@@ -868,7 +972,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow)
} else {
tfp_sprintf(buff, "%d%c", statsConfig()->stats_total_dist_m / METERS_PER_KILOMETER, SYM_KM);
}
- osdDisplayStatisticLabel(displayRow, "TOTAL DISTANCE", buff);
+ osdDisplayStatisticLabel(midCol, displayRow, "TOTAL DISTANCE", buff);
return true;
#endif
}
@@ -899,6 +1003,8 @@ static void osdRenderStatsBegin(void)
// call repeatedly until it returns true which indicates that all stats have been rendered.
static bool osdRenderStatsContinue(void)
{
+ uint8_t midCol = osdDisplayPort->cols / 2;
+
if (osdStatsRenderingState.row == 0) {
bool displayLabel = false;
@@ -915,7 +1021,7 @@ static bool osdRenderStatsContinue(void)
}
if (displayLabel) {
- displayWrite(osdDisplayPort, 2, osdStatsRenderingState.row++, DISPLAYPORT_ATTR_NONE, " --- STATS ---");
+ displayWrite(osdDisplayPort, midCol - (strlen("--- STATS ---") / 2), osdStatsRenderingState.row++, DISPLAYPORT_SEVERITY_NORMAL, "--- STATS ---");
return false;
}
}
@@ -1008,20 +1114,24 @@ static bool osdRefreshStats(void)
static timeDelta_t osdShowArmed(void)
{
+ uint8_t midRow = osdDisplayPort->rows / 2;
+ uint8_t midCol = osdDisplayPort->cols / 2;
timeDelta_t ret;
displayClearScreen(osdDisplayPort, DISPLAY_CLEAR_WAIT);
if ((osdConfig()->logo_on_arming == OSD_LOGO_ARMING_ON) || ((osdConfig()->logo_on_arming == OSD_LOGO_ARMING_FIRST) && !ARMING_FLAG(WAS_EVER_ARMED))) {
- osdDrawLogo(3, 1);
+ uint8_t midRow = osdDisplayPort->rows / 2;
+ uint8_t midCol = osdDisplayPort->cols / 2;
+ osdDrawLogo(midCol - (OSD_LOGO_COLS) / 2, midRow - 5);
ret = osdConfig()->logo_on_arming_duration * 1e5;
} else {
ret = (REFRESH_1S / 2);
}
- displayWrite(osdDisplayPort, 12, 7, DISPLAYPORT_ATTR_NONE, "ARMED");
+ displayWrite(osdDisplayPort, midCol - (strlen("ARMED") / 2), midRow, DISPLAYPORT_SEVERITY_NORMAL, "ARMED");
if (isFlipOverAfterCrashActive()) {
- displayWrite(osdDisplayPort, 8, 8, DISPLAYPORT_ATTR_NONE, CRASH_FLIP_WARNING);
+ displayWrite(osdDisplayPort, midCol - (strlen(CRASH_FLIP_WARNING) / 2), midRow + 1, DISPLAYPORT_SEVERITY_NORMAL, CRASH_FLIP_WARNING);
}
return ret;
@@ -1034,6 +1144,7 @@ STATIC_UNIT_TESTED bool osdProcessStats1(timeUs_t currentTimeUs)
{
static timeUs_t lastTimeUs = 0;
static timeUs_t osdStatsRefreshTimeUs;
+ static timeUs_t osdAuxRefreshTimeUs = 0;
bool refreshStatsRequired = false;
@@ -1084,6 +1195,16 @@ STATIC_UNIT_TESTED bool osdProcessStats1(timeUs_t currentTimeUs)
}
}
}
+
+ if (VISIBLE(osdElementConfig()->item_pos[OSD_AUX_VALUE])) {
+ const uint8_t auxChannel = osdConfig()->aux_channel + NON_AUX_CHANNEL_COUNT - 1;
+ if (currentTimeUs > osdAuxRefreshTimeUs) {
+ // aux channel start after main channels
+ osdAuxValue = (constrain(rcData[auxChannel], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * osdConfig()->aux_scale / PWM_RANGE;
+ osdAuxRefreshTimeUs = currentTimeUs + REFRESH_1S;
+ }
+ }
+
lastTimeUs = currentTimeUs;
return refreshStatsRequired;
@@ -1117,14 +1238,14 @@ void osdProcessStats2(timeUs_t currentTimeUs)
#endif
}
-void osdProcessStats3()
+void osdProcessStats3(void)
{
#if defined(USE_ACC)
if (sensors(SENSOR_ACC)
&& (VISIBLE(osdElementConfig()->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];
+ const float a = acc.accADC[axis];
osdGForce += a * a;
}
osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h
index d51d3535f0..c7c53f9ced 100644
--- a/src/main/osd/osd.h
+++ b/src/main/osd/osd.h
@@ -55,7 +55,7 @@ extern const char * const osdTimerSourceNames[OSD_NUM_TIMER_TYPES];
#define OSD_PROFILE_BITS_POS 11
#define OSD_PROFILE_MASK (((1 << OSD_PROFILE_COUNT) - 1) << OSD_PROFILE_BITS_POS)
-#define OSD_POS_MAX 0x3FF
+#define OSD_POS_MAX 0x7FF
#define OSD_POSCFG_MAX UINT16_MAX // element positions now use all 16 bits
#define OSD_PROFILE_FLAG(x) (1 << ((x) - 1 + OSD_PROFILE_BITS_POS))
#define OSD_PROFILE_1_FLAG OSD_PROFILE_FLAG(1)
@@ -71,14 +71,24 @@ extern const char * const osdTimerSourceNames[OSD_NUM_TIMER_TYPES];
// Character coordinate
-#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
-#define OSD_POSITION_XY_MASK ((1 << OSD_POSITION_BITS) - 1)
-#define OSD_TYPE_MASK 0xC000 // bits 14-15
-#define OSD_POS(x,y) ((x & OSD_POSITION_XY_MASK) | ((y & OSD_POSITION_XY_MASK) << OSD_POSITION_BITS))
-#define OSD_X(x) (x & OSD_POSITION_XY_MASK)
+#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
+#define OSD_POSITION_BIT_XHD 10 // extra bit used to extend X range in a backward compatible manner for HD displays
+#define OSD_POSITION_XHD_MASK (1 << OSD_POSITION_BIT_XHD)
+#define OSD_POSITION_XY_MASK ((1 << OSD_POSITION_BITS) - 1)
+#define OSD_TYPE_MASK 0xC000 // bits 14-15
+#define OSD_POS(x,y) ((x & OSD_POSITION_XY_MASK) | ((x << (OSD_POSITION_BIT_XHD - OSD_POSITION_BITS)) & OSD_POSITION_XHD_MASK) | \
+ ((y & OSD_POSITION_XY_MASK) << OSD_POSITION_BITS))
+#define OSD_X(x) ((x & OSD_POSITION_XY_MASK) | ((x & OSD_POSITION_XHD_MASK) >> (OSD_POSITION_BIT_XHD - OSD_POSITION_BITS)))
#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & OSD_POSITION_XY_MASK)
#define OSD_TYPE(x) ((x & OSD_TYPE_MASK) >> 14)
+#define OSD_SD_COLS VIDEO_COLUMNS_SD
+#define OSD_SD_ROWS VIDEO_LINES_PAL
+
+// Default HD OSD canvas size to be applied unless the goggles announce otherwise
+#define OSD_HD_COLS 53
+#define OSD_HD_ROWS 20
+
// Timer configuration
// Stored as 15[alarm:8][precision:4][source:4]0
#define OSD_TIMER(src, prec, alarm) ((src & 0x0F) | ((prec & 0x0F) << 4) | ((alarm & 0xFF ) << 8))
@@ -149,7 +159,7 @@ typedef enum {
OSD_FLIGHT_DIST,
OSD_STICK_OVERLAY_LEFT,
OSD_STICK_OVERLAY_RIGHT,
- OSD_DISPLAY_NAME,
+ OSD_PILOT_NAME,
OSD_ESC_RPM_FREQ,
OSD_RATE_PROFILE_NAME,
OSD_PID_PROFILE_NAME,
@@ -161,6 +171,21 @@ typedef enum {
OSD_TOTAL_FLIGHTS,
OSD_UP_DOWN_REFERENCE,
OSD_TX_UPLINK_POWER,
+ OSD_WATT_HOURS_DRAWN,
+ OSD_AUX_VALUE,
+ OSD_READY_MODE,
+ OSD_RSNR_VALUE,
+ OSD_SYS_GOGGLE_VOLTAGE,
+ OSD_SYS_VTX_VOLTAGE,
+ OSD_SYS_BITRATE,
+ OSD_SYS_DELAY,
+ OSD_SYS_DISTANCE,
+ OSD_SYS_LQ,
+ OSD_SYS_GOGGLE_DVR,
+ OSD_SYS_VTX_DVR,
+ OSD_SYS_WARNINGS,
+ OSD_SYS_VTX_TEMP,
+ OSD_SYS_FAN_SPEED,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@@ -200,6 +225,8 @@ typedef enum {
OSD_STAT_TOTAL_TIME,
OSD_STAT_TOTAL_DIST,
OSD_STAT_MIN_RSSI_DBM,
+ OSD_STAT_WATT_HOURS_DRAWN,
+ OSD_STAT_MIN_RSNR,
OSD_STAT_COUNT // MUST BE LAST
} osd_stats_e;
@@ -245,6 +272,7 @@ typedef enum {
OSD_WARNING_LINK_QUALITY,
OSD_WARNING_RSSI_DBM,
OSD_WARNING_OVER_CAP,
+ OSD_WARNING_RSNR,
OSD_WARNING_COUNT // MUST BE LAST
} osdWarningsFlags_e;
@@ -259,9 +287,9 @@ typedef enum {
// Make sure the number of warnings do not exceed the available 32bit storage
STATIC_ASSERT(OSD_WARNING_COUNT <= 32, osdwarnings_overflow);
-#define ESC_RPM_ALARM_OFF -1
-#define ESC_TEMP_ALARM_OFF INT8_MIN
-#define ESC_CURRENT_ALARM_OFF -1
+#define ESC_RPM_ALARM_OFF -1
+#define ESC_TEMP_ALARM_OFF 0
+#define ESC_CURRENT_ALARM_OFF -1
#define OSD_GPS_RESCUE_DISABLED_WARNING_DURATION_US 3000000 // 3 seconds
@@ -282,7 +310,7 @@ typedef struct osdConfig_s {
uint8_t ahMaxPitch;
uint8_t ahMaxRoll;
uint32_t enabled_stats;
- int8_t esc_temp_alarm;
+ uint8_t esc_temp_alarm;
int16_t esc_rpm_alarm;
int16_t esc_current_alarm;
uint8_t core_temp_alarm;
@@ -292,6 +320,7 @@ typedef struct osdConfig_s {
char profile[OSD_PROFILE_COUNT][OSD_PROFILE_NAME_LENGTH + 1];
uint16_t link_quality_alarm;
int16_t rssi_dbm_alarm;
+ int16_t rsnr_alarm;
uint8_t gps_sats_show_hdop;
int8_t rcChannels[OSD_RCCHANNELS_COUNT]; // RC channel values to display, -1 if none
uint8_t displayPortDevice; // osdDisplayPortDevice_e
@@ -303,6 +332,14 @@ typedef struct osdConfig_s {
uint16_t framerate_hz;
uint8_t cms_background_type; // For supporting devices, determines whether the CMS background is transparent or opaque
uint8_t stat_show_cell_value;
+ #ifdef USE_CRAFTNAME_MSGS
+ uint8_t osd_craftname_msgs; // Insert LQ/RSSI-dBm and warnings into CraftName
+ #endif //USE_CRAFTNAME_MSGS
+ uint8_t aux_channel;
+ uint16_t aux_scale;
+ uint8_t aux_symbol;
+ uint8_t canvas_cols; // Canvas dimensions for HD display
+ uint8_t canvas_rows;
} osdConfig_t;
PG_DECLARE(osdConfig_t, osdConfig);
@@ -323,10 +360,12 @@ typedef struct statistic_s {
int32_t max_altitude;
int16_t max_distance;
float max_g_force;
+ int16_t max_esc_temp_ix;
int16_t max_esc_temp;
int32_t max_esc_rpm;
uint16_t min_link_quality;
int16_t min_rssi_dbm;
+ int16_t min_rsnr;
} statistic_t;
extern timeUs_t resumeRefreshAt;
@@ -337,6 +376,7 @@ extern float osdGForce;
#ifdef USE_ESC_SENSOR
extern escSensorData_t *osdEscDataCombined;
#endif
+extern uint16_t osdAuxValue;
void osdInit(displayPort_t *osdDisplayPort, osdDisplayPortDevice_e displayPortDevice);
bool osdUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 6321342a39..5105d8bd55 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -96,6 +96,10 @@
type 2: Graphical bar showing battery used (grows as used)
type 3: Numeric % of remaining battery
type 4: Numeric % or used battery
+
+ VTX_CHANNEL
+ type 1: Contains Band:Channel:Power:Pit
+ type 2: Contains only Power
*/
#include
@@ -159,7 +163,6 @@
#include "sensors/adcinternal.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
-#include "sensors/esc_sensor.h"
#include "sensors/sensors.h"
#ifdef USE_GPS_PLUS_CODES
@@ -236,7 +239,7 @@ enum {UP, DOWN};
static int osdDisplayWrite(osdElementParms_t *element, uint8_t x, uint8_t y, uint8_t attr, const char *s)
{
if (IS_BLINK(element->item)) {
- attr |= DISPLAYPORT_ATTR_BLINK;
+ attr |= DISPLAYPORT_BLINK;
}
return displayWrite(element->osdDisplayPort, x, y, attr, s);
@@ -259,12 +262,12 @@ static int getEscRpm(int i)
{
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
- return 100.0f / (motorConfig()->motorPoleCount / 2.0f) * getDshotTelemetry(i);
+ return erpmToRpm(getDshotTelemetry(i));
}
#endif
#ifdef USE_ESC_SENSOR
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- return calcEscRpm(getEscSensorData(i)->rpm);
+ return erpmToRpm(getEscSensorData(i)->rpm);
}
#endif
return 0;
@@ -284,7 +287,7 @@ static void renderOsdEscRpmOrFreq(getEscRpmOrFreqFnPtr escFnPtr, osdElementParms
const int rpm = MIN((*escFnPtr)(i),99999);
const int len = tfp_sprintf(rpmStr, "%d", rpm);
rpmStr[len] = '\0';
- osdDisplayWrite(element, x, y + i, DISPLAYPORT_ATTR_NONE, rpmStr);
+ osdDisplayWrite(element, x, y + i, DISPLAYPORT_SEVERITY_NORMAL, rpmStr);
}
element->drawElement = false;
}
@@ -329,8 +332,8 @@ static void osdFormatCoordinate(char *buff, gpsCoordinateType_e coordinateType,
gpsValue = (coordinateType == GPS_LONGITUDE) ? gpsSol.llh.lon : gpsSol.llh.lat;
}
- const int degreesPart = ABS(gpsValue) / GPS_DEGREES_DIVIDER;
- int fractionalPart = ABS(gpsValue) % GPS_DEGREES_DIVIDER;
+ const int degreesPart = abs(gpsValue) / GPS_DEGREES_DIVIDER;
+ int fractionalPart = abs(gpsValue) % GPS_DEGREES_DIVIDER;
switch (variantType) {
#ifdef USE_GPS_PLUS_CODES
@@ -660,6 +663,12 @@ static void osdElementAltitude(osdElementParms_t *element)
#ifdef USE_GPS
haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX);
#endif // USE_GPS
+ int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitudeCm()) / 100;
+
+ if ((alt >= osdConfig()->alt_alarm) && ARMING_FLAG(ARMED)) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
if (haveBaro || haveGps) {
osdFormatAltitudeString(element->buff, getEstimatedAltitudeCm(), element->type);
} else {
@@ -704,7 +713,7 @@ static void osdElementArtificialHorizon(osdElementParms_t *element)
for (int x = -4; x <= 4; x++) {
const int y = ((-rollAngle * x) / 64) - pitchAngle;
if (y >= 0 && y <= 81) {
- osdDisplayWriteChar(element, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), DISPLAYPORT_ATTR_NONE, (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
+ osdDisplayWriteChar(element, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), DISPLAYPORT_SEVERITY_NORMAL, (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
}
}
@@ -716,7 +725,7 @@ static void osdElementUpDownReference(osdElementParms_t *element)
// Up/Down reference feature displays reference points on the OSD at Zenith and Nadir
const float earthUpinBodyFrame[3] = {-rMat[2][0], -rMat[2][1], -rMat[2][2]}; //transforum the up vector to the body frame
- if (ABS(earthUpinBodyFrame[2]) < SINE_25_DEG && ABS(earthUpinBodyFrame[1]) < SINE_25_DEG) {
+ if (fabsf(earthUpinBodyFrame[2]) < SINE_25_DEG && fabsf(earthUpinBodyFrame[1]) < SINE_25_DEG) {
float thetaB; // pitch from body frame to zenith/nadir
float psiB; // psi from body frame to zenith/nadir
char *symbol[2] = {"U", "D"}; // character buffer
@@ -734,7 +743,7 @@ static void osdElementUpDownReference(osdElementParms_t *element)
int posX = element->elemPosX + lrintf(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14));
int posY = element->elemPosY + lrintf(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8));
- osdDisplayWrite(element, posX, posY, DISPLAYPORT_ATTR_NONE, symbol[direction]);
+ osdDisplayWrite(element, posX, posY, DISPLAYPORT_SEVERITY_NORMAL, symbol[direction]);
}
element->drawElement = false; // element already drawn
}
@@ -743,6 +752,19 @@ static void osdElementUpDownReference(osdElementParms_t *element)
static void osdElementAverageCellVoltage(osdElementParms_t *element)
{
const int cellV = getBatteryAverageCellVoltage();
+ const batteryState_e batteryState = getBatteryState();
+
+ switch (batteryState) {
+ case BATTERY_WARNING:
+ element->attr = DISPLAYPORT_SEVERITY_WARNING;
+ break;
+ case BATTERY_CRITICAL:
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ break;
+ default:
+ break;
+ }
+
osdPrintFloat(element->buff, osdGetBatterySymbol(cellV), cellV / 100.0f, "", 2, false, SYM_VOLT);
}
@@ -773,30 +795,31 @@ static void osdBackgroundCameraFrame(osdElementParms_t *element)
element->buff[width - 1] = SYM_STICK_OVERLAY_CENTER;
element->buff[width] = 0; // string terminator
- osdDisplayWrite(element, xpos, ypos, DISPLAYPORT_ATTR_NONE, element->buff);
+ osdDisplayWrite(element, xpos, ypos, DISPLAYPORT_SEVERITY_NORMAL, element->buff);
for (int i = 1; i < (height - 1); i++) {
- osdDisplayWriteChar(element, xpos, ypos + i, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_VERTICAL);
- osdDisplayWriteChar(element, xpos + width - 1, ypos + i, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_VERTICAL);
+ osdDisplayWriteChar(element, xpos, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
+ osdDisplayWriteChar(element, xpos + width - 1, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
}
- osdDisplayWrite(element, xpos, ypos + height - 1, DISPLAYPORT_ATTR_NONE, element->buff);
+ osdDisplayWrite(element, xpos, ypos + height - 1, DISPLAYPORT_SEVERITY_NORMAL, element->buff);
element->drawElement = false; // element already drawn
}
+static void toUpperCase(char* dest, const char* src, unsigned int maxSrcLength)
+{
+ unsigned int i;
+ for (i = 0; i < maxSrcLength && src[i]; i++) {
+ dest[i] = toupper((unsigned char)src[i]);
+ }
+ dest[i] = '\0';
+}
+
static void osdBackgroundCraftName(osdElementParms_t *element)
{
- if (strlen(pilotConfig()->name) == 0) {
+ if (strlen(pilotConfig()->craftName) == 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';
+ toUpperCase(element->buff, pilotConfig()->craftName, MAX_NAME_LENGTH);
}
}
@@ -870,20 +893,12 @@ static void osdElementDisarmed(osdElementParms_t *element)
}
}
-static void osdBackgroundDisplayName(osdElementParms_t *element)
+static void osdBackgroundPilotName(osdElementParms_t *element)
{
- if (strlen(pilotConfig()->displayName) == 0) {
- strcpy(element->buff, "DISPLAY_NAME");
+ if (strlen(pilotConfig()->pilotName) == 0) {
+ strcpy(element->buff, "PILOT_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';
+ toUpperCase(element->buff, pilotConfig()->pilotName, MAX_NAME_LENGTH);
}
}
@@ -901,15 +916,7 @@ static void osdElementRateProfileName(osdElementParms_t *element)
if (strlen(currentControlRateProfile->profileName) == 0) {
tfp_sprintf(element->buff, "RATE_%u", getCurrentControlRateProfileIndex() + 1);
} else {
- unsigned i;
- for (i = 0; i < MAX_PROFILE_NAME_LENGTH; i++) {
- if (currentControlRateProfile->profileName[i]) {
- element->buff[i] = toupper((unsigned char)currentControlRateProfile->profileName[i]);
- } else {
- break;
- }
- }
- element->buff[i] = '\0';
+ toUpperCase(element->buff, currentControlRateProfile->profileName, MAX_PROFILE_NAME_LENGTH);
}
}
@@ -918,15 +925,7 @@ static void osdElementPidProfileName(osdElementParms_t *element)
if (strlen(currentPidProfile->profileName) == 0) {
tfp_sprintf(element->buff, "PID_%u", getCurrentPidProfileIndex() + 1);
} else {
- unsigned i;
- for (i = 0; i < MAX_PROFILE_NAME_LENGTH; i++) {
- if (currentPidProfile->profileName[i]) {
- element->buff[i] = toupper((unsigned char)currentPidProfile->profileName[i]);
- } else {
- break;
- }
- }
- element->buff[i] = '\0';
+ toUpperCase(element->buff, currentPidProfile->profileName, MAX_PROFILE_NAME_LENGTH);
}
}
#endif
@@ -939,29 +938,39 @@ static void osdElementOsdProfileName(osdElementParms_t *element)
if (strlen(osdConfig()->profile[profileIndex - 1]) == 0) {
tfp_sprintf(element->buff, "OSD_%u", profileIndex);
} else {
- unsigned i;
- for (i = 0; i < OSD_PROFILE_NAME_LENGTH; i++) {
- if (osdConfig()->profile[profileIndex - 1][i]) {
- element->buff[i] = toupper((unsigned char)osdConfig()->profile[profileIndex - 1][i]);
- } else {
- break;
- }
- }
- element->buff[i] = '\0';
+ toUpperCase(element->buff, osdConfig()->profile[profileIndex - 1], OSD_PROFILE_NAME_LENGTH);
}
}
#endif
-#ifdef USE_ESC_SENSOR
+#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
+
static void osdElementEscTemperature(osdElementParms_t *element)
{
+#if defined(USE_ESC_SENSOR)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
tfp_sprintf(element->buff, "E%c%3d%c", SYM_TEMPERATURE, osdConvertTemperatureToSelectedUnit(osdEscDataCombined->temperature), osdGetTemperatureSymbolForSelectedUnit());
+ } else
+#endif
+#if defined(USE_DSHOT_TELEMETRY)
+ {
+ uint32_t osdEleIx = tfp_sprintf(element->buff, "E%c", SYM_TEMPERATURE);
+
+ for (uint8_t k = 0; k < getMotorCount(); k++) {
+ if ((dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0) {
+ osdEleIx += tfp_sprintf(element->buff + osdEleIx, "%3d%c",
+ osdConvertTemperatureToSelectedUnit(dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]),
+ osdGetTemperatureSymbolForSelectedUnit());
+ } else {
+ osdEleIx += tfp_sprintf(element->buff + osdEleIx, " 0%c", osdGetTemperatureSymbolForSelectedUnit());
+ }
+ }
}
+#else
+ {}
+#endif
}
-#endif // USE_ESC_SENSOR
-#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
static void osdElementEscRpm(osdElementParms_t *element)
{
renderOsdEscRpmOrFreq(&getEscRpm,element);
@@ -971,6 +980,7 @@ static void osdElementEscRpmFreq(osdElementParms_t *element)
{
renderOsdEscRpmOrFreq(&getEscRpmFreq,element);
}
+
#endif
static void osdElementFlymode(osdElementParms_t *element)
@@ -1001,6 +1011,13 @@ static void osdElementFlymode(osdElementParms_t *element)
}
}
+static void osdElementReadyMode(osdElementParms_t *element)
+{
+ if (IS_RC_MODE_ACTIVE(BOXREADY) && !ARMING_FLAG(ARMED)) {
+ strcpy(element->buff, "READY");
+ }
+}
+
#ifdef USE_ACC
static void osdElementGForce(osdElementParms_t *element)
{
@@ -1023,7 +1040,7 @@ 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);
+ const int h = DECIDEGREES_TO_DEGREES(GPS_directionToHome - attitude.values.yaw);
element->buff[0] = osdGetDirectionSymbolFromHeading(h);
} else {
element->buff[0] = SYM_OVER_HOME;
@@ -1106,13 +1123,13 @@ static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
for (int y = -hudheight; y <= hudheight; y++) {
- osdDisplayWriteChar(element, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NONE, SYM_AH_DECORATION);
- osdDisplayWriteChar(element, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_ATTR_NONE, SYM_AH_DECORATION);
+ osdDisplayWriteChar(element, element->elemPosX - hudwidth, element->elemPosY + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_DECORATION);
+ osdDisplayWriteChar(element, element->elemPosX + hudwidth, element->elemPosY + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_DECORATION);
}
// AH level indicators
- osdDisplayWriteChar(element, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_ATTR_NONE, SYM_AH_LEFT);
- osdDisplayWriteChar(element, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_ATTR_NONE, SYM_AH_RIGHT);
+ osdDisplayWriteChar(element, element->elemPosX - hudwidth + 1, element->elemPosY, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_LEFT);
+ osdDisplayWriteChar(element, element->elemPosX + hudwidth - 1, element->elemPosY, DISPLAYPORT_SEVERITY_NORMAL, SYM_AH_RIGHT);
element->drawElement = false; // element already drawn
}
@@ -1121,6 +1138,11 @@ static void osdBackgroundHorizonSidebars(osdElementParms_t *element)
static void osdElementLinkQuality(osdElementParms_t *element)
{
uint16_t osdLinkQuality = 0;
+
+ if (rxGetLinkQualityPercent() < osdConfig()->link_quality_alarm) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-99
osdLinkQuality = rxGetLinkQuality();
const uint8_t osdRfMode = rxGetRfMode();
@@ -1172,17 +1194,46 @@ static void osdElementLogStatus(osdElementParms_t *element)
static void osdElementMahDrawn(osdElementParms_t *element)
{
- tfp_sprintf(element->buff, "%4d%c", getMAhDrawn(), SYM_MAH);
+ const int mAhDrawn = getMAhDrawn();
+
+ if (mAhDrawn >= osdConfig()->cap_alarm) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
+ tfp_sprintf(element->buff, "%4d%c", mAhDrawn, SYM_MAH);
+}
+
+static void osdElementWattHoursDrawn(osdElementParms_t *element)
+{
+ const int mAhDrawn = getMAhDrawn();
+ const float wattHoursDrawn = getWhDrawn();
+
+ if (mAhDrawn >= osdConfig()->cap_alarm) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
+ if (wattHoursDrawn < 1.0f) {
+ tfp_sprintf(element->buff, "%3dMWH", lrintf(wattHoursDrawn * 1000));
+ } else {
+ int wattHourWholeNumber = (int)wattHoursDrawn;
+ int wattHourDecimalValue = (int)((wattHoursDrawn - wattHourWholeNumber) * 100);
+
+ tfp_sprintf(element->buff, wattHourDecimalValue >= 10 ? "%3d.%2dWH" : "%3d.0%1dWH", wattHourWholeNumber, wattHourDecimalValue);
+ }
}
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.
-
+ const int mAhDrawn = getMAhDrawn();
const int usedCapacity = getMAhDrawn();
int displayBasis = usedCapacity;
+ if (mAhDrawn >= osdConfig()->cap_alarm) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
switch (element->type) {
case OSD_ELEMENT_TYPE_3: // mAh remaining percentage (counts down as battery is used)
displayBasis = constrain(batteryConfig()->batteryCapacity - usedCapacity, 0, batteryConfig()->batteryCapacity);
@@ -1231,6 +1282,18 @@ static void osdElementMainBatteryVoltage(osdElementParms_t *element)
{
unsigned decimalPlaces;
const float batteryVoltage = getBatteryVoltage() / 100.0f;
+ batteryState_e batteryState = getBatteryState();
+
+ switch (batteryState) {
+ case BATTERY_WARNING:
+ element->attr = DISPLAYPORT_SEVERITY_WARNING;
+ break;
+ case BATTERY_CRITICAL:
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ break;
+ default:
+ break;
+ }
if (batteryVoltage >= 10) { // if voltage is 10v or more then display only 1 decimal place
decimalPlaces = 1;
@@ -1327,7 +1390,7 @@ static void osdElementRcChannels(osdElementParms_t *element)
// Decimal notation can be added when tfp_sprintf supports float among fancy options.
char fmtbuf[6];
tfp_sprintf(fmtbuf, "%5d", data);
- osdDisplayWrite(element, xpos, ypos + i, DISPLAYPORT_ATTR_NONE, fmtbuf);
+ osdDisplayWrite(element, xpos, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, fmtbuf);
}
}
@@ -1338,6 +1401,10 @@ static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
{
const int mAhDrawn = getMAhDrawn();
+ if (mAhDrawn >= osdConfig()->cap_alarm) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
if (mAhDrawn <= 0.1 * osdConfig()->cap_alarm) { // also handles the mAhDrawn == 0 condition
tfp_sprintf(element->buff, "--:--");
} else if (mAhDrawn > osdConfig()->cap_alarm) {
@@ -1355,6 +1422,10 @@ static void osdElementRssi(osdElementParms_t *element)
osdRssi = 99;
}
+ if (getRssiPercent() < osdConfig()->rssi_alarm) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+
tfp_sprintf(element->buff, "%c%2d", SYM_RSSI, osdRssi);
}
@@ -1368,10 +1439,25 @@ static void osdElementRtcTime(osdElementParms_t *element)
#ifdef USE_RX_RSSI_DBM
static void osdElementRssiDbm(osdElementParms_t *element)
{
- tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, getRssiDbm());
+ const int8_t antenna = getActiveAntenna();
+ static bool diversity = false;
+
+ if (antenna || diversity) {
+ diversity = true;
+ tfp_sprintf(element->buff, "%c%3d:%d", SYM_RSSI, getRssiDbm(), antenna + 1);
+ } else {
+ tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, getRssiDbm());
+ }
}
#endif // USE_RX_RSSI_DBM
+#ifdef USE_RX_RSNR
+static void osdElementRsnr(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%c%3d", SYM_RSSI, getRsnr());
+}
+#endif // USE_RX_RSNR
+
#ifdef USE_OSD_STICK_OVERLAY
static void osdBackgroundStickOverlay(osdElementParms_t *element)
{
@@ -1383,11 +1469,11 @@ static void osdBackgroundStickOverlay(osdElementParms_t *element)
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)) {
- osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_CENTER);
+ osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_CENTER);
} else if (x == ((OSD_STICK_OVERLAY_WIDTH - 1) / 2)) {
- osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_VERTICAL);
+ osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
} else if (y == ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2)) {
- osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_ATTR_NONE, SYM_STICK_OVERLAY_HORIZONTAL);
+ osdDisplayWriteChar(element, xpos + x, ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_HORIZONTAL);
}
}
}
@@ -1415,7 +1501,7 @@ static void osdElementStickOverlay(osdElementParms_t *element)
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);
- osdDisplayWriteChar(element, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_ATTR_NONE, cursor);
+ osdDisplayWriteChar(element, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_SEVERITY_NORMAL, cursor);
element->drawElement = false; // element already drawn
}
@@ -1428,6 +1514,15 @@ static void osdElementThrottlePosition(osdElementParms_t *element)
static void osdElementTimer(osdElementParms_t *element)
{
+ 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) {
+ element->attr = DISPLAYPORT_SEVERITY_CRITICAL;
+ }
+ }
+
osdFormatTimer(element->buff, true, true, element->item - OSD_ITEM_TIMER_1);
}
@@ -1455,16 +1550,29 @@ static void osdElementVtxChannel(osdElementParms_t *element)
vtxStatusIndicator = 'P';
}
- if (vtxStatus & VTX_STATUS_LOCKED) {
- tfp_sprintf(element->buff, "-:-:-:L");
- } else if (vtxStatusIndicator) {
- tfp_sprintf(element->buff, "%c:%s:%s:%c", vtxBandLetter, vtxChannelName, vtxPowerLabel, vtxStatusIndicator);
- } else {
- tfp_sprintf(element->buff, "%c:%s:%s", vtxBandLetter, vtxChannelName, vtxPowerLabel);
+switch (element->type) {
+ case OSD_ELEMENT_TYPE_2:
+ tfp_sprintf(element->buff, "%s", vtxPowerLabel);
+ break;
+
+ default:
+ if (vtxStatus & VTX_STATUS_LOCKED) {
+ tfp_sprintf(element->buff, "-:-:-:L");
+ } else if (vtxStatusIndicator) {
+ tfp_sprintf(element->buff, "%c:%s:%s:%c", vtxBandLetter, vtxChannelName, vtxPowerLabel, vtxStatusIndicator);
+ } else {
+ tfp_sprintf(element->buff, "%c:%s:%s", vtxBandLetter, vtxChannelName, vtxPowerLabel);
+ }
+ break;
}
}
#endif // USE_VTX_COMMON
+static void osdElementAuxValue(osdElementParms_t *element)
+{
+ tfp_sprintf(element->buff, "%c%d", osdConfig()->aux_symbol, osdAuxValue);
+}
+
static void osdElementWarnings(osdElementParms_t *element)
{
bool elementBlinking = false;
@@ -1474,8 +1582,48 @@ static void osdElementWarnings(osdElementParms_t *element)
} else {
CLR_BLINK(OSD_WARNINGS);
}
+
+#ifdef USE_CRAFTNAME_MSGS
+ // Injects data into the CraftName variable for systems which limit
+ // the available MSP data field in their OSD.
+ if (osdConfig()->osd_craftname_msgs == true) {
+ // if warning is not set, or blink is off, then display LQ & RSSI
+ if (blinkState || (strlen(element->buff) == 0)) {
+#ifdef USE_RX_LINK_QUALITY_INFO
+ // replicate the LQ functionality without the special font symbols
+ uint16_t osdLinkQuality = 0;
+ if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) { // 0-99
+ osdLinkQuality = rxGetLinkQuality();
+#ifdef USE_RX_RSSI_DBM
+ const uint8_t osdRfMode = rxGetRfMode();
+ tfp_sprintf(element->buff, "LQ %2d:%03d %3d", osdRfMode, osdLinkQuality, getRssiDbm());
+ } else if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_GHST) { // 0-100
+ osdLinkQuality = rxGetLinkQuality();
+ tfp_sprintf(element->buff, "LQ %03d %3d", osdLinkQuality, getRssiDbm());
+#endif
+ } else { // 0-9
+ osdLinkQuality = rxGetLinkQuality() * 10 / LINK_QUALITY_MAX_VALUE;
+ if (osdLinkQuality >= 10) {
+ osdLinkQuality = 9;
+ }
+ tfp_sprintf(element->buff, "LQ %1d", osdLinkQuality);
+ }
+#endif // USE_RX_LINK_QUALITY_INFO
+ }
+ strncpy(pilotConfigMutable()->craftName, element->buff, MAX_NAME_LENGTH - 1);
+ }
+#endif // USE_CRAFTNAME_MSGS
}
+#ifdef USE_MSP_DISPLAYPORT
+static void osdElementSys(osdElementParms_t *element)
+{
+ UNUSED(element);
+
+ // Nothing to render for a system element
+}
+#endif
+
// 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
@@ -1496,6 +1644,7 @@ static const uint8_t osdElementDisplayOrder[] = {
OSD_VTX_CHANNEL,
OSD_CURRENT_DRAW,
OSD_MAH_DRAWN,
+ OSD_WATT_HOURS_DRAWN,
OSD_CRAFT_NAME,
OSD_ALTITUDE,
OSD_ROLL_PIDS,
@@ -1511,6 +1660,7 @@ static const uint8_t osdElementDisplayOrder[] = {
OSD_MAIN_BATT_USAGE,
OSD_DISARMED,
OSD_NUMERICAL_HEADING,
+ OSD_READY_MODE,
#ifdef USE_VARIO
OSD_NUMERICAL_VARIO,
#endif
@@ -1523,7 +1673,7 @@ static const uint8_t osdElementDisplayOrder[] = {
#ifdef USE_ACC
OSD_FLIP_ARROW,
#endif
- OSD_DISPLAY_NAME,
+ OSD_PILOT_NAME,
#ifdef USE_RTC_TIME
OSD_RTC_DATETIME,
#endif
@@ -1542,6 +1692,9 @@ static const uint8_t osdElementDisplayOrder[] = {
#ifdef USE_RX_RSSI_DBM
OSD_RSSI_DBM_VALUE,
#endif
+#ifdef USE_RX_RSNR
+ OSD_RSNR_VALUE,
+#endif
#ifdef USE_OSD_STICK_OVERLAY
OSD_STICK_OVERLAY_LEFT,
OSD_STICK_OVERLAY_RIGHT,
@@ -1558,6 +1711,18 @@ static const uint8_t osdElementDisplayOrder[] = {
#ifdef USE_PERSISTENT_STATS
OSD_TOTAL_FLIGHTS,
#endif
+ OSD_AUX_VALUE,
+ OSD_SYS_GOGGLE_VOLTAGE,
+ OSD_SYS_VTX_VOLTAGE,
+ OSD_SYS_BITRATE,
+ OSD_SYS_DELAY,
+ OSD_SYS_DISTANCE,
+ OSD_SYS_LQ,
+ OSD_SYS_GOGGLE_DVR,
+ OSD_SYS_VTX_DVR,
+ OSD_SYS_WARNINGS,
+ OSD_SYS_VTX_TEMP,
+ OSD_SYS_FAN_SPEED,
};
// Define the mapping between the OSD element id and the function to draw it
@@ -1582,6 +1747,7 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
#endif
[OSD_CURRENT_DRAW] = osdElementCurrentDraw,
[OSD_MAH_DRAWN] = osdElementMahDrawn,
+ [OSD_WATT_HOURS_DRAWN] = osdElementWattHoursDrawn,
#ifdef USE_GPS
[OSD_GPS_SPEED] = osdElementGpsSpeed,
[OSD_GPS_SATS] = osdElementGpsSats,
@@ -1594,6 +1760,7 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_PIDRATE_PROFILE] = osdElementPidRateProfile,
[OSD_WARNINGS] = osdElementWarnings,
[OSD_AVG_CELL_VOLTAGE] = osdElementAverageCellVoltage,
+ [OSD_READY_MODE] = osdElementReadyMode,
#ifdef USE_GPS
[OSD_GPS_LON] = osdElementGpsCoordinate,
[OSD_GPS_LAT] = osdElementGpsCoordinate,
@@ -1614,10 +1781,8 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_NUMERICAL_VARIO] = osdElementNumericalVario,
#endif
[OSD_COMPASS_BAR] = osdElementCompassBar,
-#ifdef USE_ESC_SENSOR
- [OSD_ESC_TMP] = osdElementEscTemperature,
-#endif
#if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
+ [OSD_ESC_TMP] = osdElementEscTemperature,
[OSD_ESC_RPM] = osdElementEscRpm,
#endif
[OSD_REMAINING_TIME_ESTIMATE] = osdElementRemainingTimeEstimate,
@@ -1654,7 +1819,7 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_STICK_OVERLAY_LEFT] = osdElementStickOverlay,
[OSD_STICK_OVERLAY_RIGHT] = osdElementStickOverlay,
#endif
- [OSD_DISPLAY_NAME] = NULL, // only has background
+ [OSD_PILOT_NAME] = NULL, // only has background
#if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
[OSD_ESC_RPM_FREQ] = osdElementEscRpmFreq,
#endif
@@ -1667,6 +1832,9 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
#endif
#ifdef USE_RX_RSSI_DBM
[OSD_RSSI_DBM_VALUE] = osdElementRssiDbm,
+#endif
+#ifdef USE_RX_RSNR
+ [OSD_RSNR_VALUE] = osdElementRsnr,
#endif
[OSD_RC_CHANNELS] = osdElementRcChannels,
#ifdef USE_GPS
@@ -1674,6 +1842,20 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
#endif
#ifdef USE_PERSISTENT_STATS
[OSD_TOTAL_FLIGHTS] = osdElementTotalFlights,
+#endif
+ [OSD_AUX_VALUE] = osdElementAuxValue,
+#ifdef USE_MSP_DISPLAYPORT
+ [OSD_SYS_GOGGLE_VOLTAGE] = osdElementSys,
+ [OSD_SYS_VTX_VOLTAGE] = osdElementSys,
+ [OSD_SYS_BITRATE] = osdElementSys,
+ [OSD_SYS_DELAY] = osdElementSys,
+ [OSD_SYS_DISTANCE] = osdElementSys,
+ [OSD_SYS_LQ] = osdElementSys,
+ [OSD_SYS_GOGGLE_DVR] = osdElementSys,
+ [OSD_SYS_VTX_DVR] = osdElementSys,
+ [OSD_SYS_WARNINGS] = osdElementSys,
+ [OSD_SYS_VTX_TEMP] = osdElementSys,
+ [OSD_SYS_FAN_SPEED] = osdElementSys,
#endif
};
@@ -1688,7 +1870,7 @@ const osdElementDrawFn osdElementBackgroundFunction[OSD_ITEM_COUNT] = {
[OSD_STICK_OVERLAY_LEFT] = osdBackgroundStickOverlay,
[OSD_STICK_OVERLAY_RIGHT] = osdBackgroundStickOverlay,
#endif
- [OSD_DISPLAY_NAME] = osdBackgroundDisplayName,
+ [OSD_PILOT_NAME] = osdBackgroundPilotName,
};
static void osdAddActiveElement(osd_items_e element)
@@ -1729,14 +1911,10 @@ void osdAddActiveElements(void)
osdAddActiveElement(OSD_EFFICIENCY);
}
#endif // GPS
-#ifdef USE_ESC_SENSOR
- if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
- osdAddActiveElement(OSD_ESC_TMP);
- }
-#endif
#if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || (motorConfig()->dev.useDshotTelemetry)) {
+ osdAddActiveElement(OSD_ESC_TMP);
osdAddActiveElement(OSD_ESC_RPM);
osdAddActiveElement(OSD_ESC_RPM_FREQ);
}
@@ -1769,12 +1947,16 @@ static void osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
element.buff = (char *)&buff;
element.osdDisplayPort = osdDisplayPort;
element.drawElement = true;
- element.attr = DISPLAYPORT_ATTR_NONE;
+ element.attr = DISPLAYPORT_SEVERITY_NORMAL;
// Call the element drawing function
- osdElementDrawFunction[item](&element);
- if (element.drawElement) {
- osdDisplayWrite(&element, elemPosX, elemPosY, element.attr, buff);
+ if ((item >= OSD_SYS_GOGGLE_VOLTAGE) && (item < OSD_ITEM_COUNT)) {
+ displaySys(osdDisplayPort, elemPosX, elemPosY, (displayPortSystemElement_e)(item - OSD_SYS_GOGGLE_VOLTAGE + DISPLAYPORT_SYS_GOGGLE_VOLTAGE));
+ } else {
+ osdElementDrawFunction[item](&element);
+ if (element.drawElement) {
+ osdDisplayWrite(&element, elemPosX, elemPosY, element.attr, buff);
+ }
}
}
@@ -1801,18 +1983,18 @@ static void osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_
// Call the element background drawing function
osdElementBackgroundFunction[item](&element);
if (element.drawElement) {
- osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_ATTR_NONE, buff);
+ osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_SEVERITY_NORMAL, buff);
}
}
static uint8_t activeElement = 0;
-uint8_t osdGetActiveElement()
+uint8_t osdGetActiveElement(void)
{
return activeElement;
}
-uint8_t osdGetActiveElementCount()
+uint8_t osdGetActiveElementCount(void)
{
return activeOsdElementCount;
}
@@ -1862,7 +2044,8 @@ void osdElementsInit(bool backgroundLayerFlag)
pt1FilterInit(&batteryEfficiencyFilt, pt1FilterGain(EFFICIENCY_CUTOFF_HZ, 1.0f / osdConfig()->framerate_hz));
}
-void osdSyncBlink() {
+void osdSyncBlink(void)
+{
static int blinkCount = 0;
// If the OSD blink is due a transition, do so
@@ -1953,19 +2136,38 @@ void osdUpdateAlarms(void)
CLR_BLINK(OSD_HOME_DIST);
}
} else {
- CLR_BLINK(OSD_HOME_DIST);;
+ CLR_BLINK(OSD_HOME_DIST);
}
#endif
-#ifdef USE_ESC_SENSOR
+#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
+ bool blink;
+
+#if defined(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);
+ blink = osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && osdEscDataCombined->temperature >= osdConfig()->esc_temp_alarm;
+ } else
+#endif
+#if defined(USE_DSHOT_TELEMETRY)
+ {
+ blink = false;
+ if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF) {
+ for (uint32_t k = 0; !blink && (k < getMotorCount()); k++) {
+ blink = (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0 &&
+ dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] >= osdConfig()->esc_temp_alarm;
+ }
}
}
+#else
+ {}
+#endif
+
+ if (blink) {
+ SET_BLINK(OSD_ESC_TMP);
+ } else {
+ CLR_BLINK(OSD_ESC_TMP);
+ }
#endif
}
diff --git a/src/main/osd/osd_warnings.c b/src/main/osd/osd_warnings.c
index d0f823c5e1..a1dbcce2b9 100644
--- a/src/main/osd/osd_warnings.c
+++ b/src/main/osd/osd_warnings.c
@@ -38,6 +38,7 @@
#include "drivers/display.h"
#include "drivers/osd_symbols.h"
#include "drivers/time.h"
+#include "drivers/dshot.h"
#include "fc/core.h"
#include "fc/rc.h"
@@ -74,7 +75,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
static unsigned armingDisabledDisplayIndex;
warningText[0] = '\0';
- *displayAttr = DISPLAYPORT_ATTR_NONE;
+ *displayAttr = DISPLAYPORT_SEVERITY_NORMAL;
*blinking = false;
// Cycle through the arming disabled reasons
@@ -104,7 +105,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
}
tfp_sprintf(warningText, "%s", armingDisableFlagNames[armingDisabledDisplayIndex]);
- *displayAttr = DISPLAYPORT_ATTR_WARNING;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
return;
} else {
armingDisabledUpdateTimeUs = 0;
@@ -122,14 +123,14 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
} else {
tfp_sprintf(warningText, "ARM IN %d.%d", armingDelayTime / 10, armingDelayTime % 10);
}
- *displayAttr = DISPLAYPORT_ATTR_INFO;
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
return;
}
#endif // USE_DSHOT
if (osdWarnGetState(OSD_WARNING_FAIL_SAFE) && failsafeIsActive()) {
tfp_sprintf(warningText, "FAIL SAFE");
- *displayAttr = DISPLAYPORT_ATTR_CRITICAL;
- *blinking = true;;
+ *displayAttr = DISPLAYPORT_SEVERITY_CRITICAL;
+ *blinking = true;
return;
}
@@ -137,11 +138,11 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
if (osdWarnGetState(OSD_WARNING_CRASH_FLIP) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
if (isFlipOverAfterCrashActive()) { // if was armed in crash flip mode
tfp_sprintf(warningText, CRASH_FLIP_WARNING);
- *displayAttr = DISPLAYPORT_ATTR_INFO;
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
return;
} else if (!ARMING_FLAG(ARMED)) { // if disarmed, but crash flip mode is activated
tfp_sprintf(warningText, "CRASH FLIP SWITCH");
- *displayAttr = DISPLAYPORT_ATTR_INFO;
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
return;
}
}
@@ -161,10 +162,10 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
// Blink the message if the throttle is within 10% of the launch setting
if ( calculateThrottlePercent() >= MAX(currentPidProfile->launchControlThrottlePercent - 10, 0)) {
- *blinking = true;;
+ *blinking = true;
}
- *displayAttr = DISPLAYPORT_ATTR_INFO;
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
return;
}
#endif // USE_LAUNCH_CONTROL
@@ -172,34 +173,43 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
// RSSI
if (osdWarnGetState(OSD_WARNING_RSSI) && (getRssiPercent() < osdConfig()->rssi_alarm)) {
tfp_sprintf(warningText, "RSSI LOW");
- *displayAttr = DISPLAYPORT_ATTR_WARNING;
- *blinking = true;;
+ *displayAttr = DISPLAYPORT_SEVERITY_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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
#endif // USE_RX_RSSI_DBM
+#ifdef USE_RX_RSNR
+ // rsnr
+ if (osdWarnGetState(OSD_WARNING_RSNR) && (getRsnr() < osdConfig()->rsnr_alarm)) {
+ tfp_sprintf(warningText, "RSNR LOW");
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
+ return;
+ }
+#endif // USE_RX_RSNR
#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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_CRITICAL;
+ *blinking = true;
return;
}
@@ -210,8 +220,8 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
!gpsRescueIsDisabled() &&
!gpsRescueIsAvailable()) {
tfp_sprintf(warningText, "RESCUE N/A");
- *displayAttr = DISPLAYPORT_ATTR_WARNING;
- *blinking = true;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
@@ -223,8 +233,8 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
}
@@ -234,8 +244,8 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
// Show warning if in HEADFREE flight mode
if (FLIGHT_MODE(HEADFREE_MODE)) {
tfp_sprintf(warningText, "HEADFREE");
- *displayAttr = DISPLAYPORT_ATTR_WARNING;
- *blinking = true;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
@@ -243,8 +253,8 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
#endif // USE_ADC_INTERNAL
@@ -272,7 +282,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
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) {
+ if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && erpmToRpm(escData->rpm) <= (uint32_t)osdConfig()->esc_rpm_alarm) {
warnFlag = 'R';
}
if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {
@@ -295,17 +305,75 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
if (escWarningCount > 0) {
tfp_sprintf(warningText, "%s", escWarningMsg);
- *displayAttr = DISPLAYPORT_ATTR_WARNING;
- *blinking = true;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
}
#endif // USE_ESC_SENSOR
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+ // Show esc error
+ if (osdWarnGetState(OSD_WARNING_ESC_FAIL)) {
+ uint32_t dshotEscErrorLengthMotorBegin;
+ uint32_t dshotEscErrorLength = 0;
+
+ // Write 'ESC'
+ warningText[dshotEscErrorLength++] = 'E';
+ warningText[dshotEscErrorLength++] = 'S';
+ warningText[dshotEscErrorLength++] = 'C';
+
+ for (uint8_t k = 0; k < getMotorCount(); k++) {
+ // Skip if no extended telemetry at all
+ if ((dshotTelemetryState.motorState[k].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) == 0) {
+ continue;
+ }
+
+ // Remember text index before writing warnings
+ dshotEscErrorLengthMotorBegin = dshotEscErrorLength;
+
+ // Write ESC nr
+ warningText[dshotEscErrorLength++] = ' ';
+ warningText[dshotEscErrorLength++] = '0' + k + 1;
+
+ // Add esc warnings
+ if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF
+ && (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) != 0
+ && (dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount) <= osdConfig()->esc_rpm_alarm) {
+ warningText[dshotEscErrorLength++] = 'R';
+ }
+ if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF
+ && (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) != 0
+ && dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] >= osdConfig()->esc_temp_alarm) {
+ warningText[dshotEscErrorLength++] = 'T';
+ }
+ if (ARMING_FLAG(ARMED) && osdConfig()->esc_current_alarm != ESC_CURRENT_ALARM_OFF
+ && (dshotTelemetryState.motorState[k].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) != 0
+ && dshotTelemetryState.motorState[k].telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT] >= osdConfig()->esc_current_alarm) {
+ warningText[dshotEscErrorLength++] = 'C';
+ }
+
+ // If no esc warning data undo esc nr (esc telemetry data types depends on the esc hw/sw)
+ if (dshotEscErrorLengthMotorBegin + 2 == dshotEscErrorLength)
+ dshotEscErrorLength = dshotEscErrorLengthMotorBegin;
+ }
+
+ // If warning exists then notify, otherwise clear warning message
+ if (dshotEscErrorLength > 3) {
+ warningText[dshotEscErrorLength] = 0; // End string
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
+ return;
+ } else {
+ warningText[0] = 0;
+ }
+ }
+#endif
+
if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
tfp_sprintf(warningText, "LOW BATTERY");
- *displayAttr = DISPLAYPORT_ATTR_WARNING;
- *blinking = true;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
@@ -313,8 +381,8 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
// 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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
return;
}
#endif // USE_RC_SMOOTHING_FILTER
@@ -322,23 +390,32 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
// 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;;
+ *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+ *blinking = true;
+ return;
+ }
+
+#ifdef USE_BATTERY_CONTINUE
+ // Show warning if battery is not fresh and battery continue is active
+ if (hasUsedMAh()) {
+ tfp_sprintf(warningText, "BATTERY CONTINUE");
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
return;
}
+#endif // USE_BATTERY_CONTINUE
// 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;
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
return;
}
// Visual beeper
if (osdWarnGetState(OSD_WARNING_VISUAL_BEEPER) && osdGetVisualBeeperState()) {
tfp_sprintf(warningText, " * * * *");
- *displayAttr = DISPLAYPORT_ATTR_INFO;
+ *displayAttr = DISPLAYPORT_SEVERITY_INFO;
osdSetVisualBeeperState(false);
return;
}
diff --git a/src/main/pg/displayport_profiles.c b/src/main/pg/displayport_profiles.c
index 2e702b6b54..6009c42a51 100644
--- a/src/main/pg/displayport_profiles.c
+++ b/src/main/pg/displayport_profiles.c
@@ -30,12 +30,7 @@
#if defined(USE_MSP_DISPLAYPORT)
-PG_REGISTER_WITH_RESET_FN(displayPortProfile_t, displayPortProfileMsp, PG_DISPLAY_PORT_MSP_CONFIG, 0);
-
-void pgResetFn_displayPortProfileMsp(displayPortProfile_t *displayPortProfile)
-{
- displayPortProfile->displayPortSerial = SERIAL_PORT_NONE;
-}
+PG_REGISTER(displayPortProfile_t, displayPortProfileMsp, PG_DISPLAY_PORT_MSP_CONFIG, 1);
#endif
diff --git a/src/main/pg/displayport_profiles.h b/src/main/pg/displayport_profiles.h
index d7642c3655..22a7968c00 100644
--- a/src/main/pg/displayport_profiles.h
+++ b/src/main/pg/displayport_profiles.h
@@ -20,6 +20,7 @@
#pragma once
+#include "io/displayport_msp.h"
#include "pg/pg.h"
typedef struct displayPortProfile_s {
@@ -28,11 +29,10 @@ typedef struct displayPortProfile_s {
bool invert;
uint8_t blackBrightness;
uint8_t whiteBrightness;
- int8_t displayPortSerial; // serialPortIdentifier_e
// For attribute-rich OSDs
- uint8_t attrValues[4]; // NORMAL, INFORMATIONAL, WARNING, CRITICAL
+ uint8_t fontSelection[DISPLAYPORT_SEVERITY_COUNT];
uint8_t useDeviceBlink; // Use device local blink capability
} displayPortProfile_t;
diff --git a/src/main/pg/msp.c b/src/main/pg/msp.c
new file mode 100644
index 0000000000..0af6870104
--- /dev/null
+++ b/src/main/pg/msp.c
@@ -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 .
+ */
+
+#include "platform.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "msp.h"
+
+PG_REGISTER(mspConfig_t, mspConfig, PG_MSP_CONFIG, 0);
diff --git a/src/main/pg/msp.h b/src/main/pg/msp.h
new file mode 100644
index 0000000000..7bff150020
--- /dev/null
+++ b/src/main/pg/msp.h
@@ -0,0 +1,31 @@
+/*
+ * 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/io_types.h"
+
+#include "pg/pg.h"
+
+typedef struct mspConfig_s {
+ uint8_t halfDuplex; // allow msp to operate in half duplex mode
+} mspConfig_t;
+
+PG_DECLARE(mspConfig_t, mspConfig);