diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index a4ebb3316..45b010bb5 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -70,6 +70,7 @@ static bool reverseMotors = false; static applyRatesFn *applyRates; static uint16_t currentRxRefreshRate; static bool isRxDataNew = false; +static bool isRxRateValid = false; static float rcCommandDivider = 500.0f; static float rcCommandYawDivider = 500.0f; @@ -83,14 +84,15 @@ enum { }; #ifdef USE_RC_SMOOTHING_FILTER +#define RC_SMOOTHING_CUTOFF_MIN_HZ 15 // Minimum rc smoothing cutoff frequency #define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training #define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes #define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before initial training starts #define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000 // Guard time to wait after retraining to prevent retraining again too quickly #define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining -#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms -#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz +#define RC_SMOOTHING_RX_RATE_MIN_US 950 // 0.950ms to fit 1kHz without an issue +#define RC_SMOOTHING_RX_RATE_MAX_US 65500 // 65.5ms or 15.26hz #define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; @@ -144,6 +146,11 @@ float getRcCommandDelta(int axis) { return rcCommandDelta[axis]; } + +bool getRxRateValid(void) +{ + return isRxRateValid; +} #endif #define THROTTLE_LOOKUP_LENGTH 12 @@ -301,7 +308,8 @@ void updateRcRefreshRate(timeUs_t currentTimeUs) refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol } lastRxTimeUs = currentTimeUs; - currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000); + isRxRateValid = (refreshRateUs >= RC_SMOOTHING_RX_RATE_MIN_US && refreshRateUs <= RC_SMOOTHING_RX_RATE_MAX_US); + currentRxRefreshRate = constrain(refreshRateUs, RC_SMOOTHING_RX_RATE_MIN_US, RC_SMOOTHING_RX_RATE_MAX_US); } uint16_t getCurrentRxRefreshRate(void) @@ -323,13 +331,6 @@ FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t aut } } -// Preforms a reasonableness check on the rx frame time to avoid bad data -// skewing the average. -static FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate) -{ - return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US); -} - // Initialize or update the filters base on either the manually selected cutoff, or // the auto-calculated cutoff frequency based on detected rx frame rate. FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) @@ -338,10 +339,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi uint16_t oldCutoff = smoothingData->setpointCutoffFrequency; if (smoothingData->setpointCutoffSetting == 0) { - smoothingData->setpointCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint); + smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint)); } if (smoothingData->throttleCutoffSetting == 0) { - smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle); + smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle)); } // initialize or update the Setpoint filter @@ -375,7 +376,7 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi // update or initialize the FF filter oldCutoff = smoothingData->feedforwardCutoffFrequency; if (rcSmoothingData.ffCutoffSetting == 0) { - smoothingData->feedforwardCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint); + smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint)); } if (!smoothingData->filterInitialized) { pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis); @@ -473,7 +474,7 @@ static FAST_CODE void processRcSmoothingFilter(void) // If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate // and use that to calculate the filter cutoff frequencies if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization - if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) { + if (rxIsReceivingSignal() && isRxRateValid) { // set the guard time expiration if it's not set if (validRxFrameTimeMs == 0) { diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index 4ea3c1a6e..4fe55b9b1 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -47,3 +47,4 @@ float applyCurve(int axis, float deflection); bool getShouldUpdateFeedforward(); void updateRcRefreshRate(timeUs_t currentTimeUs); uint16_t getCurrentRxRefreshRate(void); +bool getRxRateValid(void); diff --git a/src/main/flight/feedforward.c b/src/main/flight/feedforward.c index c638f1254..10c1420ed 100644 --- a/src/main/flight/feedforward.c +++ b/src/main/flight/feedforward.c @@ -95,7 +95,7 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward // interpolate setpoint if necessary if (rcCommandDelta == 0.0f) { // duplicate packet data on this axis - if (goodPacketCount[axis] >= 2 && setpointPercent < 0.95f) { + if (getRxRateValid() && goodPacketCount[axis] >= 2 && setpointPercent < 0.95f) { // interpolate if two or more preceding valid packets, or if sticks near max setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator) * rxInterval * jitterAttenuator; // setpoint interpolation includes previous acceleration and attenuation diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 1ce57dc7e..37eee2445 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -120,6 +120,7 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define DELAY_50_HZ (1000000 / 50) #define DELAY_33_HZ (1000000 / 33) +#define DELAY_15_HZ (1000000 / 15) #define DELAY_10_HZ (1000000 / 10) #define DELAY_5_HZ (1000000 / 5) #define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent) @@ -695,7 +696,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } rxDataProcessingRequired = false; - rxNextUpdateAtUs = currentTimeUs + DELAY_33_HZ; + rxNextUpdateAtUs = currentTimeUs + DELAY_15_HZ; // only proceed when no more samples to skip and suspend period is over if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {