Add smoothing to glide related OSD elements

docs/Settings.md

@@ -4902,6 +4902,16 @@ To vertically adjust the whole OSD and AHI and scrolling bars
 
 ---
 
+### osd_horizontal_velocity_filter_tc
+
+OSD horizontal velocity filter time constant in seconds. Set to 0 to disable filtering (passthrough).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 5 |
+
+---
+
 ### osd_hud_homepoint
 
 To 3D-display the home point location in the hud
@@ -5532,6 +5542,16 @@ Use custom pilot logo with/instead of the INAV logo. The pilot logo must be char
 
 ---
 
+### osd_vertical_velocity_filter_tc_ratio
+
+Multiplier applied to the horizontal velocity filter time constant to derive vertical velocity smoothing.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3 | 1 | 10 |
+
+---
+
 ### osd_video_system
 
 Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR', `BF43COMPAT`, `BFHDCOMPAT` and `DJI_NATIVE`

src/main/fc/settings.yaml

@@ -3363,6 +3363,20 @@ groups:
         field: rssi_alarm
         min: 0
         max: 100
+      - name: osd_horizontal_velocity_filter_tc
+        description: "OSD horizontal velocity filter time constant in seconds. Set to 0 to disable filtering (passthrough)."
+        type: float
+        default_value: 0
+        min: 0
+        max: 5
+        field: horizontal_velocity_filter_tc
+      - name: osd_vertical_velocity_filter_tc_ratio
+        description: "Multiplier applied to the horizontal velocity filter time constant to derive vertical velocity smoothing."
+        type: float
+        default_value: 3
+        min: 1
+        max: 10
+        field: vertical_velocity_filter_tc_ratio
       - name: osd_time_alarm
         description: "Value above which to make the OSD flight time indicator blink (minutes)"
         default_value: 10

src/main/io/osd.c

@@ -129,6 +129,8 @@
 #define VIDEO_BUFFER_CHARS_DJIWTF 1320
 
 #define GFORCE_FILTER_TC 0.2
+#define HORIZONTAL_VELOCITY_FILTER_TC osdConfig()->horizontal_velocity_filter_tc
+#define VERTICAL_VELOCITY_FILTER_TC (HORIZONTAL_VELOCITY_FILTER_TC * osdConfig()->vertical_velocity_filter_tc_ratio)
 
 #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18
 #define IS_HI(X)  (rxGetChannelValue(X) > 1750)
@@ -174,6 +176,21 @@ static timeMs_t layoutOverrideUntil = 0;
 static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT];
 static float GForce, GForceAxis[XYZ_AXIS_COUNT];
 
+static pt1Filter_t verticalVelocityFilter;
+static pt1Filter_t horizontalVelocityFilter;
+static float filteredVerticalVelocity = 0.0f;
+static float filteredHorizontalVelocity = 0.0f;
+
+static float getAveragedVerticalVelocity(void)
+{
+    return filteredVerticalVelocity;
+}
+
+static float getAveragedHorizontalVelocity(void)
+{
+    return filteredHorizontalVelocity;
+}
+
 typedef struct statistic_s {
     uint16_t max_speed;
     uint16_t max_3D_speed;
@@ -225,7 +242,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 16);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
 
 void osdStartedSaveProcess(void) {
@@ -1268,12 +1285,12 @@ static inline int32_t osdGetAltitudeMsl(void)
 }
 
 uint16_t osdGetRemainingGlideTime(void) {
-    float value = getEstimatedActualVelocity(Z);
+    float value = getAveragedVerticalVelocity();
     static pt1Filter_t glideTimeFilterState;
     const  timeMs_t curTimeMs = millis();
     static timeMs_t glideTimeUpdatedMs;
 
-    value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs));
+    value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.15, MS2S(curTimeMs - glideTimeUpdatedMs));
     glideTimeUpdatedMs = curTimeMs;
 
     if (value < 0) {
@@ -2025,13 +2042,13 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_GLIDESLOPE:
         {
-            float horizontalSpeed = gpsSol.groundSpeed;
-            float sinkRate = -getEstimatedActualVelocity(Z);
+            float horizontalSpeed = getAveragedHorizontalVelocity();
+            float sinkRate = -getAveragedVerticalVelocity();
             static pt1Filter_t gsFilterState;
             const timeMs_t currentTimeMs = millis();
             static timeMs_t gsUpdatedTimeMs;
             float glideSlope = horizontalSpeed / sinkRate;
-            glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs));
+            glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.15, MS2S(currentTimeMs - gsUpdatedTimeMs));
             gsUpdatedTimeMs = currentTimeMs;
 
             buff[0] = SYM_GLIDESLOPE;
