From 2bfa08e344f09cb8d48e3ee549a7fa9cf4673acd Mon Sep 17 00:00:00 2001 From: y-decimal <83334510+y-decimal@users.noreply.github.com> Date: Mon, 27 Apr 2026 18:57:56 +0200 Subject: [PATCH 1/6] feat: implement velocity averaging for OSD display fix: update glide time calculations to use averaged velocities Co-authored-by: Copilot --- src/main/io/osd.c | 51 +++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 47 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4fe19a4040..794080bba7c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -174,6 +174,47 @@ static timeMs_t layoutOverrideUntil = 0; static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT]; static float GForce, GForceAxis[XYZ_AXIS_COUNT]; +static float verticalVelocityBuffer[5] = {0}; +static float averagedVerticalVelocity = 0.0f; +static float horizontalVelocityBuffer[5] = {0}; +static float averagedHorizontalVelocity = 0.0f; +static uint8_t velocityAveragingIndex = 0; + +static void osdUpdateVelocityAverages(void) +{ + const float verticalVelocity = getEstimatedActualVelocity(Z); +#if defined(USE_GPS) + const float horizontalVelocity = gpsSol.groundSpeed; +#else + const float horizontalVelocity = 0.0f; +#endif + + verticalVelocityBuffer[velocityAveragingIndex] = isnormal(verticalVelocity) ? verticalVelocity : 0.0f; + horizontalVelocityBuffer[velocityAveragingIndex] = isnormal(horizontalVelocity) ? horizontalVelocity : 0.0f; + + velocityAveragingIndex = (velocityAveragingIndex + 1) % ARRAYLEN(verticalVelocityBuffer); + + float verticalVelocitySum = 0.0f; + float horizontalVelocitySum = 0.0f; + for (uint8_t i = 0; i < ARRAYLEN(verticalVelocityBuffer); i++) { + verticalVelocitySum += verticalVelocityBuffer[i]; + horizontalVelocitySum += horizontalVelocityBuffer[i]; + } + + averagedVerticalVelocity = verticalVelocitySum / ARRAYLEN(verticalVelocityBuffer); + averagedHorizontalVelocity = horizontalVelocitySum / ARRAYLEN(horizontalVelocityBuffer); +} + +static float getAveragedVerticalVelocity(void) +{ + return averagedVerticalVelocity; +} + +static float getAveragedHorizontalVelocity(void) +{ + return averagedHorizontalVelocity; +} + typedef struct statistic_s { uint16_t max_speed; uint16_t max_3D_speed; @@ -1272,7 +1313,7 @@ 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; @@ -2029,8 +2070,8 @@ 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; @@ -3148,7 +3189,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); @@ -5793,6 +5834,8 @@ static void osdFilterData(timeUs_t currentTimeUs) { } } + osdUpdateVelocityAverages(); + lastRefresh = currentTimeUs; } From 7b7264153a6be3814082d4abffbf772ec2ff2710 Mon Sep 17 00:00:00 2001 From: y-decimal <83334510+y-decimal@users.noreply.github.com> Date: Mon, 27 Apr 2026 19:40:54 +0200 Subject: [PATCH 2/6] refactor: redo velocity averaging implementation with a pt1 filter instead of rolling average, in order to have time constant and sample rate independent filtering behaviour Co-authored-by: Copilot --- src/main/io/osd.c | 57 +++++++++++++++++++---------------------------- 1 file changed, 23 insertions(+), 34 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1bdd0cd072e..bbd9e374661 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -129,6 +129,7 @@ #define VIDEO_BUFFER_CHARS_DJIWTF 1320 #define GFORCE_FILTER_TC 0.2 +#define VELOCITY_FILTER_TC 1.5 #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18 #define IS_HI(X) (rxGetChannelValue(X) > 1750) @@ -174,45 +175,19 @@ static timeMs_t layoutOverrideUntil = 0; static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT]; static float GForce, GForceAxis[XYZ_AXIS_COUNT]; -static float verticalVelocityBuffer[5] = {0}; -static float averagedVerticalVelocity = 0.0f; -static float horizontalVelocityBuffer[5] = {0}; -static float averagedHorizontalVelocity = 0.0f; -static uint8_t velocityAveragingIndex = 0; - -static void osdUpdateVelocityAverages(void) -{ - const float verticalVelocity = getEstimatedActualVelocity(Z); -#if defined(USE_GPS) - const float horizontalVelocity = gpsSol.groundSpeed; -#else - const float horizontalVelocity = 0.0f; -#endif - - verticalVelocityBuffer[velocityAveragingIndex] = isnormal(verticalVelocity) ? verticalVelocity : 0.0f; - horizontalVelocityBuffer[velocityAveragingIndex] = isnormal(horizontalVelocity) ? horizontalVelocity : 0.0f; - - velocityAveragingIndex = (velocityAveragingIndex + 1) % ARRAYLEN(verticalVelocityBuffer); - - float verticalVelocitySum = 0.0f; - float horizontalVelocitySum = 0.0f; - for (uint8_t i = 0; i < ARRAYLEN(verticalVelocityBuffer); i++) { - verticalVelocitySum += verticalVelocityBuffer[i]; - horizontalVelocitySum += horizontalVelocityBuffer[i]; - } - - averagedVerticalVelocity = verticalVelocitySum / ARRAYLEN(verticalVelocityBuffer); - averagedHorizontalVelocity = horizontalVelocitySum / ARRAYLEN(horizontalVelocityBuffer); -} +static pt1Filter_t verticalVelocityFilter; +static pt1Filter_t horizontalVelocityFilter; +static float filteredVerticalVelocity = 0.0f; +static float filteredHorizontalVelocity = 0.0f; static float getAveragedVerticalVelocity(void) { - return averagedVerticalVelocity; + return filteredVerticalVelocity; } static float getAveragedHorizontalVelocity(void) { - return averagedHorizontalVelocity; + return filteredHorizontalVelocity; } typedef struct statistic_s { @@ -5799,6 +5774,13 @@ 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); @@ -5807,10 +5789,17 @@ static void osdFilterData(timeUs_t currentTimeUs) { pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); } + pt1FilterInitRC(&verticalVelocityFilter, VELOCITY_FILTER_TC, 0); + pt1FilterReset(&verticalVelocityFilter, getEstimatedActualVelocity(Z)); +#if defined(USE_GPS) + pt1FilterInitRC(&horizontalVelocityFilter, VELOCITY_FILTER_TC, 0); + pt1FilterReset(&horizontalVelocityFilter, gpsSol.groundSpeed); +#else + pt1FilterInitRC(&horizontalVelocityFilter, VELOCITY_FILTER_TC, 0); + pt1FilterReset(&horizontalVelocityFilter, 0); +#endif } - osdUpdateVelocityAverages(); - lastRefresh = currentTimeUs; } From 8c6eeb7048d28aafb238f7a79354c31dc2a6728b Mon Sep 17 00:00:00 2001 From: y-decimal <83334510+y-decimal@users.noreply.github.com> Date: Tue, 28 Apr 2026 18:05:46 +0200 Subject: [PATCH 3/6] fix: remove duplicate closing brace in osdFilterData function --- src/main/io/osd.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bbd9e374661..ae8e15cbca2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5780,7 +5780,6 @@ static void osdFilterData(timeUs_t currentTimeUs) { #else filteredHorizontalVelocity = 0.0f; #endif - } } else { pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); pt1FilterReset(&GForceFilter, GForce); From 80ec939423ff820cf09ae1f18af2e8ceb43a5d1d Mon Sep 17 00:00:00 2001 From: y-decimal <83334510+y-decimal@users.noreply.github.com> Date: Mon, 18 May 2026 15:30:39 +0200 Subject: [PATCH 4/6] feat: add OSD velocity filter time constant setting to cli feat: update OSD to use configurable velocity filter time constant --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 7 +++++++ src/main/io/osd.c | 2 +- src/main/io/osd.h | 1 + 4 files changed, 19 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 39e3de90d52..006e965e472 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5532,6 +5532,16 @@ Use custom pilot logo with/instead of the INAV logo. The pilot logo must be char --- +### osd_velocity_filter_tc + +OSD velocity filter time constant in seconds. Set to 0 to disable filtering (passthrough). + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 5 | + +--- + ### osd_video_system Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR', `BF43COMPAT`, `BFHDCOMPAT` and `DJI_NATIVE` diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 91ef0ff1b3e..0247553eb74 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3363,6 +3363,13 @@ groups: field: rssi_alarm min: 0 max: 100 + - name: osd_velocity_filter_tc + description: "OSD velocity filter time constant in seconds. Set to 0 to disable filtering (passthrough)." + type: float + default_value: 0 + min: 0 + max: 5 + field: velocity_filter_tc - name: osd_time_alarm description: "Value above which to make the OSD flight time indicator blink (minutes)" default_value: 10 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ae8e15cbca2..e9bdc5004cc 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -129,7 +129,7 @@ #define VIDEO_BUFFER_CHARS_DJIWTF 1320 #define GFORCE_FILTER_TC 0.2 -#define VELOCITY_FILTER_TC 1.5 +#define VELOCITY_FILTER_TC osdConfig()->velocity_filter_tc #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18 #define IS_HI(X) (rxGetChannelValue(X) > 1750) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 88240ff84c0..26b76c3e551 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -536,6 +536,7 @@ 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 velocity_filter_tc; // OSD velocity filter time constant (seconds). 0 = passthrough #ifdef USE_GEOZONE uint8_t geozoneDistanceWarning; // Distance to fence or action bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action From 3dc6648d298c4b20aad684c64146a2c8a4b7c4e5 Mon Sep 17 00:00:00 2001 From: y-decimal <83334510+y-decimal@users.noreply.github.com> Date: Mon, 18 May 2026 18:54:40 +0200 Subject: [PATCH 5/6] fix: adjust glide slope filter cutoff frequency for improved stabilityglide --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e9bdc5004cc..6eb64d53179 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1289,7 +1289,7 @@ uint16_t osdGetRemainingGlideTime(void) { 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) { @@ -2047,7 +2047,7 @@ static bool osdDrawSingleElement(uint8_t item) 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; From fd2abd03e9b4326e7f6c807be73791eb4edeff01 Mon Sep 17 00:00:00 2001 From: y-decimal <83334510+y-decimal@users.noreply.github.com> Date: Mon, 18 May 2026 18:56:41 +0200 Subject: [PATCH 6/6] refactor: split filtering into separate components for horizontal and vertical velocity, vertical velocity filter time constant is set as a multiple of horizontal velocity (due to higher volatility of vertical readings) --- docs/Settings.md | 16 +++++++++++++--- src/main/fc/settings.yaml | 13 ++++++++++--- src/main/io/osd.c | 15 +++++++++------ src/main/io/osd.h | 3 ++- 4 files changed, 34 insertions(+), 13 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 006e965e472..edf738dfb66 100644 --- a/docs/Settings.md +++ b/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,13 +5542,13 @@ Use custom pilot logo with/instead of the INAV logo. The pilot logo must be char --- -### osd_velocity_filter_tc +### osd_vertical_velocity_filter_tc_ratio -OSD velocity filter time constant in seconds. Set to 0 to disable filtering (passthrough). +Multiplier applied to the horizontal velocity filter time constant to derive vertical velocity smoothing. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 5 | +| 3 | 1 | 10 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0247553eb74..ec956937ffa 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3363,13 +3363,20 @@ groups: field: rssi_alarm min: 0 max: 100 - - name: osd_velocity_filter_tc - description: "OSD velocity filter time constant in seconds. Set to 0 to disable filtering (passthrough)." + - 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: velocity_filter_tc + 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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6eb64d53179..2385e3981db 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -129,7 +129,8 @@ #define VIDEO_BUFFER_CHARS_DJIWTF 1320 #define GFORCE_FILTER_TC 0.2 -#define VELOCITY_FILTER_TC osdConfig()->velocity_filter_tc +#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) @@ -241,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) { @@ -4392,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) @@ -5788,13 +5791,13 @@ static void osdFilterData(timeUs_t currentTimeUs) { pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); } - pt1FilterInitRC(&verticalVelocityFilter, VELOCITY_FILTER_TC, 0); + pt1FilterInitRC(&verticalVelocityFilter, VERTICAL_VELOCITY_FILTER_TC, 0); pt1FilterReset(&verticalVelocityFilter, getEstimatedActualVelocity(Z)); #if defined(USE_GPS) - pt1FilterInitRC(&horizontalVelocityFilter, VELOCITY_FILTER_TC, 0); + pt1FilterInitRC(&horizontalVelocityFilter, HORIZONTAL_VELOCITY_FILTER_TC, 0); pt1FilterReset(&horizontalVelocityFilter, gpsSol.groundSpeed); #else - pt1FilterInitRC(&horizontalVelocityFilter, VELOCITY_FILTER_TC, 0); + pt1FilterInitRC(&horizontalVelocityFilter, HORIZONTAL_VELOCITY_FILTER_TC, 0); pt1FilterReset(&horizontalVelocityFilter, 0); #endif } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 26b76c3e551..cd3c4a0787e 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -536,7 +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 velocity_filter_tc; // OSD velocity filter time constant (seconds). 0 = passthrough + 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