diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 37b28f180e..e45669f478 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -53,6 +53,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" +#include "fc/fc_rc.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -1325,11 +1326,13 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_RC_SMOOTHING_FILTER BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_input_cutoff", "%d", rxConfig()->rc_smoothing_input_cutoff); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_derivative_cutoff", "%d", rxConfig()->rc_smoothing_derivative_cutoff); BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rxConfig()->rc_smoothing_debug_axis); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_input_type", "%d", rxConfig()->rc_smoothing_input_type); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_derivative_type", "%d", rxConfig()->rc_smoothing_derivative_type); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rxConfig()->rc_smoothing_input_cutoff, + rxConfig()->rc_smoothing_derivative_cutoff); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rxConfig()->rc_smoothing_input_type, + rxConfig()->rc_smoothing_derivative_type); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE), + rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index a9685ed1d4..477373949c 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -69,11 +69,15 @@ 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_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1 +#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 calculatedFrameTimeAverageUs; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis) @@ -259,9 +263,18 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -uint8_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline) +int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) { - return lrintf(filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS)); + if (avgRxFrameTime > 0) { + float cutoff = (1 / avgRxFrameTime) / 2; // calculate the nyquist frequency + cutoff = cutoff * 0.90f; // Use 90% of the calculated nyquist frequency + if (pt1) { + cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics + } + return lrintf(cutoff); + } else { + return 0; + } } FAST_CODE uint8_t processRcSmoothingFilter(void) @@ -275,10 +288,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 +309,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 + calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2)); + const float avgRxFrameTime = (rxFrameTimeSum / (rxFrameCount - 2)) * 1e-6f; + + defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (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(avgRxFrameTime, (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 +352,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, calculatedFrameTimeAverageUs); + } 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 +584,23 @@ void initRcProcessing(void) break; } } + +#ifdef USE_RC_SMOOTHING_FILTER +int 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 calculatedFrameTimeAverageUs; + 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..64567da0ef 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); +int 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 4462f7db1e..7fa72cca95 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -308,18 +308,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; + } } } } @@ -902,7 +905,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