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

Make filter type selectable and add BIQUAD

Browse source 
Adds options to select the filter type for both input and derivative.
rc_smoothing_input_type = PT1 | BIQUAD (default is BIQUAD)
rc_smoothing_derivative_type = OFF | PT1 | BIQUAD (default is OFF)
This commit is contained in:
Bruce Luckcuck 2018-06-01 21:23:30 -04:00
parent 8b42c80790
commit 7e18d7c1b9
8 changed files with 72 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

25
src/main/fc/fc_rc.c
View file

@ -250,7 +250,8 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
uint8_t updatedChannel = 0;
static float lastRxData[4];
static pt1Filter_t rcCommandFilter[4];
static pt1Filter_t rcCommandFilterPt1[4];
static biquadFilter_t rcCommandFilterBiquad[4];
static bool initialized = false;
static bool filterInitialized = false;
static float rxFrameTimeSum;
@ -285,9 +286,17 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
const float dT = targetPidLooptime * 1e-6f;
for (int i = 0; i < interpolationChannels; i++) {
pt1FilterInit(&rcCommandFilter[i], pt1FilterGain(filterCutoffFrequency, dT));
switch (rxConfig()->rc_smoothing_input_type) {
case RC_SMOOTHING_INPUT_BIQUAD:
biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
break;
case RC_SMOOTHING_INPUT_PT1:
default:
pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT));
break;
}
}
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis);
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
filterInitialized = true;
}
} else {
@ -302,7 +311,15 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
if (filterInitialized) {
rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilter[updatedChannel], lastRxData[updatedChannel]);
switch (rxConfig()->rc_smoothing_input_type) {
case RC_SMOOTHING_INPUT_BIQUAD:
rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
break;
case RC_SMOOTHING_INPUT_PT1:
default:
rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]);
break;
}
} else {
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
rcCommand[updatedChannel] = lastRxData[updatedChannel];

13
src/main/fc/rc_controls.h
View file

@ -61,7 +61,18 @@ typedef enum {
typedef enum {
RC_SMOOTHING_TYPE_INTERPOLATION,
RC_SMOOTHING_TYPE_FILTER
} rcInterpolationType_e;
} rcSmoothingType_e;
typedef enum {
RC_SMOOTHING_INPUT_PT1,
RC_SMOOTHING_INPUT_BIQUAD
} rcSmoothingInputFilter_e;
typedef enum {
RC_SMOOTHING_DERIVATIVE_OFF,
RC_SMOOTHING_DERIVATIVE_PT1,
RC_SMOOTHING_DERIVATIVE_BIQUAD
} rcSmoothingDerivativeFilter_e;
#define ROL_LO (1 << (2 * ROLL))

28
src/main/flight/pid.c
View file

@ -211,9 +211,11 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativeLpf[2];
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2];
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2];
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
void pidInitFilters(const pidProfile_t *pidProfile)
@ -302,11 +304,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis) {
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) {
setpointDerivativeLpfInitialized = true;
rcSmoothingDebugAxis = debugAxis;
rcSmoothingFilterType = filterType;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
pt1FilterInit(&setpointDerivativeLpf[axis], pt1FilterGain(filterCutoff, dT));
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
}
}
#endif // USE_RC_SMOOTHING_FILTER
@ -884,9 +894,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
}
if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
pidSetpointDelta = pt1FilterApply(&setpointDerivativeLpf[axis], pidSetpointDelta);
if ((dynCd != 0) && setpointDerivativeLpfInitialized && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
switch (rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
pidSetpointDelta = biquadFilterApply(&setpointDerivativeBiquad[axis], pidSetpointDelta);
break;
}
if (axis == rcSmoothingDebugAxis) {
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f));
}

2
src/main/flight/pid.h
View file

@ -178,4 +178,4 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
bool crashRecoveryModeActive(void);
void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);

10
src/main/interface/settings.c
View file

@ -356,6 +356,12 @@ static const char * const lookupTableRcSmoothingType[] = {
static const char * const lookupTableRcSmoothingDebug[] = {
"ROLL", "PITCH", "YAW", "THROTTLE"
};
static const char * const lookupTableRcSmoothingInputType[] = {
"PT1", "BIQUAD"
};
static const char * const lookupTableRcSmoothingDerivativeType[] = {
"OFF", "PT1", "BIQUAD"
};
#endif // USE_RC_SMOOTHING_FILTER
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
@ -442,6 +448,8 @@ const lookupTableEntry_t lookupTables[] = {
#ifdef USE_RC_SMOOTHING_FILTER
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER
};
@ -544,6 +552,8 @@ const clivalue_t valueTable[] = {
{ "rc_smoothing_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) },
{ "rc_smoothing_derivative_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_cutoff) },
{ "rc_smoothing_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
{ "rc_smoothing_input_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_INPUT_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_type) },
{ "rc_smoothing_derivative_type",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_type) },
#endif // USE_RC_SMOOTHING_FILTER
{ "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },

2
src/main/interface/settings.h
View file

@ -107,6 +107,8 @@ typedef enum {
#ifdef USE_RC_SMOOTHING_FILTER
TABLE_RC_SMOOTHING_TYPE,
TABLE_RC_SMOOTHING_DEBUG,
TABLE_RC_SMOOTHING_INPUT_TYPE,
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;

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

@ -64,6 +64,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default
.rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis
.rc_smoothing_input_type = RC_SMOOTHING_INPUT_BIQUAD,
.rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_OFF,
);
#ifdef RX_CHANNELS_TAER

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

@ -55,6 +55,8 @@ typedef struct rxConfig_s {
uint8_t rc_smoothing_input_cutoff; // Filter cutoff frequency for the input filter (0 = auto)
uint8_t rc_smoothing_derivative_cutoff; // Filter cutoff frequency for the setpoint weight derivative filter (0 = auto)
uint8_t rc_smoothing_debug_axis; // Axis to log as debug values when debug_mode = RC_SMOOTHING
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)
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);