diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 8902d83fc..6aad78126 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4816,16 +4816,19 @@ static void cliRcSmoothing(char *cmdline) cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000); } } - cliPrint("# Input filter type: "); - cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]); + 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)"); } - cliPrint("# Derivative filter type: "); - cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]); + 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)"); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e611f79d9..46bd9227e 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -400,7 +400,7 @@ static const char * const lookupTableRcSmoothingInputType[] = { "PT1", "BIQUAD" }; static const char * const lookupTableRcSmoothingDerivativeType[] = { - "OFF", "PT1", "BIQUAD" + "OFF", "PT1", "BIQUAD", "AUTO" }; #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 3c207dc4d..bef07e88b 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -44,6 +44,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/interpolated_setpoint.h" #include "flight/gps_rescue.h" #include "flight/pid.h" @@ -91,6 +92,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 static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; #endif // USE_RC_SMOOTHING_FILTER @@ -401,7 +403,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi // update or initialize the derivative filter oldCutoff = smoothingData->derivativeCutoffFrequency; - if ((smoothingData->derivativeCutoffSetting == 0) && (smoothingData->derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + 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); } @@ -444,20 +449,18 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing // examining the rx frame times completely FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) { - bool ret = false; - // if the input cutoff is 0 (auto) then we need to calculate cutoffs if (rcSmoothingData.inputCutoffSetting == 0) { - ret = true; + return true; } - // if the derivative type isn't OFF and the cutoff is 0 then we need to calculate - if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) { - if (rcSmoothingData.derivativeCutoffSetting == 0) { - ret = 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)) { + return true; } - return ret; + return false; } static FAST_CODE uint8_t processRcSmoothingFilter(void) @@ -477,14 +480,37 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void) rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis; rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type; rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff; - rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type; + + 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) { - rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting; + 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; + } } calculateCutoffs = rcSmoothingAutoCalculate(); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index bdaa78b81..d31b433b8 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -74,7 +74,8 @@ typedef enum { typedef enum { RC_SMOOTHING_DERIVATIVE_OFF, RC_SMOOTHING_DERIVATIVE_PT1, - RC_SMOOTHING_DERIVATIVE_BIQUAD + RC_SMOOTHING_DERIVATIVE_BIQUAD, + RC_SMOOTHING_DERIVATIVE_AUTO, } rcSmoothingDerivativeFilter_e; #define ROL_LO (1 << (2 * ROLL)) @@ -122,6 +123,7 @@ typedef struct rcSmoothingFilter_s { rcSmoothingInputFilter_e inputFilterType; uint8_t inputCutoffSetting; uint16_t inputCutoffFrequency; + rcSmoothingDerivativeFilter_e derivativeFilterTypeSetting; rcSmoothingDerivativeFilter_e derivativeFilterType; uint8_t derivativeCutoffSetting; uint16_t derivativeCutoffFrequency; diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index f1c88ecc6..7bf771427 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -67,7 +67,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .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_BIQUAD, + .rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_AUTO, // automatically choose type based on feedforward method .rc_smoothing_auto_factor = 10, .srxl2_unit_id = 1, .srxl2_baud_fast = true,