From b2a50819a3212cae6d388cbce883b8c7d4549da6 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 28 Nov 2019 11:34:52 -0500 Subject: [PATCH] Update RC smoothing "auto" settings to consider interpolated feedforward Allows default settings for RC smoothing to work seamlessly regardless of the feedforward type selected ("classic" vs. interpolated). Adds a new "AUTO" setting for the derivative filter type that will select based on whether interpolated feedforward is enabled (use PT1) or not (use BIQUAD). The derivative filter cutoff if set to auto (0) will also use a fixed cutoff calculated from a 100hz base if interpolated feedforward is enabled. Otherwise if classic FF is active then it will default to the previous method of calculating the cutoff based on the RX frame rate. --- src/main/cli/cli.c | 11 +++++---- src/main/cli/settings.c | 2 +- src/main/fc/rc.c | 50 +++++++++++++++++++++++++++++---------- src/main/fc/rc_controls.h | 4 +++- src/main/pg/rx.c | 2 +- 5 files changed, 50 insertions(+), 19 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 8902d83fc3..6aad781266 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 e611f79d96..46bd9227ee 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 3c207dc4d4..bef07e88b2 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 bdaa78b81c..d31b433b86 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 f1c88ecc64..7bf7714276 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,