From 82bc2f9c1cab6873eadef401011bfbdcfda6def2 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Tue, 27 May 2025 11:49:53 -0600 Subject: [PATCH 01/15] Improve rc filter updating and initialization --- src/main/fc/rc.c | 101 +++++++++++++++++++++++--------------- src/main/fc/rc_controls.h | 1 - 2 files changed, 61 insertions(+), 41 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 16216ad695..06eb4e9937 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -341,59 +341,81 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * { // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency; + const uint16_t oldThrottleCutoff = smoothingData->throttleCutoffFrequency; const uint16_t oldFeedforwardCutoff = smoothingData->feedforwardCutoffFrequency; const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz - if (smoothingData->setpointCutoffSetting == 0) { + + const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0; + const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0; + const bool autoFeedforwardSmoothing = smoothingData->throttleCutoffSetting == 0; + const bool autoSetpointAndFeedforward = autoSetpointSmoothing && autoFeedforwardSmoothing; + + if (autoSetpointAndFeedforward) { + // if both are using auto smoothing, both will have the same cutoff smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); + smoothingData->feedforwardCutoffFrequency = smoothingData->setpointCutoffFrequency; + } else { + if (autoSetpointSmoothing) { + smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); + } + + if (autoFeedforwardSmoothing) { + smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); + } } - if (smoothingData->throttleCutoffSetting == 0) { + + if (autoThrottleSmoothing) { smoothingData->throttleCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle)); } - if (smoothingData->feedforwardCutoffSetting == 0) { - smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorFeedforward)); + const float dT = targetPidLooptime * 1e-6f; + const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency; + const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency; + const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; + + if (!smoothingData->filterInitialized) { + for (int i = FD_ROLL; i < FD_YAW; i++) { + pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); + pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); + pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT)); + } + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); } - const float dT = targetPidLooptime * 1e-6f; - if ((smoothingData->setpointCutoffFrequency != oldSetpointCutoff) || !smoothingData->filterInitialized) { - // note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff - // initialize or update the setpoint cutoff based filters - const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency; - for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { - if (i < THROTTLE) { - if (!smoothingData->filterInitialized) { - pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); - } else { - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); - } - } else { - const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; - if (!smoothingData->filterInitialized) { - pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT)); - } else { - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3FilterGain(throttleCutoffFrequency, dT)); - } + // note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff + if (autoSetpointAndFeedforward && (smoothingData->setpointCutoffFrequency != oldSetpointCutoff)) { + // Update the RC Setpoint/Deflection filter and FeedForward Filter + // all cutoffs will be the same, we can optimize :) + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); + const float pt3K = smoothingData->filterSetpoint[0].k; + for (int i = FD_ROLL; i < FD_YAW; i++) { + smoothingData->filterSetpoint[i].k = pt3K; + smoothingData->filterRcDeflection[i].k = pt3K; + smoothingData->filterFeedforward[i].k = pt3K; + } + } else { + if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); + const float pt3K = smoothingData->filterSetpoint[0].k; + for (int i = FD_ROLL; i < FD_YAW; i++) { + smoothingData->filterSetpoint[i].k = pt3K; + smoothingData->filterRcDeflection[i].k = pt3K; } } - // initialize or update the RC Deflection filter - for (int i = FD_ROLL; i < FD_YAW; i++) { - if (!smoothingData->filterInitialized) { - pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); - } else { - pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); + + // Update the Feedforward filter + if (smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) { + pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[0], pt3FilterGain(setpointCutoffFrequency, dT)); + const float pt3K = smoothingData->filterSetpoint[0].k; + for (int i = FD_ROLL; i <= FD_YAW; i++) { + smoothingData->filterFeedforward[i].k = pt3K; } } } - // initialize or update the Feedforward filter - if ((smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) || !smoothingData->filterInitialized) { - for (int i = FD_ROLL; i <= FD_YAW; i++) { - const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency; - if (!smoothingData->filterInitialized) { - pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT)); - } else { - pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT)); - } - } + + // Update the throttle filter + if (smoothingData->throttleCutoffFrequency != oldThrottleCutoff) { + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); } DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency); @@ -426,7 +448,6 @@ static FAST_CODE void processRcSmoothingFilter(void) rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis; rcSmoothingData.autoSmoothnessFactorSetpoint = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f)); - rcSmoothingData.autoSmoothnessFactorFeedforward = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_rpy / 10.0f)); rcSmoothingData.autoSmoothnessFactorThrottle = 1.5f / (1.0f + (rxConfig()->rc_smoothing_auto_factor_throttle / 10.0f)); rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 70a3657d02..2fed4cfd70 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -104,7 +104,6 @@ typedef struct rcSmoothingFilter_s { uint8_t debugAxis; float autoSmoothnessFactorSetpoint; - float autoSmoothnessFactorFeedforward; float autoSmoothnessFactorThrottle; } rcSmoothingFilter_t; From 86fd501d5e174973bbdf2aba7536b59961a27461 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Tue, 27 May 2025 12:07:23 -0600 Subject: [PATCH 02/15] Fix small bugs --- src/main/fc/rc.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 06eb4e9937..503bb95549 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -347,7 +347,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0; const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0; - const bool autoFeedforwardSmoothing = smoothingData->throttleCutoffSetting == 0; + const bool autoFeedforwardSmoothing = smoothingData->feedforwardCutoffSetting == 0; const bool autoSetpointAndFeedforward = autoSetpointSmoothing && autoFeedforwardSmoothing; if (autoSetpointAndFeedforward) { @@ -374,11 +374,13 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; if (!smoothingData->filterInitialized) { - for (int i = FD_ROLL; i < FD_YAW; i++) { + for (int i = FD_ROLL; i <= FD_YAW; i++) { pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); - pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT)); } + for (int i = FD_ROLL; i < FD_YAW; i++) { + pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); + } pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); } @@ -388,7 +390,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * // all cutoffs will be the same, we can optimize :) pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); const float pt3K = smoothingData->filterSetpoint[0].k; - for (int i = FD_ROLL; i < FD_YAW; i++) { + for (int i = FD_ROLL; i <= FD_YAW; i++) { smoothingData->filterSetpoint[i].k = pt3K; smoothingData->filterRcDeflection[i].k = pt3K; smoothingData->filterFeedforward[i].k = pt3K; @@ -397,8 +399,10 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); const float pt3K = smoothingData->filterSetpoint[0].k; - for (int i = FD_ROLL; i < FD_YAW; i++) { + for (int i = FD_ROLL; i <= FD_YAW; i++) { smoothingData->filterSetpoint[i].k = pt3K; + } + for (int i = FD_ROLL; i < FD_YAW; i++) { smoothingData->filterRcDeflection[i].k = pt3K; } } From e8f2e7097a355f8b41bb6d1b6127730f7301cd08 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Tue, 27 May 2025 12:35:52 -0600 Subject: [PATCH 03/15] Improve readability --- src/main/fc/rc.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 503bb95549..2ebc730ba1 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -391,19 +391,21 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); const float pt3K = smoothingData->filterSetpoint[0].k; for (int i = FD_ROLL; i <= FD_YAW; i++) { - smoothingData->filterSetpoint[i].k = pt3K; - smoothingData->filterRcDeflection[i].k = pt3K; - smoothingData->filterFeedforward[i].k = pt3K; + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); + pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); + } + for (int i = FD_ROLL; i < FD_YAW; i++) { + pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } } else { if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); const float pt3K = smoothingData->filterSetpoint[0].k; for (int i = FD_ROLL; i <= FD_YAW; i++) { - smoothingData->filterSetpoint[i].k = pt3K; + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); } for (int i = FD_ROLL; i < FD_YAW; i++) { - smoothingData->filterRcDeflection[i].k = pt3K; + pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } } @@ -412,7 +414,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[0], pt3FilterGain(setpointCutoffFrequency, dT)); const float pt3K = smoothingData->filterSetpoint[0].k; for (int i = FD_ROLL; i <= FD_YAW; i++) { - smoothingData->filterFeedforward[i].k = pt3K; + pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); } } } From cf14059642783b22bfec42f2eef2b0d993bd9f98 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Fri, 30 May 2025 10:22:49 -0600 Subject: [PATCH 04/15] Make feedforward_smooth_factor create the same filter tau regardless of rx packet rate --- src/main/common/filter.c | 9 +++------ src/main/fc/rc.c | 7 ++++--- src/main/flight/pid_init.c | 7 ++++++- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 1fad9d791f..3670a0a254 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -57,8 +57,7 @@ float pt1FilterGainFromDelay(float delay, float dT) return 1.0f; // gain = 1 means no filtering } - const float cutoffHz = 1.0f / (2.0f * M_PIf * delay); - return pt1FilterGain(cutoffHz, dT); + return dT / (dT + delay); } void pt1FilterInit(pt1Filter_t *filter, float k) @@ -93,8 +92,7 @@ float pt2FilterGainFromDelay(float delay, float dT) return 1.0f; // gain = 1 means no filtering } - const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT2); - return pt2FilterGain(cutoffHz, dT); + return dT / (dT + delay * CUTOFF_CORRECTION_PT2); } void pt2FilterInit(pt2Filter_t *filter, float k) @@ -131,8 +129,7 @@ float pt3FilterGainFromDelay(float delay, float dT) return 1.0f; // gain = 1 means no filtering } - const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT3); - return pt3FilterGain(cutoffHz, dT); + return dT / (dT + delay * CUTOFF_CORRECTION_PT3); } void pt3FilterInit(pt3Filter_t *filter, float k) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 2ebc730ba1..f990d807f2 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -569,7 +569,7 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli static float prevSetpoint[3]; // equals raw unless extrapolated forward static float prevSetpointSpeed[3]; // for setpointDelta calculation static float prevSetpointSpeedDelta[3]; // for duplicate extrapolation - static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets + static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets const float rcCommandDelta = rcCommand[axis] - prevRcCommand[axis]; prevRcCommand[axis] = rcCommand[axis]; @@ -648,14 +648,15 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs; // smooth the setpointSpeed value - setpointSpeed = prevSetpointSpeed[axis] + pid->feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]); + const float pt1_k = pt1FilterGainFromDelay(pid->feedforwardSmoothFactor, 1.0f / rxRate); + setpointSpeed = prevSetpointSpeed[axis] + pt1_k * (setpointSpeed - prevSetpointSpeed[axis]); // calculate setpointDelta from smoothed setpoint speed setpointSpeedDelta = setpointSpeed - prevSetpointSpeed[axis]; prevSetpointSpeed[axis] = setpointSpeed; // smooth the setpointDelta element (effectively a second order filter since incoming setpoint was already smoothed) - setpointSpeedDelta = prevSetpointSpeedDelta[axis] + pid->feedforwardSmoothFactor * (setpointSpeedDelta - prevSetpointSpeedDelta[axis]); + setpointSpeedDelta = prevSetpointSpeedDelta[axis] + pt1_k * (setpointSpeedDelta - prevSetpointSpeedDelta[axis]); prevSetpointSpeedDelta[axis] = setpointSpeedDelta; // apply gain factor to delta and adjust for rxRate diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 799ee18605..2ba538c7b4 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -549,7 +549,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidRuntime.feedforwardTransition = pidProfile->feedforward_transition / 100.0f; pidRuntime.feedforwardTransitionInv = (pidProfile->feedforward_transition == 0) ? 0.0f : 100.0f / pidProfile->feedforward_transition; pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging; - pidRuntime.feedforwardSmoothFactor = 1.0f - (0.01f * pidProfile->feedforward_smooth_factor); + // feedforward_smooth_factor effect previously would change based on packet looprate + // normalizing to 250hz packet rate as that is the most commonly used ELRS packet rate + float scaledSmoothFactor = 0.01f * pidProfile->feedforward_smooth_factor; + float rxDt = 1.0f / 250.0f; + float feedforwardSmoothingTau = (rxDt * scaledSmoothFactor) / (1.0f - scaledSmoothFactor); + pidRuntime.feedforwardSmoothFactor = feedforwardSmoothingTau; pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor; pidRuntime.feedforwardJitterFactorInv = 1.0f / (1.0f + pidProfile->feedforward_jitter_factor); pidRuntime.feedforwardBoostFactor = 0.001f * pidProfile->feedforward_boost; From 29fbc0af2c268a28cfb553e3d15cb603a1c89df0 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Mon, 2 Jun 2025 20:33:31 -0600 Subject: [PATCH 05/15] Setpoint and FF always use the same cutoff. --- src/main/blackbox/blackbox.c | 4 +-- src/main/cli/cli.c | 8 +----- src/main/cli/settings.c | 1 - src/main/fc/parameter_names.h | 1 - src/main/fc/rc.c | 48 +++++------------------------------ src/main/fc/rc_controls.h | 2 -- src/main/msp/msp.c | 4 +-- src/main/pg/rx.c | 1 - src/main/pg/rx.h | 1 - 9 files changed, 10 insertions(+), 60 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b4d7f02e7e..253d5df46a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1757,13 +1757,11 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d", rxConfig()->rc_smoothing_auto_factor_rpy); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->feedforwardCutoffSetting); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d", rcSmoothingData->setpointCutoffSetting); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d", rcSmoothingData->throttleCutoffSetting); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d", rcSmoothingData->debugAxis); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency, - rcSmoothingData->setpointCutoffFrequency, + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d", rcSmoothingData->setpointCutoffFrequency, rcSmoothingData->throttleCutoffFrequency); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_smoothed", "%d", lrintf(rcSmoothingData->smoothedRxRateHz)); #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 8fd7d3aee2..6f9ca1cec5 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -5005,18 +5005,12 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline) cliPrintLine("NO SIGNAL"); } } - cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency); + cliPrintf("# Active setpoint and FF cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency); if (rcSmoothingData->setpointCutoffSetting) { cliPrintLine("(manual)"); } else { cliPrintLine("(auto)"); } - cliPrintf("# Active FF cutoff: %dhz ", rcSmoothingData->feedforwardCutoffFrequency); - if (rcSmoothingData->feedforwardCutoffSetting) { - cliPrintLine("(manual)"); - } else { - cliPrintLine("(auto)"); - } cliPrintf("# Active throttle cutoff: %dhz ", rcSmoothingData->throttleCutoffFrequency); if (rcSmoothingData->throttleCutoffSetting) { cliPrintLine("(manual)"); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 91e23d6d29..c9350f391b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -840,7 +840,6 @@ const clivalue_t valueTable[] = { { PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, 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_rpy) }, { PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, 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_throttle) }, { PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_cutoff) }, - { PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_feedforward_cutoff) }, { PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_cutoff) }, { PARAM_NAME_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) }, #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 8d9c812183..6592ac9b4a 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -38,7 +38,6 @@ #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR "rc_smoothing_auto_factor" #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE "rc_smoothing_auto_factor_throttle" #define PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF "rc_smoothing_setpoint_cutoff" -#define PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF "rc_smoothing_feedforward_cutoff" #define PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF "rc_smoothing_throttle_cutoff" #define PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS "rc_smoothing_debug_axis" #define PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS "rc_smoothing_active_cutoffs_ff_sp_thr" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index f990d807f2..9ba346f599 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -342,26 +342,13 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency; const uint16_t oldThrottleCutoff = smoothingData->throttleCutoffFrequency; - const uint16_t oldFeedforwardCutoff = smoothingData->feedforwardCutoffFrequency; const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0; const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0; - const bool autoFeedforwardSmoothing = smoothingData->feedforwardCutoffSetting == 0; - const bool autoSetpointAndFeedforward = autoSetpointSmoothing && autoFeedforwardSmoothing; - if (autoSetpointAndFeedforward) { - // if both are using auto smoothing, both will have the same cutoff + if (autoSetpointSmoothing) { smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); - smoothingData->feedforwardCutoffFrequency = smoothingData->setpointCutoffFrequency; - } else { - if (autoSetpointSmoothing) { - smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); - } - - if (autoFeedforwardSmoothing) { - smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); - } } if (autoThrottleSmoothing) { @@ -370,13 +357,12 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * const float dT = targetPidLooptime * 1e-6f; const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency; - const float feedforwardCutoffFrequency = smoothingData->feedforwardCutoffFrequency; const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; if (!smoothingData->filterInitialized) { for (int i = FD_ROLL; i <= FD_YAW; i++) { pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); - pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT)); + pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(setpointCutoffFrequency, dT)); } for (int i = FD_ROLL; i < FD_YAW; i++) { pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); @@ -385,7 +371,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * } // note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff - if (autoSetpointAndFeedforward && (smoothingData->setpointCutoffFrequency != oldSetpointCutoff)) { + if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { // Update the RC Setpoint/Deflection filter and FeedForward Filter // all cutoffs will be the same, we can optimize :) pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); @@ -397,26 +383,6 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * for (int i = FD_ROLL; i < FD_YAW; i++) { pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } - } else { - if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); - const float pt3K = smoothingData->filterSetpoint[0].k; - for (int i = FD_ROLL; i <= FD_YAW; i++) { - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); - } - for (int i = FD_ROLL; i < FD_YAW; i++) { - pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); - } - } - - // Update the Feedforward filter - if (smoothingData->feedforwardCutoffFrequency != oldFeedforwardCutoff) { - pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[0], pt3FilterGain(setpointCutoffFrequency, dT)); - const float pt3K = smoothingData->filterSetpoint[0].k; - for (int i = FD_ROLL; i <= FD_YAW; i++) { - pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); - } - } } // Update the throttle filter @@ -425,15 +391,15 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * } DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency); - DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency); + DEBUG_SET(DEBUG_RC_SMOOTHING, 2, 0); } -// Determine if we need to caclulate filter cutoffs. If not then we can avoid +// Determine if we need to calculate filter cutoffs. If not then we can avoid // examining the rx frame times completely FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) { // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs - if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.feedforwardCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) { + if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) { return true; } return false; @@ -458,10 +424,8 @@ static FAST_CODE void processRcSmoothingFilter(void) rcSmoothingData.setpointCutoffSetting = rxConfig()->rc_smoothing_setpoint_cutoff; rcSmoothingData.throttleCutoffSetting = rxConfig()->rc_smoothing_throttle_cutoff; - rcSmoothingData.feedforwardCutoffSetting = rxConfig()->rc_smoothing_feedforward_cutoff; rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting; - rcSmoothingData.feedforwardCutoffFrequency = rcSmoothingData.feedforwardCutoffSetting; rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting; if (rxConfig()->rc_smoothing_mode) { diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 2fed4cfd70..e8190102d9 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -93,11 +93,9 @@ typedef struct rcSmoothingFilter_s { uint8_t setpointCutoffSetting; uint8_t throttleCutoffSetting; - uint8_t feedforwardCutoffSetting; uint16_t setpointCutoffFrequency; uint16_t throttleCutoffFrequency; - uint16_t feedforwardCutoffFrequency; float smoothedRxRateHz; uint8_t sampleCount; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index de6d7d2492..cf261669b6 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1598,7 +1598,7 @@ case MSP_NAME: #if defined(USE_RC_SMOOTHING_FILTER) sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_type sbufWriteU8(dst, rxConfig()->rc_smoothing_setpoint_cutoff); - sbufWriteU8(dst, rxConfig()->rc_smoothing_feedforward_cutoff); + sbufWriteU8(dst, 0); // was rxConfig()->rc_smoothing_feedforward_cutoff, now always combined with rc_smoothing_setpoint_cutoff sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_input_type sbufWriteU8(dst, 0); // not required in API 1.44, was rxConfig()->rc_smoothing_derivative_type #else @@ -3840,7 +3840,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #if defined(USE_RC_SMOOTHING_FILTER) sbufReadU8(src); // not required in API 1.44, was rc_smoothing_type configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_setpoint_cutoff, sbufReadU8(src)); - configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_feedforward_cutoff, sbufReadU8(src)); + sbufReadU8(src); // was rc_smoothing_feedforward_cutoff sbufReadU8(src); // not required in API 1.44, was rc_smoothing_input_type sbufReadU8(src); // not required in API 1.44, was rc_smoothing_derivative_type #else diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 824118e4ed..7ddcb682e2 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -104,7 +104,6 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT, .rc_smoothing_mode = 1, .rc_smoothing_setpoint_cutoff = 0, - .rc_smoothing_feedforward_cutoff = 0, .rc_smoothing_throttle_cutoff = 0, .rc_smoothing_debug_axis = ROLL, .rc_smoothing_auto_factor_rpy = 30, diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index 1c57377961..823ac2e69f 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -51,7 +51,6 @@ typedef struct rxConfig_s { int8_t rssi_offset; // offset applied to the RSSI value before it is returned uint8_t rc_smoothing_mode; // Whether filter based rc smoothing is on or off uint8_t rc_smoothing_setpoint_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto) - uint8_t rc_smoothing_feedforward_cutoff; // Filter cutoff frequency for the feedforward filter (0 = auto) uint8_t rc_smoothing_throttle_cutoff; // Filter cutoff frequency for the setpoint filter (0 = auto) uint8_t rc_smoothing_debug_axis; // Axis to log as debug values when debug_mode = RC_SMOOTHING uint8_t rc_smoothing_auto_factor_rpy; // Used to adjust the "smoothness" determined by the auto cutoff calculations From cb12cff8c31c9ab074f2d2285931060af3c5e46d Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Thu, 5 Jun 2025 10:56:44 -0600 Subject: [PATCH 06/15] apply ledvinap suggestions --- src/main/common/filter.c | 6 ++++ src/main/fc/rc.c | 74 +++++++++++++++------------------------ src/main/fc/rc_controls.h | 2 ++ 3 files changed, 36 insertions(+), 46 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 3670a0a254..8bdb300040 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -57,6 +57,8 @@ float pt1FilterGainFromDelay(float delay, float dT) return 1.0f; // gain = 1 means no filtering } + // cutoffHz = 1.0f / (2.0f * M_PIf * delay) + return dT / (dT + delay); } @@ -92,6 +94,8 @@ float pt2FilterGainFromDelay(float delay, float dT) return 1.0f; // gain = 1 means no filtering } + // cutoffHz = 1.0f / (2.0f * M_PIf * delay * CUTOFF_CORRECTION_PT2) + return dT / (dT + delay * CUTOFF_CORRECTION_PT2); } @@ -129,6 +133,8 @@ float pt3FilterGainFromDelay(float delay, float dT) return 1.0f; // gain = 1 means no filtering } + // cutoffHz = 1.0f / (2.0f * M_PIf * delay * CUTOFF_CORRECTION_PT3) + return dT / (dT + delay * CUTOFF_CORRECTION_PT3); } diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 9ba346f599..0cb8243a2b 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -340,58 +340,38 @@ bool getRxRateValid(void) static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) { // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency - const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency; - const uint16_t oldThrottleCutoff = smoothingData->throttleCutoffFrequency; - const uint16_t minCutoffHz = 15; // don't let any RC smoothing filter cutoff go below 15Hz + const float minCutoffHz = 15.0f; // don't let any RC smoothing filter cutoff go below 15Hz const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0; const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0; + float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency; + float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; if (autoSetpointSmoothing) { - smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); + setpointCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint); } if (autoThrottleSmoothing) { - smoothingData->throttleCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle)); + throttleCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle); } const float dT = targetPidLooptime * 1e-6f; - const float setpointCutoffFrequency = smoothingData->setpointCutoffFrequency; - const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; - if (!smoothingData->filterInitialized) { - for (int i = FD_ROLL; i <= FD_YAW; i++) { - pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); - pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(setpointCutoffFrequency, dT)); - } - for (int i = FD_ROLL; i < FD_YAW; i++) { - pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT)); - } - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); + // Update the RC Setpoint/Deflection filter and FeedForward Filter + // all cutoffs will be the same, we can optimize :) + const float pt3K = pt3FilterGain(setpointCutoffFrequency, dT); + for (int i = FD_ROLL; i <= FD_YAW; i++) { + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); + pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); + } + for (int i = FD_ROLL; i <= FD_PITCH; i++) { + pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } - // note that cutoff frequencies are integers, filter cutoffs won't re-calculate until there is > 1hz variation from previous cutoff - if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { - // Update the RC Setpoint/Deflection filter and FeedForward Filter - // all cutoffs will be the same, we can optimize :) - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); - const float pt3K = smoothingData->filterSetpoint[0].k; - for (int i = FD_ROLL; i <= FD_YAW; i++) { - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); - pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); - } - for (int i = FD_ROLL; i < FD_YAW; i++) { - pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); - } - } - - // Update the throttle filter - if (smoothingData->throttleCutoffFrequency != oldThrottleCutoff) { - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); - } + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency); - DEBUG_SET(DEBUG_RC_SMOOTHING, 2, 0); + DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->throttleCutoffFrequency); } // Determine if we need to calculate filter cutoffs. If not then we can avoid @@ -531,8 +511,6 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli static float prevRcCommand[3]; // for rcCommandDelta test static float prevRcCommandDeltaAbs[3]; // for duplicate interpolation static float prevSetpoint[3]; // equals raw unless extrapolated forward - static float prevSetpointSpeed[3]; // for setpointDelta calculation - static float prevSetpointSpeedDelta[3]; // for duplicate extrapolation static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets const float rcCommandDelta = rcCommand[axis] - prevRcCommand[axis]; @@ -569,7 +547,9 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli if (fabsf(setpoint) < 0.90f * maxRcRate[axis]) { // this is a single packet duplicate, and we assume that it is of approximately normal duration // hence no multiplication of prevSetpointSpeedDelta by rxInterval / prevRxInterval - setpointSpeed = prevSetpointSpeed[axis] + prevSetpointSpeedDelta[axis]; + float prevSetpointSpeed = rcSmoothingData.filterSetpointSpeed[axis].state; + float prevSetpointDelta = rcSmoothingData.filterSetpointDelta[axis].state; + setpointSpeed = prevSetpointSpeed + prevSetpointDelta; // pretend that there was stick movement also, to hold the same jitter value rcCommandDeltaAbs = prevRcCommandDeltaAbs[axis]; } @@ -579,7 +559,7 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli setpointSpeed = 0.0f; // zero the acceleration by setting previous speed to zero // feedforward will smoothly decay and be attenuated by the jitter reduction value for zero rcCommandDelta - prevSetpointSpeed[axis] = 0.0f; // zero acceleration later on + rcSmoothingData.filterSetpointSpeed[axis].state = 0.0f; // zero acceleration later on } isPrevPacketDuplicate[axis] = isDuplicate; } @@ -612,16 +592,18 @@ static FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, fli prevRcCommandDeltaAbs[axis] = rcCommandDeltaAbs; // smooth the setpointSpeed value - const float pt1_k = pt1FilterGainFromDelay(pid->feedforwardSmoothFactor, 1.0f / rxRate); - setpointSpeed = prevSetpointSpeed[axis] + pt1_k * (setpointSpeed - prevSetpointSpeed[axis]); + const float pt1K = pt1FilterGainFromDelay(pid->feedforwardSmoothFactor, 1.0f / rxRate); + pt1FilterUpdateCutoff(&rcSmoothingData.filterSetpointSpeed[axis], pt1K); + // grab the previous output from the filter + float prevSetpointSpeed = rcSmoothingData.filterSetpointSpeed[axis].state; + setpointSpeed = pt1FilterApply(&rcSmoothingData.filterSetpointSpeed[axis], setpointSpeed); // calculate setpointDelta from smoothed setpoint speed - setpointSpeedDelta = setpointSpeed - prevSetpointSpeed[axis]; - prevSetpointSpeed[axis] = setpointSpeed; + setpointSpeedDelta = setpointSpeed - prevSetpointSpeed; // smooth the setpointDelta element (effectively a second order filter since incoming setpoint was already smoothed) - setpointSpeedDelta = prevSetpointSpeedDelta[axis] + pt1_k * (setpointSpeedDelta - prevSetpointSpeedDelta[axis]); - prevSetpointSpeedDelta[axis] = setpointSpeedDelta; + pt1FilterUpdateCutoff(&rcSmoothingData.filterSetpointDelta[axis], pt1K); + setpointSpeedDelta = pt1FilterApply(&rcSmoothingData.filterSetpointDelta[axis], setpointSpeedDelta); // apply gain factor to delta and adjust for rxRate const float feedforwardBoost = setpointSpeedDelta * rxRate * pid->feedforwardBoostFactor; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index e8190102d9..fa58ba635a 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -90,6 +90,8 @@ typedef struct rcSmoothingFilter_s { pt3Filter_t filterSetpoint[4]; pt3Filter_t filterRcDeflection[RP_AXIS_COUNT]; pt3Filter_t filterFeedforward[3]; + pt1Filter_t filterSetpointSpeed[3]; + pt1Filter_t filterSetpointDelta[3]; uint8_t setpointCutoffSetting; uint8_t throttleCutoffSetting; From db8945b007d79f945830b417c3ef466107385a6d Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Thu, 5 Jun 2025 11:03:29 -0600 Subject: [PATCH 07/15] Correctly log setpoint/throttle cutoff --- src/main/fc/rc.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 0cb8243a2b..3321d22dfb 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -349,10 +349,12 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; if (autoSetpointSmoothing) { setpointCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint); + smoothingData->setpointCutoffFrequency = setpointCutoffFrequency; // update for logging } if (autoThrottleSmoothing) { throttleCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle); + smoothingData->throttleCutoffFrequency = throttleCutoffFrequency; // update for logging } const float dT = targetPidLooptime * 1e-6f; From a32c336b229d3a79ad657b46cd753506f466b41a Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Fri, 6 Jun 2025 14:59:41 -0600 Subject: [PATCH 08/15] Use THROTTLE instead of 3 --- src/main/fc/rc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 3321d22dfb..7db5d166af 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -370,7 +370,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } - pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[THROTTLE], pt3FilterGain(throttleCutoffFrequency, dT)); DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency); DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->throttleCutoffFrequency); From 83a469e887a614cf76217d1bbfb835d7a032d2a0 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Wed, 4 Jun 2025 17:20:26 -0600 Subject: [PATCH 09/15] WIP --- src/main/fc/rc.c | 38 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 7db5d166af..fa5b23621c 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -335,6 +335,26 @@ bool getRxRateValid(void) #ifdef USE_RC_SMOOTHING_FILTER +static FAST_CODE_NOINLINE void rcSmoothingSetFilterTau(rcSmoothingFilter_t *smoothingData) +{ + const float cen_tau = smoothingData->setpointTauCenter; + const float end_tau = smoothingData->setpointTauEnd; + const float throttle_tau = smoothingData->throttleTau; + + const float dT = targetPidLooptime * 1e-6f; + + for (int i = FD_ROLL; i <= FD_YAW; i++) { + const float tau = (1.0f - rcDeflectionAbs[i]) * cen_tau + rcDeflectionAbs[i] * end_tau; + const float pt3K = pt3FilterGainFromDelay(tau, dT); + + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); + pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); + if (i < FD_YAW) { + pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); + } + } +} + // Initialize or update the filters base on either the manually selected cutoff, or // the auto-calculated cutoff frequency based on detected rx frame rate. static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) @@ -381,7 +401,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t * FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) { // if any rc smoothing cutoff is 0 (auto) then we need to calculate cutoffs - if ((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) { + if (((rcSmoothingData.setpointCutoffSetting == 0) || (rcSmoothingData.throttleCutoffSetting == 0)) && !rxConfig()->rc_smoothing_use_tau) { return true; } return false; @@ -410,11 +430,23 @@ static FAST_CODE void processRcSmoothingFilter(void) rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting; rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting; + rcSmoothingData.setpointTauCenter = rxConfig()->rc_smoothing_setpoint_tau_center / 10.0f; + rcSmoothingData.setpointTauEnd = rxConfig()->rc_smoothing_setpoint_tau_end / 10.0f; + rcSmoothingData.throttleTau = rxConfig()->rc_smoothing_throttle_tau / 10.0f; + if (rxConfig()->rc_smoothing_mode) { calculateCutoffs = rcSmoothingAutoCalculate(); // if we don't need to calculate cutoffs dynamically then the filters can be initialized now if (!calculateCutoffs) { - rcSmoothingSetFilterCutoffs(&rcSmoothingData); + if (rxConfig()->rc_smoothing_use_tau) { + const float dT = targetPidLooptime * 1e-6f; + + rcSmoothingSetFilterTau(&rcSmoothingData); + const float pt3K = pt3FilterGainFromDelay(rcSmoothingData.throttleTau, dT); + pt3FilterUpdateCutoff(rcSmoothingData->filterSetpoint[3], pt3K); + } else { + rcSmoothingSetFilterCutoffs(&rcSmoothingData); + } rcSmoothingData.filterInitialized = true; } } @@ -465,6 +497,8 @@ static FAST_CODE void processRcSmoothingFilter(void) DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.sampleCount); DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.smoothedRxRateHz); // value used by filters DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // guard time = 1, guard time expired = 2 + } else if (rxConfig()->rc_smoothing_use_tau) { + rcSmoothingSetFilterTau(&rcSmoothingData); } // Get new values to be smoothed for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { From 0485c3e8c0a36a30549755d107009a17da2ce47d Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Wed, 4 Jun 2025 17:20:26 -0600 Subject: [PATCH 10/15] Smoothing based on stick location. Just need to add this to CLI and OSD --- src/main/fc/rc.c | 13 ++++++++----- src/main/fc/rc_controls.h | 3 +++ src/main/pg/rx.c | 4 ++++ src/main/pg/rx.h | 4 ++++ 4 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index fa5b23621c..e7d95cdab1 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -339,7 +339,6 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterTau(rcSmoothingFilter_t *smoo { const float cen_tau = smoothingData->setpointTauCenter; const float end_tau = smoothingData->setpointTauEnd; - const float throttle_tau = smoothingData->throttleTau; const float dT = targetPidLooptime * 1e-6f; @@ -430,9 +429,13 @@ static FAST_CODE void processRcSmoothingFilter(void) rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting; rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting; - rcSmoothingData.setpointTauCenter = rxConfig()->rc_smoothing_setpoint_tau_center / 10.0f; - rcSmoothingData.setpointTauEnd = rxConfig()->rc_smoothing_setpoint_tau_end / 10.0f; - rcSmoothingData.throttleTau = rxConfig()->rc_smoothing_throttle_tau / 10.0f; + const float cen_tau = rxConfig()->rc_smoothing_setpoint_tau_center; + const float end_tau = rxConfig()->rc_smoothing_setpoint_tau_end; + const float throttle_tau = rxConfig()->rc_smoothing_throttle_tau; + // unit is 10th of a millisecond, sort of + rcSmoothingData.setpointTauCenter = cen_tau / 10000.0f + cen_tau * cen_tau / 1250000.0f; + rcSmoothingData.setpointTauEnd = end_tau / 10000.0f + end_tau * end_tau / 1250000.0f; + rcSmoothingData.throttleTau = throttle_tau / 10000.0f + throttle_tau * throttle_tau / 1250000.0f; if (rxConfig()->rc_smoothing_mode) { calculateCutoffs = rcSmoothingAutoCalculate(); @@ -443,7 +446,7 @@ static FAST_CODE void processRcSmoothingFilter(void) rcSmoothingSetFilterTau(&rcSmoothingData); const float pt3K = pt3FilterGainFromDelay(rcSmoothingData.throttleTau, dT); - pt3FilterUpdateCutoff(rcSmoothingData->filterSetpoint[3], pt3K); + pt3FilterUpdateCutoff(&rcSmoothingData.filterSetpoint[3], pt3K); } else { rcSmoothingSetFilterCutoffs(&rcSmoothingData); } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index fa58ba635a..b8258ca10c 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -105,6 +105,9 @@ typedef struct rcSmoothingFilter_s { float autoSmoothnessFactorSetpoint; float autoSmoothnessFactorThrottle; + float setpointTauCenter; + float setpointTauEnd; + float throttleTau; } rcSmoothingFilter_t; typedef struct rcControlsConfig_s { diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 7ddcb682e2..281bb891e2 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -113,6 +113,10 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .sbus_baud_fast = false, .msp_override_channels_mask = 0, .crsf_use_negotiated_baud = false, + .rc_smoothing_use_tau = false, + .rc_smoothing_setpoint_tau_center = 50, + .rc_smoothing_setpoint_tau_end = 15, + .rc_smoothing_throttle_tau = 20, ); #ifdef RX_CHANNELS_TAER diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index 823ac2e69f..f51bf7e574 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -63,6 +63,10 @@ typedef struct rxConfig_s { uint32_t msp_override_channels_mask; // Channels to override when the MSP override mode is enabled uint8_t msp_override_failsafe; // if false then extra RC link is always required in msp_override mode, true - allows control via msp_override without extra RC link (autonomous use case) uint8_t crsf_use_negotiated_baud; // Use negotiated baud rate for CRSF V3 + uint8_t rc_smoothing_use_tau; // if set to 1 it will use the tau values for filtering + uint8_t rc_smoothing_setpoint_tau_center; // center stick tau for rc smoothing + uint8_t rc_smoothing_setpoint_tau_end; // end stick tau for rc smoothing + uint8_t rc_smoothing_throttle_tau; // throttle tau for rc smoothing } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); From adfca5d98cd7245c360f394f2172ac9ec5973037 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Thu, 5 Jun 2025 17:01:00 -0600 Subject: [PATCH 11/15] Add to cli --- src/main/cli/settings.c | 4 ++++ src/main/fc/parameter_names.h | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index c9350f391b..4cc15fe442 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -842,6 +842,10 @@ const clivalue_t valueTable[] = { { PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_cutoff) }, { PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_cutoff) }, { PARAM_NAME_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) }, + { PARAM_NAME_RC_SMOOTHING_USE_TAU, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_use_tau) }, + { PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_CENTER,VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_tau_center) }, + { PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_END, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_tau_end) }, + { PARAM_NAME_RC_SMOOTHING_THROTTLE_TAU, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_tau) }, #endif // USE_RC_SMOOTHING_FILTER { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 6592ac9b4a..cf92b0af6f 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -40,6 +40,10 @@ #define PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF "rc_smoothing_setpoint_cutoff" #define PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF "rc_smoothing_throttle_cutoff" #define PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS "rc_smoothing_debug_axis" +#define PARAM_NAME_RC_SMOOTHING_USE_TAU "rc_smoothing_use_tau" +#define PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_CENTER "rc_smoothing_setpoint_tau_center" +#define PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_END "rc_smoothing_setpoint_tau_end" +#define PARAM_NAME_RC_SMOOTHING_THROTTLE_TAU "rc_smoothing_throttle_tau" #define PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS "rc_smoothing_active_cutoffs_ff_sp_thr" #define PARAM_NAME_SERIAL_RX_PROVIDER "serialrx_provider" #define PARAM_NAME_MOTOR_IDLE "motor_idle" From a01db9f398c31fec59c8f7fa6f95f94a7873e79a Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Fri, 6 Jun 2025 11:22:52 -0600 Subject: [PATCH 12/15] Make the tau values change based on the control rate profile, only use the tau values if the rc smoothing use tau is set to true (doesn't get modified with rate profile changes) --- src/main/cli/settings.c | 6 +++--- src/main/fc/controlrate_profile.c | 3 +++ src/main/fc/controlrate_profile.h | 3 +++ src/main/fc/rc.c | 36 ++++++++++++++++--------------- src/main/fc/rc_controls.h | 1 - src/main/pg/rx.c | 3 --- src/main/pg/rx.h | 3 --- 7 files changed, 28 insertions(+), 27 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 4cc15fe442..f6171946c9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -843,9 +843,6 @@ const clivalue_t valueTable[] = { { PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_cutoff) }, { PARAM_NAME_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) }, { PARAM_NAME_RC_SMOOTHING_USE_TAU, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_use_tau) }, - { PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_CENTER,VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_tau_center) }, - { PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_END, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_setpoint_tau_end) }, - { PARAM_NAME_RC_SMOOTHING_THROTTLE_TAU, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_throttle_tau) }, #endif // USE_RC_SMOOTHING_FILTER { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, @@ -1086,6 +1083,9 @@ const clivalue_t valueTable[] = { { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, { "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) }, { "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) }, + { PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_CENTER,VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rc_smoothing_setpoint_tau_center) }, + { PARAM_NAME_RC_SMOOTHING_SETPOINT_TAU_END, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rc_smoothing_setpoint_tau_end) }, + { PARAM_NAME_RC_SMOOTHING_THROTTLE_TAU, VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rc_smoothing_throttle_tau) }, // PG_SERIAL_CONFIG { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 6fb219fbb5..0715e07f69 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -63,6 +63,9 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .profileName = { 0 }, .quickRatesRcExpo = 0, .thrHover8 = 50, + .rc_smoothing_setpoint_tau_center = 50, + .rc_smoothing_setpoint_tau_end = 15, + .rc_smoothing_throttle_tau = 20, ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 61607f4b83..1c2d91477f 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -61,6 +61,9 @@ typedef struct controlRateConfig_s { char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates uint8_t thrHover8; + uint8_t rc_smoothing_setpoint_tau_center; // center stick tau for rc smoothing + uint8_t rc_smoothing_setpoint_tau_end; // end stick tau for rc smoothing + uint8_t rc_smoothing_throttle_tau; // throttle tau for rc smoothing } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index e7d95cdab1..ee0dcd8b38 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -335,7 +335,7 @@ bool getRxRateValid(void) #ifdef USE_RC_SMOOTHING_FILTER -static FAST_CODE_NOINLINE void rcSmoothingSetFilterTau(rcSmoothingFilter_t *smoothingData) +static FAST_CODE_NOINLINE void rcSmoothingUpdateFilterTau(rcSmoothingFilter_t *smoothingData) { const float cen_tau = smoothingData->setpointTauCenter; const float end_tau = smoothingData->setpointTauEnd; @@ -429,25 +429,11 @@ static FAST_CODE void processRcSmoothingFilter(void) rcSmoothingData.setpointCutoffFrequency = rcSmoothingData.setpointCutoffSetting; rcSmoothingData.throttleCutoffFrequency = rcSmoothingData.throttleCutoffSetting; - const float cen_tau = rxConfig()->rc_smoothing_setpoint_tau_center; - const float end_tau = rxConfig()->rc_smoothing_setpoint_tau_end; - const float throttle_tau = rxConfig()->rc_smoothing_throttle_tau; - // unit is 10th of a millisecond, sort of - rcSmoothingData.setpointTauCenter = cen_tau / 10000.0f + cen_tau * cen_tau / 1250000.0f; - rcSmoothingData.setpointTauEnd = end_tau / 10000.0f + end_tau * end_tau / 1250000.0f; - rcSmoothingData.throttleTau = throttle_tau / 10000.0f + throttle_tau * throttle_tau / 1250000.0f; - if (rxConfig()->rc_smoothing_mode) { calculateCutoffs = rcSmoothingAutoCalculate(); // if we don't need to calculate cutoffs dynamically then the filters can be initialized now if (!calculateCutoffs) { - if (rxConfig()->rc_smoothing_use_tau) { - const float dT = targetPidLooptime * 1e-6f; - - rcSmoothingSetFilterTau(&rcSmoothingData); - const float pt3K = pt3FilterGainFromDelay(rcSmoothingData.throttleTau, dT); - pt3FilterUpdateCutoff(&rcSmoothingData.filterSetpoint[3], pt3K); - } else { + if (!rxConfig()->rc_smoothing_use_tau) { rcSmoothingSetFilterCutoffs(&rcSmoothingData); } rcSmoothingData.filterInitialized = true; @@ -501,7 +487,7 @@ static FAST_CODE void processRcSmoothingFilter(void) DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.smoothedRxRateHz); // value used by filters DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // guard time = 1, guard time expired = 2 } else if (rxConfig()->rc_smoothing_use_tau) { - rcSmoothingSetFilterTau(&rcSmoothingData); + rcSmoothingUpdateFilterTau(&rcSmoothingData); } // Get new values to be smoothed for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { @@ -980,6 +966,22 @@ void initRcProcessing(void) const int maxYawRate = (int)maxRcRate[FD_YAW]; initYawSpinRecovery(maxYawRate); #endif + + if (rxConfig()->rc_smoothing_use_tau) { + const float cen_tau = currentControlRateProfile->rc_smoothing_setpoint_tau_center; + const float end_tau = currentControlRateProfile->rc_smoothing_setpoint_tau_end; + const float throttle_tau = currentControlRateProfile->rc_smoothing_throttle_tau; + // unit is 10th of a millisecond, sort of + rcSmoothingData.setpointTauCenter = cen_tau / 10000.0f + cen_tau * cen_tau / 1250000.0f; + rcSmoothingData.setpointTauEnd = end_tau / 10000.0f + end_tau * end_tau / 1250000.0f; + const float throttleTau = throttle_tau / 10000.0f + throttle_tau * throttle_tau / 1250000.0f; + + const float dT = targetPidLooptime * 1e-6f; + + rcSmoothingUpdateFilterTau(&rcSmoothingData); + + pt3FilterUpdateCutoff(&rcSmoothingData.filterSetpoint[3], pt3FilterGainFromDelay(throttleTau, dT)); + } } // send rc smoothing details to blackbox diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index b8258ca10c..32b7f48c8c 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -107,7 +107,6 @@ typedef struct rcSmoothingFilter_s { float autoSmoothnessFactorThrottle; float setpointTauCenter; float setpointTauEnd; - float throttleTau; } rcSmoothingFilter_t; typedef struct rcControlsConfig_s { diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 281bb891e2..608cf875ba 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -114,9 +114,6 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .msp_override_channels_mask = 0, .crsf_use_negotiated_baud = false, .rc_smoothing_use_tau = false, - .rc_smoothing_setpoint_tau_center = 50, - .rc_smoothing_setpoint_tau_end = 15, - .rc_smoothing_throttle_tau = 20, ); #ifdef RX_CHANNELS_TAER diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index f51bf7e574..f048e0bd64 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -64,9 +64,6 @@ typedef struct rxConfig_s { uint8_t msp_override_failsafe; // if false then extra RC link is always required in msp_override mode, true - allows control via msp_override without extra RC link (autonomous use case) uint8_t crsf_use_negotiated_baud; // Use negotiated baud rate for CRSF V3 uint8_t rc_smoothing_use_tau; // if set to 1 it will use the tau values for filtering - uint8_t rc_smoothing_setpoint_tau_center; // center stick tau for rc smoothing - uint8_t rc_smoothing_setpoint_tau_end; // end stick tau for rc smoothing - uint8_t rc_smoothing_throttle_tau; // throttle tau for rc smoothing } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); From e7fa8b7c413d3b91252ba903e38d187389671842 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Fri, 6 Jun 2025 11:38:28 -0600 Subject: [PATCH 13/15] add rc smoothing tau to tests to fix them --- src/test/unit/rc_controls_unittest.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index cf16c68aee..60ae70c55a 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -272,6 +272,9 @@ protected: .profileName = "default", .quickRatesRcExpo = 0, .thrHover8 = 0, + .rc_smoothing_setpoint_tau_center = 0, + .rc_smoothing_setpoint_tau_end = 0, + .rc_smoothing_throttle_tau = 0, }; channelRange_t fullRange = { @@ -379,6 +382,9 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp .profileName = "default", .quickRatesRcExpo = 0, .thrHover8 = 0, + .rc_smoothing_setpoint_tau_center = 0, + .rc_smoothing_setpoint_tau_end = 0, + .rc_smoothing_throttle_tau = 0, }; // and From 69a39da2c087fea934c8151f8e4df14a6eacca8d Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Fri, 6 Jun 2025 14:55:22 -0600 Subject: [PATCH 14/15] Apply those good ledvinap suggestions --- src/main/common/maths.h | 4 ++++ src/main/fc/rc.c | 6 +++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index e097d98c9a..7f75953437 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -164,3 +164,7 @@ static inline float constrainf(float amt, float low, float high) else return amt; } + +static inline float lerp(float t, float a, float b) { + return a + t * (b - a); +} diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index ee0dcd8b38..1d2a8e9d40 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -342,13 +342,13 @@ static FAST_CODE_NOINLINE void rcSmoothingUpdateFilterTau(rcSmoothingFilter_t *s const float dT = targetPidLooptime * 1e-6f; - for (int i = FD_ROLL; i <= FD_YAW; i++) { - const float tau = (1.0f - rcDeflectionAbs[i]) * cen_tau + rcDeflectionAbs[i] * end_tau; + for (unsigned int i = FD_ROLL; i <= FD_YAW; i++) { + const float tau = lerp(rcDeflectionAbs[i], cen_tau, end_tau); const float pt3K = pt3FilterGainFromDelay(tau, dT); pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[i], pt3K); pt3FilterUpdateCutoff(&smoothingData->filterFeedforward[i], pt3K); - if (i < FD_YAW) { + if (i < ARRAYLEN(smoothingData->filterRcDeflection)) { pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } } From 5060abbdf51f5588ec6801dc142b9c8edf2cd100 Mon Sep 17 00:00:00 2001 From: Quick-Flash Date: Fri, 6 Jun 2025 15:02:31 -0600 Subject: [PATCH 15/15] Use THROTTLE instead of 3 --- src/main/fc/rc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 1d2a8e9d40..b91d771e36 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -980,7 +980,7 @@ void initRcProcessing(void) rcSmoothingUpdateFilterTau(&rcSmoothingData); - pt3FilterUpdateCutoff(&rcSmoothingData.filterSetpoint[3], pt3FilterGainFromDelay(throttleTau, dT)); + pt3FilterUpdateCutoff(&rcSmoothingData.filterSetpoint[THROTTLE], pt3FilterGainFromDelay(throttleTau, dT)); } }