Use PT1 for source frame RSSI filtering (#8013)

Use PT1 for source frame RSSI filtering

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) },

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,

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);

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) {
@@ -639,9 +657,14 @@ void setRssi(uint16_t rssiValue, rssiSource_e source)
         return;
     }
 
+    // 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)
 {

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;
+}
+
 }

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;
+    }
 }