diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 27f33eb206..57e6e493a4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1461,8 +1461,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting, rcSmoothingData->derivativeCutoffSetting); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType, - rcSmoothingData->derivativeFilterType); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency, rcSmoothingData->derivativeCutoffFrequency); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs); diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 4f9b73f944..95b2880ab9 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4967,28 +4967,17 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline) cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000); } } - cliPrintLinef("# Input filter type: %s", lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]); cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency); if (rcSmoothingData->inputCutoffSetting == 0) { cliPrintLine("(auto)"); } else { cliPrintLine("(manual)"); } - cliPrintf("# Derivative filter type: %s", lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]); - if (rcSmoothingData->derivativeFilterTypeSetting == RC_SMOOTHING_DERIVATIVE_AUTO) { - cliPrintLine(" (auto)"); - } else { - cliPrintLinefeed(); - } cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency); - if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) { - cliPrintLine("off)"); + if (rcSmoothingData->derivativeCutoffSetting == 0) { + cliPrintLine("auto)"); } else { - if (rcSmoothingData->derivativeCutoffSetting == 0) { - cliPrintLine("auto)"); - } else { - cliPrintLine("manual)"); - } + cliPrintLine("manual)"); } } else { cliPrintLine("INTERPOLATION"); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 270254875a..eed757110b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -412,12 +412,6 @@ 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", "AUTO" -}; #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_VTX_COMMON @@ -605,8 +599,6 @@ 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 #ifdef USE_VTX_COMMON LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm), @@ -750,8 +742,6 @@ const clivalue_t valueTable[] = { { "rc_smoothing_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) }, { "rc_smoothing_derivative_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 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) }, { "rc_smoothing_auto_smoothness",VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor) }, #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 1a8b9c9bc6..47a3992ce2 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -112,8 +112,6 @@ 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 #ifdef USE_VTX_COMMON TABLE_VTX_LOW_POWER_DISARM, diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 4ab5f9abca..e6b2f011a4 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -65,6 +65,71 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input) return filter->state; } +// PT2 Low Pass filter + +float pt2FilterGain(float f_cut, float dT) +{ + const float order = 2.0f; + const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1)); + float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut); + //float RC = 1 / ( 2 * 1.553773974f * M_PIf * f_cut); + // where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2 + return dT / (RC + dT); +} + +void pt2FilterInit(pt2Filter_t *filter, float k) +{ + filter->state = 0.0f; + filter->state1 = 0.0f; + filter->k = k; +} + +void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) +{ + filter->k = k; +} + +FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input) +{ + filter->state1 = filter->state1 + filter->k * (input - filter->state1); + filter->state = filter->state + filter->k * (filter->state1 - filter->state); + return filter->state; +} + +// PT3 Low Pass filter + +float pt3FilterGain(float f_cut, float dT) +{ + const float order = 3.0f; + const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1)); + float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut); + // float RC = 1 / ( 2 * 1.961459177f * M_PIf * f_cut); + // where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3 + return dT / (RC + dT); +} + +void pt3FilterInit(pt3Filter_t *filter, float k) +{ + filter->state = 0.0f; + filter->state1 = 0.0f; + filter->state2 = 0.0f; + filter->k = k; +} + +void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) +{ + filter->k = k; +} + +FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input) +{ + filter->state1 = filter->state1 + filter->k * (input - filter->state1); + filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2); + filter->state = filter->state + filter->k * (filter->state2 - filter->state); + return filter->state; +} + + // Slew filter with limit void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold) diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 5bc1a045b1..9c3a8f3d31 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -29,6 +29,19 @@ typedef struct pt1Filter_s { float k; } pt1Filter_t; +typedef struct pt2Filter_s { + float state; + float state1; + float k; +} pt2Filter_t; + +typedef struct pt3Filter_s { + float state; + float state1; + float state2; + float k; +} pt3Filter_t; + typedef struct slewFilter_s { float state; float slewLimit; @@ -52,6 +65,8 @@ typedef struct laggedMovingAverage_s { typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, + FILTER_PT2, + FILTER_PT3, } lowpassFilterType_e; typedef enum { @@ -81,5 +96,15 @@ void pt1FilterInit(pt1Filter_t *filter, float k); void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input); +float pt2FilterGain(float f_cut, float dT); +void pt2FilterInit(pt2Filter_t *filter, float k); +void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k); +float pt2FilterApply(pt2Filter_t *filter, float input); + +float pt3FilterGain(float f_cut, float dT); +void pt3FilterInit(pt3Filter_t *filter, float k); +void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k); +float pt3FilterApply(pt3Filter_t *filter, float input); + void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold); float slewFilterApply(slewFilter_t *filter, float input); diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index c466f0314e..07dbee889e 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -82,7 +82,6 @@ enum { }; #ifdef USE_RC_SMOOTHING_FILTER -#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1 #define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training #define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes @@ -91,7 +90,7 @@ enum { #define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining #define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms #define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz -#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled +#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ 100 // Initial value for "auto" when interpolated feedforward is enabled static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; #endif // USE_RC_SMOOTHING_FILTER @@ -395,16 +394,12 @@ uint16_t getCurrentRxRefreshRate(void) #ifdef USE_RC_SMOOTHING_FILTER // Determine a cutoff frequency based on filter type and the calculated // average rx frame time -FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1, uint8_t autoSmoothnessFactor) +FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor) { if (avgRxFrameTimeUs > 0) { - const float cutoffFactor = (100 - autoSmoothnessFactor) / 100.0f; - float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency + const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f)); + float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)); // link frequency cutoff = cutoff * cutoffFactor; - - if (pt1) { - cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics - } return lrintf(cutoff); } else { return 0; @@ -426,31 +421,17 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi uint16_t oldCutoff = smoothingData->inputCutoffFrequency; if (smoothingData->inputCutoffSetting == 0) { - smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor); + smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor); } // initialize or update the input filter if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch - switch (smoothingData->inputFilterType) { - - case RC_SMOOTHING_INPUT_PT1: - if (!smoothingData->filterInitialized) { - pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); - } else { - pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); - } - break; - - case RC_SMOOTHING_INPUT_BIQUAD: - default: - if (!smoothingData->filterInitialized) { - biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); - } else { - biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); - } - break; + if (!smoothingData->filterInitialized) { + pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT)); + } else { + pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT)); } } } @@ -458,15 +439,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi // update or initialize the derivative filter oldCutoff = smoothingData->derivativeCutoffFrequency; - if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) - && (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF) - && (rcSmoothingData.derivativeCutoffSetting == 0)) { - - smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor); + if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) { + smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor); } if (!smoothingData->filterInitialized) { - pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis, smoothingData->derivativeFilterType); + pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis); } else if (smoothingData->derivativeCutoffFrequency != oldCutoff) { pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency); } @@ -505,14 +483,7 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) { // if the input cutoff is 0 (auto) then we need to calculate cutoffs - if (rcSmoothingData.inputCutoffSetting == 0) { - return true; - } - - // if the derivative type isn't OFF, and the cutoff is 0, and interpolated feedforward is not enabled then we need to calculate - if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) - && (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF) - && (rcSmoothingData.derivativeCutoffSetting == 0)) { + if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) { return true; } return false; @@ -533,39 +504,19 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void) rcSmoothingData.averageFrameTimeUs = 0; rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor; rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis; - rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type; rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff; - - rcSmoothingData.derivativeFilterTypeSetting = rxConfig()->rc_smoothing_derivative_type; - if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_AUTO) { - // for derivative filter type "AUTO" set to BIQUAD for classic FF and PT1 for interpolated FF - if (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF) { - rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_BIQUAD; - } else { - rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_PT1; - } - } else { - rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type; - } - rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff; rcSmoothingResetAccumulation(&rcSmoothingData); rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting; - if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) { - if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) { - // calculate the fixed derivative cutoff used for interpolated feedforward - const float cutoffFactor = (100 - rcSmoothingData.autoSmoothnessFactor) / 100.0f; - float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ * cutoffFactor; // PT1 cutoff frequency - if (rcSmoothingData.derivativeFilterType == RC_SMOOTHING_DERIVATIVE_BIQUAD) { - // convert to an equivalent BIQUAD cutoff - derivativeCutoff = sqrt(derivativeCutoff * RC_SMOOTHING_IDENTITY_FREQUENCY); - } - rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff); - } else { - rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting; - } + if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) { + // calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known + const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f)); + float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency + rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff); + } else { + rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting; } calculateCutoffs = rcSmoothingAutoCalculate(); @@ -658,16 +609,7 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void) for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value if (rcSmoothingData.filterInitialized) { - switch (rcSmoothingData.inputFilterType) { - case RC_SMOOTHING_INPUT_PT1: - rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); - break; - - case RC_SMOOTHING_INPUT_BIQUAD: - default: - rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); - break; - } + rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); } 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.h b/src/main/fc/rc.h index 4f84fb4659..bc9068c0b6 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -34,7 +34,7 @@ typedef enum { #ifdef USE_RC_SMOOTHING_FILTER #define RC_SMOOTHING_AUTO_FACTOR_MIN 0 -#define RC_SMOOTHING_AUTO_FACTOR_MAX 50 +#define RC_SMOOTHING_AUTO_FACTOR_MAX 250 #endif void processRcCommand(void); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 61bb8847af..bc6aeabc68 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -70,18 +70,6 @@ typedef enum { RC_SMOOTHING_TYPE_FILTER } 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, - RC_SMOOTHING_DERIVATIVE_AUTO, -} rcSmoothingDerivativeFilter_e; - #define ROL_LO (1 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL)) #define ROL_HI (2 << (2 * ROLL)) @@ -116,19 +104,11 @@ typedef struct rcSmoothingFilterTraining_s { uint16_t max; } rcSmoothingFilterTraining_t; -typedef union rcSmoothingFilterTypes_u { - pt1Filter_t pt1Filter; - biquadFilter_t biquadFilter; -} rcSmoothingFilterTypes_t; - typedef struct rcSmoothingFilter_s { bool filterInitialized; - rcSmoothingFilterTypes_t filter[4]; - rcSmoothingInputFilter_e inputFilterType; + pt3Filter_t filter[4]; uint8_t inputCutoffSetting; uint16_t inputCutoffFrequency; - rcSmoothingDerivativeFilter_e derivativeFilterTypeSetting; - rcSmoothingDerivativeFilter_e derivativeFilterType; uint8_t derivativeCutoffSetting; uint16_t derivativeCutoffFrequency; int averageFrameTimeUs; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bbed1c5d75..461bb3a154 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -205,7 +205,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .dyn_idle_max_increase = 150, .ff_interpolate_sp = FF_INTERPOLATE_ON, .ff_max_rate_limit = 100, - .ff_smooth_factor = 37, + .ff_smooth_factor = 0, .ff_boost = 15, .dyn_lpf_curve_expo = 5, .level_race_mode = false, @@ -638,14 +638,7 @@ float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelt DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f)); } if (pidRuntime.setpointDerivativeLpfInitialized) { - switch (pidRuntime.rcSmoothingFilterType) { - case RC_SMOOTHING_DERIVATIVE_PT1: - ret = pt1FilterApply(&pidRuntime.setpointDerivativePt1[axis], pidSetpointDelta); - break; - case RC_SMOOTHING_DERIVATIVE_BIQUAD: - ret = biquadFilterApplyDF1(&pidRuntime.setpointDerivativeBiquad[axis], pidSetpointDelta); - break; - } + ret = pt3FilterApply(&pidRuntime.setpointDerivativePt3[axis], pidSetpointDelta); if (axis == pidRuntime.rcSmoothingDebugAxis) { DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f)); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ae7b669029..786b9b5936 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -340,8 +340,7 @@ typedef struct pidRuntime_s { #endif #ifdef USE_RC_SMOOTHING_FILTER - pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT]; - biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT]; + pt3Filter_t setpointDerivativePt3[XYZ_AXIS_COUNT]; bool setpointDerivativeLpfInitialized; uint8_t rcSmoothingDebugAxis; uint8_t rcSmoothingFilterType; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index e4223278e4..787759315a 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -222,37 +222,22 @@ void pidInit(const pidProfile_t *pidProfile) } #ifdef USE_RC_SMOOTHING_FILTER -void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) +void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis) { pidRuntime.rcSmoothingDebugAxis = debugAxis; - pidRuntime.rcSmoothingFilterType = filterType; - if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + if (filterCutoff > 0) { pidRuntime.setpointDerivativeLpfInitialized = true; for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - switch (pidRuntime.rcSmoothingFilterType) { - case RC_SMOOTHING_DERIVATIVE_PT1: - pt1FilterInit(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT)); - break; - case RC_SMOOTHING_DERIVATIVE_BIQUAD: - biquadFilterInitLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime); - break; - } + pt3FilterInit(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT)); } } } void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff) { - if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + if (filterCutoff > 0) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - switch (pidRuntime.rcSmoothingFilterType) { - case RC_SMOOTHING_DERIVATIVE_PT1: - pt1FilterUpdateCutoff(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT)); - break; - case RC_SMOOTHING_DERIVATIVE_BIQUAD: - biquadFilterUpdateLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime); - break; - } + pt3FilterUpdateCutoff(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT)); } } } @@ -401,7 +386,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) #ifdef USE_INTERPOLATED_SP pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp; - pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f; + if (pidProfile->ff_smooth_factor) { + pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f; + } else { + // set automatically according to boost amount, limit to 0.5 for auto + pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f); + } interpolatedSpInit(pidProfile); #endif diff --git a/src/main/flight/pid_init.h b/src/main/flight/pid_init.h index 9ca6d2690d..946a83fb7b 100644 --- a/src/main/flight/pid_init.h +++ b/src/main/flight/pid_init.h @@ -24,6 +24,6 @@ void pidInit(const pidProfile_t *pidProfile); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidSetItermAccelerator(float newItermAccelerator); -void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType); +void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis); void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff); void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex); diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 43626a94a7..d5283c2b83 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -66,9 +66,7 @@ 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_AUTO, // automatically choose type based on feedforward method - .rc_smoothing_auto_factor = 10, + .rc_smoothing_auto_factor = 30, .srxl2_unit_id = 1, .srxl2_baud_fast = true, .sbus_baud_fast = false,