diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1492282d83..4d0419362d 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 c9162c680c..c6a515afab 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -5004,18 +5004,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 9e6c0ddbf2..09a5383834 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -856,9 +856,9 @@ 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) }, + { 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) }, #endif // USE_RC_SMOOTHING_FILTER { "fpv_mix_degrees", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 90 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) }, @@ -1099,6 +1099,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/common/filter.c b/src/main/common/filter.c index 1fad9d791f..8bdb300040 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -57,8 +57,9 @@ 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); + // cutoffHz = 1.0f / (2.0f * M_PIf * delay) + + return dT / (dT + delay); } void pt1FilterInit(pt1Filter_t *filter, float k) @@ -93,8 +94,9 @@ 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); + // cutoffHz = 1.0f / (2.0f * M_PIf * delay * CUTOFF_CORRECTION_PT2) + + return dT / (dT + delay * CUTOFF_CORRECTION_PT2); } void pt2FilterInit(pt2Filter_t *filter, float k) @@ -131,8 +133,9 @@ 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); + // cutoffHz = 1.0f / (2.0f * M_PIf * delay * CUTOFF_CORRECTION_PT3) + + return dT / (dT + delay * CUTOFF_CORRECTION_PT3); } void pt3FilterInit(pt3Filter_t *filter, float k) 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/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/parameter_names.h b/src/main/fc/parameter_names.h index 9dd99ed845..1037f0a1dc 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -38,9 +38,12 @@ #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_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" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 0e00907c5a..915889e71b 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -335,77 +335,72 @@ bool getRxRateValid(void) #ifdef USE_RC_SMOOTHING_FILTER +static FAST_CODE_NOINLINE void rcSmoothingUpdateFilterTau(rcSmoothingFilter_t *smoothingData) +{ + const float cen_tau = smoothingData->setpointTauCenter; + const float end_tau = smoothingData->setpointTauEnd; + + const float dT = targetPidLooptime * 1e-6f; + + 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 < ARRAYLEN(smoothingData->filterRcDeflection)) { + 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) { // in auto mode, calculate the RC smoothing cutoff from the smoothed Rx link frequency - const uint16_t oldSetpointCutoff = smoothingData->setpointCutoffFrequency; - 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) { - smoothingData->setpointCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint)); - } - if (smoothingData->throttleCutoffSetting == 0) { - smoothingData->throttleCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle)); + 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) { + setpointCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorSetpoint); + smoothingData->setpointCutoffFrequency = setpointCutoffFrequency; // update for logging } - if (smoothingData->feedforwardCutoffSetting == 0) { - smoothingData->feedforwardCutoffFrequency = MAX(minCutoffHz, (uint16_t)(smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorFeedforward)); + if (autoThrottleSmoothing) { + throttleCutoffFrequency = MAX(minCutoffHz, smoothingData->smoothedRxRateHz * smoothingData->autoSmoothnessFactorThrottle); + smoothingData->throttleCutoffFrequency = throttleCutoffFrequency; // update for logging } 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)); - } - } - } - // 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 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); } - // 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)); - } - } + for (int i = FD_ROLL; i <= FD_PITCH; i++) { + pt3FilterUpdateCutoff(&smoothingData->filterRcDeflection[i], pt3K); } + pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[THROTTLE], pt3FilterGain(throttleCutoffFrequency, dT)); + DEBUG_SET(DEBUG_RC_SMOOTHING, 1, smoothingData->setpointCutoffFrequency); - DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->feedforwardCutoffFrequency); + DEBUG_SET(DEBUG_RC_SMOOTHING, 2, smoothingData->throttleCutoffFrequency); } -// 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)) && !rxConfig()->rc_smoothing_use_tau) { return true; } return false; @@ -426,22 +421,21 @@ 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; 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) { 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) { + rcSmoothingSetFilterCutoffs(&rcSmoothingData); + } rcSmoothingData.filterInitialized = true; } } @@ -492,6 +486,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) { + rcSmoothingUpdateFilterTau(&rcSmoothingData); } // Get new values to be smoothed for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { @@ -540,9 +536,7 @@ 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 + static bool isPrevPacketDuplicate[3]; // to identify multiple identical packets const float rcCommandDelta = rcCommand[axis] - prevRcCommand[axis]; prevRcCommand[axis] = rcCommand[axis]; @@ -578,7 +572,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]; } @@ -588,7 +584,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; } @@ -621,15 +617,18 @@ 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 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] + pid->feedforwardSmoothFactor * (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; @@ -967,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[THROTTLE], 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 70a3657d02..32b7f48c8c 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -90,22 +90,23 @@ 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; - uint8_t feedforwardCutoffSetting; uint16_t setpointCutoffFrequency; uint16_t throttleCutoffFrequency; - uint16_t feedforwardCutoffFrequency; float smoothedRxRateHz; uint8_t sampleCount; uint8_t debugAxis; float autoSmoothnessFactorSetpoint; - float autoSmoothnessFactorFeedforward; float autoSmoothnessFactorThrottle; + float setpointTauCenter; + float setpointTauEnd; } rcSmoothingFilter_t; typedef struct rcControlsConfig_s { 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; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 15cab1dec6..22b218a419 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1605,7 +1605,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 @@ -3798,7 +3798,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..608cf875ba 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, @@ -114,6 +113,7 @@ 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, ); #ifdef RX_CHANNELS_TAER diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index 1c57377961..f048e0bd64 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 @@ -64,6 +63,7 @@ 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 } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); 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