From 7e18d7c1b9ffeb5273c557819f140a0d5e232b8b Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 1 Jun 2018 21:23:30 -0400 Subject: [PATCH] Make filter type selectable and add BIQUAD 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) --- src/main/fc/fc_rc.c | 25 +++++++++++++++++++++---- src/main/fc/rc_controls.h | 13 ++++++++++++- src/main/flight/pid.c | 28 ++++++++++++++++++++++------ src/main/flight/pid.h | 2 +- src/main/interface/settings.c | 10 ++++++++++ src/main/interface/settings.h | 2 ++ src/main/pg/rx.c | 2 ++ src/main/pg/rx.h | 2 ++ 8 files changed, 72 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 4bf2a2f393..b2fc539d54 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -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]; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index f8fa15b3ee..2dd62c80f0 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -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)) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a790660e6e..70718e679a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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)); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56ad646c47..cb53cea862 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index c7d43948fe..85897e6c33 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -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) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 39e4c4725a..ba0a8727c9 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -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; diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 9511f461ef..b254ef6f4c 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -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 diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index f8af155415..f91f75f32d 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -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);