Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Use PT1 filter for frame based RSSI using rssi_src_frame_lpf_period when rssi_src_frame_errors = ON

Browse source
This commit is contained in:
SteveCEvans 2019-04-14 18:35:22 +01:00
parent 95be4b0195
commit 91b6f420fc
6 changed files with 76 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/cli/settings.c
View file

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

1
src/main/pg/rx.c
View file

@ -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,

4
src/main/pg/rx.h
View file

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

33
src/main/rx/rx.c
View file

@ -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,8 +657,13 @@ 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)

20
src/test/unit/rx_ranges_unittest.cc
View file

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

19
src/test/unit/rx_rx_unittest.cc
View file

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