diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 561c6cc163..ec5aaa11a2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -662,6 +662,7 @@ const clivalue_t valueTable[] = { { "rssi_scale", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RSSI_SCALE_MIN, RSSI_SCALE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_scale) }, { "rssi_offset", VAR_INT8 | MASTER_VALUE, .config.minmax = { -100, 100 }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_offset) }, { "rssi_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_invert) }, + { "rssi_src_frame_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_src_frame_lpf_period) }, { "rc_interp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolation) }, { "rc_interp_ch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationChannels) }, { "rc_interp_int", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationInterval) }, diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 53821dcd5f..aa4c1da2ae 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -55,6 +55,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .rssi_scale = RSSI_SCALE_DEFAULT, .rssi_offset = 0, .rssi_invert = 0, + .rssi_src_frame_lpf_period = 30, .rcInterpolation = RC_SMOOTHING_AUTO, .rcInterpolationChannels = INTERPOLATION_CHANNELS_RPYT, .rcInterpolationInterval = 19, diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index 465ef91042..dac46e01b9 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -24,6 +24,9 @@ #include "pg/pg.h" +#define GET_FRAME_ERR_LPF_FREQUENCY(period) (1 / (period / 10.0f)) +#define FRAME_ERR_RESAMPLE_US 100000 + typedef struct rxConfig_s { uint8_t rcmap[RX_MAPPABLE_CHANNEL_COUNT]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. @@ -57,6 +60,7 @@ typedef struct rxConfig_s { uint8_t rc_smoothing_input_type; // Input filter type (0 = PT1, 1 = BIQUAD) uint8_t rc_smoothing_derivative_type; // Derivative filter type (0 = OFF, 1 = PT1, 2 = BIQUAD) uint8_t rc_smoothing_auto_factor; // Used to adjust the "smoothness" determined by the auto cutoff calculations + uint8_t rssi_src_frame_lpf_period; // Period of the cutoff frequency for the source frame RSSI filter (in 0.1 s) } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 32dfaaecd9..f3e64fdeee 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -31,6 +31,7 @@ #include "common/maths.h" #include "common/utils.h" +#include "common/filter.h" #include "config/config_reset.h" #include "config/feature.h" @@ -72,6 +73,9 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh"; static uint16_t rssi = 0; // range: [0;1023] static timeUs_t lastMspRssiUpdateUs = 0; + +static pt1Filter_t frameErrFilter; + #ifdef USE_RX_LINK_QUALITY_INFO static uint8_t linkQuality = 0; #endif @@ -313,6 +317,9 @@ void rxInit(void) rssiSource = RSSI_SOURCE_RX_CHANNEL; } + // Setup source frame RSSI filtering to take averaged values every FRAME_ERR_RESAMPLE_US + pt1FilterInit(&frameErrFilter, pt1FilterGain(GET_FRAME_ERR_LPF_FREQUENCY(rxConfig()->rssi_src_frame_lpf_period), FRAME_ERR_RESAMPLE_US/1000000.0)); + rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeConfig.channelCount); } @@ -364,7 +371,7 @@ static uint8_t updateLinkQualitySamples(uint8_t value) } #endif -static void setLinkQuality(bool validFrame) +static void setLinkQuality(bool validFrame, timeDelta_t currentDeltaTime) { #ifdef USE_RX_LINK_QUALITY_INFO // calculate new sample mean @@ -372,14 +379,25 @@ static void setLinkQuality(bool validFrame) #endif if (rssiSource == RSSI_SOURCE_FRAME_ERRORS) { - setRssi(validFrame ? RSSI_MAX_VALUE : 0, RSSI_SOURCE_FRAME_ERRORS); + static uint16_t tot_rssi = 0; + static uint16_t cnt_rssi = 0; + static timeDelta_t resample_time = 0; + + resample_time += currentDeltaTime; + tot_rssi += validFrame ? RSSI_MAX_VALUE : 0; + cnt_rssi++; + + if (resample_time >= FRAME_ERR_RESAMPLE_US) { + setRssi(tot_rssi / cnt_rssi, rssiSource); + tot_rssi = 0; + cnt_rssi = 0; + resample_time -= FRAME_ERR_RESAMPLE_US; + } } } bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) { - UNUSED(currentDeltaTime); - bool signalReceived = false; bool useDataDrivenProcessing = true; @@ -410,7 +428,7 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; } - setLinkQuality(signalReceived); + setLinkQuality(signalReceived, currentDeltaTime); } if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) { @@ -478,7 +496,7 @@ static uint16_t getRxfailValue(uint8_t channel) } } - FALLTHROUGH; + FALLTHROUGH; default: case RX_FAILSAFE_MODE_INVALID: case RX_FAILSAFE_MODE_HOLD: @@ -639,8 +657,13 @@ void setRssi(uint16_t rssiValue, rssiSource_e source) return; } - // calculate new sample mean - rssi = updateRssiSamples(rssiValue); + // Filter RSSI value + if (source == RSSI_SOURCE_FRAME_ERRORS) { + rssi = pt1FilterApply(&frameErrFilter, rssiValue); + } else { + // calculate new sample mean + rssi = updateRssiSamples(rssiValue); + } } void setRssiMsp(uint8_t newMspRssi) diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 220dce3f68..fb7fdb03c8 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -223,4 +223,24 @@ void failsafeOnValidDataFailed(void) { } +float pt1FilterGain(float f_cut, float dT) +{ + UNUSED(f_cut); + UNUSED(dT); + return 0.0; +} + +void pt1FilterInit(pt1Filter_t *filter, float k) +{ + UNUSED(filter); + UNUSED(k); +} + +float pt1FilterApply(pt1Filter_t *filter, float input) +{ + UNUSED(filter); + UNUSED(input); + return 0.0; +} + } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index d73285c445..845480b9f9 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -235,4 +235,23 @@ extern "C" { void xBusInit(const rxConfig_t *, rxRuntimeConfig_t *) {} void rxMspInit(const rxConfig_t *, rxRuntimeConfig_t *) {} void rxPwmInit(const rxConfig_t *, rxRuntimeConfig_t *) {} + float pt1FilterGain(float f_cut, float dT) + { + UNUSED(f_cut); + UNUSED(dT); + return 0.0; + } + + void pt1FilterInit(pt1Filter_t *filter, float k) + { + UNUSED(filter); + UNUSED(k); + } + + float pt1FilterApply(pt1Filter_t *filter, float input) + { + UNUSED(filter); + UNUSED(input); + return 0.0; + } }