From 95ae4943df2991c98b3862ce812a6afb47212b22 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 8 Jun 2018 12:16:49 -0400 Subject: [PATCH] RC smoothing - add rx frame training delay to deal with CRSF initialization Handles cases where the flight controller and receiver are powered but the TX is not. When the TX is powered on and CRSF establishes the link, it will initially run at 50hz for about 0.6 seconds before shifting to 150hz. This change avoids that initialization and link establishment period. --- src/main/fc/fc_rc.c | 80 ++++++++++++++++++++++++++++++----------------------- 1 file changed, 45 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 477373949..28380d581 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -70,7 +70,8 @@ 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_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_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before training starts #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency; @@ -290,6 +291,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) static FAST_RAM_ZERO_INIT int rxFrameCount; static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX; static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval; + static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs; if (!initialized) { initialized = true; @@ -306,48 +308,56 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate // and use that to calculate the filter cutoff frequencies if (!filterInitialized) { - 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) { - 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; - - 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 timeMs_t currentTimeMs = millis(); + if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS)) { + if (validRxFrameTimeMs == 0) { + validRxFrameTimeMs = currentTimeMs; + } else if ((currentTimeMs - validRxFrameTimeMs) > 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) { + 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; + + 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_PT1: - pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT)); - break; - case RC_SMOOTHING_INPUT_BIQUAD: - default: - biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); - break; + 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_PT1: + pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT)); + break; + case RC_SMOOTHING_INPUT_BIQUAD: + default: + biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); + break; + } } } + pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); + filterInitialized = true; } - pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); - filterInitialized = true; } } else { rxFrameTimeSum = 0; rxFrameCount = 0; + validRxFrameTimeMs = 0; + minRxFrameInterval = UINT16_MAX; + maxRxFrameInterval = 0; } } } -- 2.11.4.GIT