From e49c10b573a2e6c68c66624463fbf861410e7032 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Mon, 4 Jun 2018 23:30:41 -0400 Subject: [PATCH 1/3] RC smoothing: improve rx frame rate detection, add rc_smoothing_info cli command Improved the rx frame rate detection/training by delaying calculation to avoid loop time jitter during flight controller initialization. For auto cutoffs calculate a value appropriate for BIQUAD or PT1 depending on the configuration. Added a new rc_smoothing_info cli command to display internal details about its operation. --- src/main/fc/fc_rc.c | 87 +++++++++++++++++++++++++++++---------- src/main/fc/fc_rc.h | 1 + src/main/fc/rc_controls.h | 7 ++++ src/main/flight/pid.c | 25 ++++++----- src/main/interface/cli.c | 41 ++++++++++++++++++ 5 files changed, 129 insertions(+), 32 deletions(-) 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 From dfa6be810abde6ebd5dca54de9b8063b44e14321 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 6 Jun 2018 17:27:36 -0400 Subject: [PATCH 2/3] Updates and cleanup from review, simplify auto cutoff calculation Renamed variables to be more representative of their content. Simplified the auto-cutoff calculation to be 90% of the nyquist frequency of the measured rx frame rate. Simplified the PT1 from BIQUAD calculation. Added active cutoffs to the blackbox log header. Reduce the number of headers by combining like entries into a single line. --- src/main/blackbox/blackbox.c | 11 +++++++---- src/main/fc/fc_rc.c | 35 ++++++++++++++++++----------------- src/main/fc/fc_rc.h | 2 +- 3 files changed, 26 insertions(+), 22 deletions(-) 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 8800b6048f..69187adba8 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -71,15 +71,12 @@ enum { #ifdef USE_RC_SMOOTHING_FILTER #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; +static FAST_RAM_ZERO_INIT uint16_t calculatedFrameTimeAverageUs; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis) @@ -265,14 +262,18 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -uint16_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline, bool pt1) +int calcRcSmoothingCutoff(float avgRxFrameTimeMs, bool pt1) { - 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; + if (avgRxFrameTimeMs > 0) { + float cutoff = (1 / (avgRxFrameTimeMs / 1000)) / 2; // calculate the nyquist frequency + cutoff = cutoff * 0.90f; // Use 90% of the calculated nyquist frequency + if (pt1) { + cutoff = sq(cutoff) / 80; // convert to a cutoff for pt1 that has similar characteristics + } + return lrintf(cutoff); + } else { + return 0; } - return lrintf(cutoff); } FAST_CODE uint8_t processRcSmoothingFilter(void) @@ -313,16 +314,16 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) DEBUG_SET(DEBUG_RC_SMOOTHING, 3, currentRxRefreshRate); // log each frame interval during training if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples - calculatedFrameAverageMs = lrintf(rxFrameTimeSum / (rxFrameCount - 2)); - const float avgRxFrameRate = (rxFrameTimeSum / (rxFrameCount - 2)) / 1000; + calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2)); + const float avgRxFrameTimeMs = (rxFrameTimeSum / (rxFrameCount - 2)) / 1000; - defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTimeMs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency; 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)); + defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTimeMs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency; } @@ -354,7 +355,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) // 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); + DEBUG_SET(DEBUG_RC_SMOOTHING, 3, calculatedFrameTimeAverageUs); } for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { @@ -584,7 +585,7 @@ void initRcProcessing(void) } #ifdef USE_RC_SMOOTHING_FILTER -uint16_t rcSmoothingGetValue(int whichValue) +int rcSmoothingGetValue(int whichValue) { switch (whichValue) { case RC_SMOOTHING_VALUE_INPUT_AUTO: @@ -596,7 +597,7 @@ uint16_t rcSmoothingGetValue(int whichValue) case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: return derivativeCutoffFrequency; case RC_SMOOTHING_VALUE_AVERAGE_FRAME: - return calculatedFrameAverageMs; + return calculatedFrameTimeAverageUs; default: return 0; } diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 3461b5f7af..64567da0ef 100644 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -39,4 +39,4 @@ void updateRcCommands(void); void resetYawAxis(void); void initRcProcessing(void); bool isMotorsReversed(void); -uint16_t rcSmoothingGetValue(int whichValue); +int rcSmoothingGetValue(int whichValue); From c70e999f24ba8298566fbf0e8e484f6a276bfe51 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 6 Jun 2018 18:42:08 -0400 Subject: [PATCH 3/3] More cleanup from review --- src/main/fc/fc_rc.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 69187adba8..477373949c 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -69,6 +69,7 @@ enum { }; #ifdef USE_RC_SMOOTHING_FILTER +#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 @@ -262,13 +263,13 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -int calcRcSmoothingCutoff(float avgRxFrameTimeMs, bool pt1) +int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) { - if (avgRxFrameTimeMs > 0) { - float cutoff = (1 / (avgRxFrameTimeMs / 1000)) / 2; // calculate the nyquist frequency + 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) / 80; // convert to a cutoff for pt1 that has similar characteristics + cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics } return lrintf(cutoff); } else { @@ -315,15 +316,15 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2)); - const float avgRxFrameTimeMs = (rxFrameTimeSum / (rxFrameCount - 2)) / 1000; + const float avgRxFrameTime = (rxFrameTimeSum / (rxFrameCount - 2)) * 1e-6f; - defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTimeMs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency; if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { derivativeCutoffFrequency = 0; } else { - defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTimeMs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency; }