diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index a9685ed1d4..8800b6048f 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -69,11 +69,17 @@ enum { }; #ifdef USE_RC_SMOOTHING_FILTER -#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 3000 // Wait 3 seconds after power to let the PID loop stabilize before starting average frame rate calculation +#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 5000 // Wait 5 seconds after power to let the PID loop stabilize before starting average frame rate calculation #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 #define RC_SMOOTHING_FILTER_INPUT_AUTO_HZ 50.0f // Used to calculate the default cutoff based on rx frame rate. For example, 9ms frame should use 50hz #define RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ 60.0f // Used to calculate the derivative default cutoff based on rx frame rate. For example, 9ms frame should use 60hz #define RC_SMOOTHING_FILTER_AUTO_MS 9.0f // Formula used: RC_SMOOTHING_FILTER_AUTO_HZ / (measured rx frame delay / RC_SMOOTHING_FILTER_AUTO_HZ) + +static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency; +static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency; +static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency; +static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency; +static FAST_RAM_ZERO_INIT uint16_t calculatedFrameAverageMs; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis) @@ -259,9 +265,14 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -uint8_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline) +uint16_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline, bool pt1) { - return lrintf(filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS)); + float cutoff = filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS); + if (pt1) { // Convert to a PT1 cutoff (2pi * cutoff)^2 / 500 / 2pi; + const float twoPi = 2.0f * M_PIf; + cutoff = (sq(cutoff * twoPi) / 500) / twoPi; + } + return lrintf(cutoff); } FAST_CODE uint8_t processRcSmoothingFilter(void) @@ -275,10 +286,8 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) static FAST_RAM_ZERO_INIT bool filterInitialized; static FAST_RAM_ZERO_INIT float rxFrameTimeSum; static FAST_RAM_ZERO_INIT int rxFrameCount; - static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency; - static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency; - static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency; - static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency; + static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX; + static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval; if (!initialized) { initialized = true; @@ -298,24 +307,36 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (millis() > RC_SMOOTHING_FILTER_TRAINING_DELAY_MS)) { rxFrameTimeSum += currentRxRefreshRate; rxFrameCount++; + maxRxFrameInterval = MAX(maxRxFrameInterval, currentRxRefreshRate); + minRxFrameInterval = MIN(minRxFrameInterval, currentRxRefreshRate); + DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rxFrameCount); // log the step count during training + DEBUG_SET(DEBUG_RC_SMOOTHING, 3, currentRxRefreshRate); // log each frame interval during training if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { - const float avgRxFrameRate = rxFrameTimeSum / rxFrameCount / 1000; - defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ); - defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ); + rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples + calculatedFrameAverageMs = lrintf(rxFrameTimeSum / (rxFrameCount - 2)); + const float avgRxFrameRate = (rxFrameTimeSum / (rxFrameCount - 2)) / 1000; + + defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency; - derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency; + + if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { + derivativeCutoffFrequency = 0; + } else { + defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency; + } const float dT = targetPidLooptime * 1e-6f; for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { if ((1 << i) & interpolationChannels) { switch (rxConfig()->rc_smoothing_input_type) { - case RC_SMOOTHING_INPUT_BIQUAD: - biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); - break; case RC_SMOOTHING_INPUT_PT1: - default: pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT)); break; + case RC_SMOOTHING_INPUT_BIQUAD: + default: + biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); + break; } } } @@ -329,20 +350,24 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) } } - DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis])); - DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultInputCutoffFrequency); + if (filterInitialized && (debugMode = DEBUG_RC_SMOOTHING)) { + // after training has completed then log the raw rc channel and the calculated + // average rx frame rate that was used to calculate the automatic filter cutoffs + DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis])); + DEBUG_SET(DEBUG_RC_SMOOTHING, 3, calculatedFrameAverageMs); + } for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { if ((1 << updatedChannel) & interpolationChannels) { if (filterInitialized) { switch (rxConfig()->rc_smoothing_input_type) { - case RC_SMOOTHING_INPUT_BIQUAD: - rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]); - break; case RC_SMOOTHING_INPUT_PT1: - default: rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]); break; + case RC_SMOOTHING_INPUT_BIQUAD: + default: + rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]); + break; } } else { // If filter isn't initialized yet then use the actual unsmoothed rx channel data @@ -557,3 +582,23 @@ void initRcProcessing(void) break; } } + +#ifdef USE_RC_SMOOTHING_FILTER +uint16_t rcSmoothingGetValue(int whichValue) +{ + switch (whichValue) { + case RC_SMOOTHING_VALUE_INPUT_AUTO: + return defaultInputCutoffFrequency; + case RC_SMOOTHING_VALUE_INPUT_ACTIVE: + return filterCutoffFrequency; + case RC_SMOOTHING_VALUE_DERIVATIVE_AUTO: + return defaultDerivativeCutoffFrequency; + case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: + return derivativeCutoffFrequency; + case RC_SMOOTHING_VALUE_AVERAGE_FRAME: + return calculatedFrameAverageMs; + default: + return 0; + } +} +#endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 3455f6824f..3461b5f7af 100644 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -39,3 +39,4 @@ void updateRcCommands(void); void resetYawAxis(void); void initRcProcessing(void); bool isMotorsReversed(void); +uint16_t rcSmoothingGetValue(int whichValue); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index d3417e3845..8820de8683 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -76,6 +76,13 @@ typedef enum { RC_SMOOTHING_DERIVATIVE_BIQUAD } rcSmoothingDerivativeFilter_e; +typedef enum { + RC_SMOOTHING_VALUE_INPUT_AUTO, + RC_SMOOTHING_VALUE_INPUT_ACTIVE, + RC_SMOOTHING_VALUE_DERIVATIVE_AUTO, + RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE, + RC_SMOOTHING_VALUE_AVERAGE_FRAME +} rcSmoothingInfoType_e; #define ROL_LO (1 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL)) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cea1de2709..3b3a8be702 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -306,18 +306,21 @@ void pidInitFilters(const pidProfile_t *pidProfile) } #ifdef USE_RC_SMOOTHING_FILTER -void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) { - setpointDerivativeLpfInitialized = true; +void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) +{ rcSmoothingDebugAxis = debugAxis; rcSmoothingFilterType = filterType; - for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { - switch (rcSmoothingFilterType) { - case RC_SMOOTHING_DERIVATIVE_PT1: - pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); - break; - case RC_SMOOTHING_DERIVATIVE_BIQUAD: - biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime); - break; + if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + setpointDerivativeLpfInitialized = true; + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + switch (rcSmoothingFilterType) { + case RC_SMOOTHING_DERIVATIVE_PT1: + pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); + break; + case RC_SMOOTHING_DERIVATIVE_BIQUAD: + biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime); + break; + } } } } @@ -896,7 +899,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT if (axis == rcSmoothingDebugAxis) { DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f)); } - if ((dynCd != 0) && setpointDerivativeLpfInitialized && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + if ((dynCd != 0) && setpointDerivativeLpfInitialized) { switch (rcSmoothingFilterType) { case RC_SMOOTHING_DERIVATIVE_PT1: pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index edffaf096e..7eb56148c4 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -3654,6 +3654,44 @@ static void cliVersion(char *cmdline) ); } +#ifdef USE_RC_SMOOTHING_FILTER +static void cliRcSmoothing(char *cmdline) +{ + UNUSED(cmdline); + cliPrint("# RC Smoothing Type: "); + if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) { + cliPrintLine("FILTER"); + uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME); + cliPrint("# Detected RX frame rate: "); + if (avgRxFrameMs == 0) { + cliPrintLine("NO SIGNAL"); + } else { + cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000); + } + cliPrintLinef("# Auto input cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_AUTO)); + cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE)); + if (rxConfig()->rc_smoothing_input_cutoff == 0) { + cliPrintLine("(auto)"); + } else { + cliPrintLine("(manual)"); + } + cliPrintLinef("# Auto derivative cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_AUTO)); + cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); + if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { + cliPrintLine("off)"); + } else { + if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { + cliPrintLine("auto)"); + } else { + cliPrintLine("manual)"); + } + } + } else { + cliPrintLine("INTERPOLATION"); + } +} +#endif // USE_RC_SMOOTHING_FILTER + #if defined(USE_RESOURCE_MGMT) #define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x)) @@ -4441,6 +4479,9 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), +#ifdef USE_RC_SMOOTHING_FILTER + CLI_COMMAND_DEF("rc_smoothing_info", "show rc_smoothing operational settings", NULL, cliRcSmoothing), +#endif // USE_RC_SMOOTHING_FILTER #ifdef USE_RESOURCE_MGMT CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource), #endif