diff --git a/docs/Settings.md b/docs/Settings.md index 39e3de90d52..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,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` diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 91ef0ff1b3e..ec956937ffa 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index beadf8c96b4..2385e3981db 100644 --- a/src/main/io/osd.c +++ b/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; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 88240ff84c0..cd3c4a0787e 100644 --- a/src/main/io/osd.h +++ b/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