@@ -3144,7 +3161,7 @@ static bool osdDrawSingleElement(uint8_t item)
             uint16_t glideSeconds = osdGetRemainingGlideTime();
             buff[0] = SYM_GLIDE_DIST;
             if (glideSeconds > 0) {
-                uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed;
+                uint32_t glideRangeCM = glideSeconds * getAveragedHorizontalVelocity();
                 osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0, 3);
             } else {
                 tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK);
@@ -4376,7 +4393,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT,
     .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT,
 
-    .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT
+    .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT,
+    .horizontal_velocity_filter_tc = SETTING_OSD_HORIZONTAL_VELOCITY_FILTER_TC_DEFAULT,
+    .vertical_velocity_filter_tc_ratio = SETTING_OSD_VERTICAL_VELOCITY_FILTER_TC_RATIO_DEFAULT
 );
 
 void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
@@ -5758,6 +5777,12 @@ static void osdFilterData(timeUs_t currentTimeUs) {
     if (lastRefresh) {
         GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT);
         for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT);
+        filteredVerticalVelocity = pt1FilterApply3(&verticalVelocityFilter, getEstimatedActualVelocity(Z), refresh_dT);
+#if defined(USE_GPS)
+        filteredHorizontalVelocity = pt1FilterApply3(&horizontalVelocityFilter, gpsSol.groundSpeed, refresh_dT);
+#else
+        filteredHorizontalVelocity = 0.0f;
+#endif   
     } else {
         pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0);
         pt1FilterReset(&GForceFilter, GForce);
@@ -5766,6 +5791,15 @@ static void osdFilterData(timeUs_t currentTimeUs) {
             pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0);
             pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]);
         }
+        pt1FilterInitRC(&verticalVelocityFilter, VERTICAL_VELOCITY_FILTER_TC, 0);
+        pt1FilterReset(&verticalVelocityFilter, getEstimatedActualVelocity(Z));
+#if defined(USE_GPS)
+        pt1FilterInitRC(&horizontalVelocityFilter, HORIZONTAL_VELOCITY_FILTER_TC, 0);
+        pt1FilterReset(&horizontalVelocityFilter, gpsSol.groundSpeed);
+#else
+        pt1FilterInitRC(&horizontalVelocityFilter, HORIZONTAL_VELOCITY_FILTER_TC, 0);
+        pt1FilterReset(&horizontalVelocityFilter, 0);
+#endif
     }
 
     lastRefresh = currentTimeUs;

src/main/io/osd.h

@@ -536,6 +536,8 @@ typedef struct osdConfig_s {
     osd_adsb_warning_style_e    adsb_warning_style;       // adsb warning element style, one or two lines
 #endif
     uint8_t  radar_peers_display_time;                  // in seconds
+    float    horizontal_velocity_filter_tc;             // OSD horizontal velocity filter time constant (seconds). 0 = passthrough
+    float    vertical_velocity_filter_tc_ratio;         // Multiplier applied to the horizontal velocity filter TC for vertical velocity smoothing
 #ifdef USE_GEOZONE
     uint8_t geozoneDistanceWarning;                     // Distance to fence or action
     bool geozoneDistanceType;                            // Shows a countdown timer or distance to fence/action