From d663c478ab453ac3da4680718599e082b5c84e32 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 23 Jun 2018 15:01:14 -0400 Subject: [PATCH] Changes from review; use union struct for filters; cleanup/tidy; save about 200 bytes on F3 Changed main storage structure to use union for the filters. Renamed storage variable passed to the sub functions to avoid confusion with the global static. Added whitespace to separate logical blocks and added additional comments to make the code more readable. Restructured derivative filter initialization/update logic. --- src/main/fc/fc_rc.c | 129 ++++++++++++++++++++++++++------------ src/main/fc/rc_controls.h | 8 ++- 2 files changed, 96 insertions(+), 41 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index ad1b7e0b94..2cba9c01d1 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -263,11 +263,14 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER +// Determine a cutoff frequency based on filter type and the calculated +// average rx frame time FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1) { if (avgRxFrameTimeUs > 0) { float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 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 } @@ -277,83 +280,103 @@ FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1) } } +// Preforms a reasonableness check on the rx frame time to avoid bad data +// skewing the average. FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate) { return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US); } -FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *rcSmoothingData) +// 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) { const float dT = targetPidLooptime * 1e-6f; - uint16_t oldCutoff = rcSmoothingData->inputCutoffFrequency; + uint16_t oldCutoff = smoothingData->inputCutoffFrequency; + if (rxConfig()->rc_smoothing_input_cutoff == 0) { - rcSmoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); } - if ((rcSmoothingData->inputCutoffFrequency != oldCutoff) || !rcSmoothingData->filterInitialized) { + + // initialize or update the input filter + if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { - if ((1 << i) & interpolationChannels) { + if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch switch (rxConfig()->rc_smoothing_input_type) { + case RC_SMOOTHING_INPUT_PT1: - if (!rcSmoothingData->filterInitialized) { - pt1FilterInit(&rcSmoothingData->filterPt1[i], pt1FilterGain(rcSmoothingData->inputCutoffFrequency, dT)); + if (!smoothingData->filterInitialized) { + pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); } else { - pt1FilterUpdateCutoff(&rcSmoothingData->filterPt1[i], pt1FilterGain(rcSmoothingData->inputCutoffFrequency, dT)); + pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); } break; + case RC_SMOOTHING_INPUT_BIQUAD: default: - if (!rcSmoothingData->filterInitialized) { - biquadFilterInitLPF(&rcSmoothingData->filterBiquad[i], rcSmoothingData->inputCutoffFrequency, targetPidLooptime); + if (!smoothingData->filterInitialized) { + biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); } else { - biquadFilterUpdateLPF(&rcSmoothingData->filterBiquad[i], rcSmoothingData->inputCutoffFrequency, targetPidLooptime); + biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); } break; } } } } - oldCutoff = rcSmoothingData->derivativeCutoffFrequency; + + // update or initialize the derivative filter + oldCutoff = smoothingData->derivativeCutoffFrequency; if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) { - rcSmoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); } - if ((rcSmoothingData->derivativeCutoffFrequency != oldCutoff) || !rcSmoothingData->filterInitialized) { - if (!rcSmoothingData->filterInitialized) { - pidInitSetpointDerivativeLpf(rcSmoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); - } else { - pidUpdateSetpointDerivativeLpf(rcSmoothingData->derivativeCutoffFrequency); - } + + if (!smoothingData->filterInitialized) { + pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); + } else if (smoothingData->derivativeCutoffFrequency != oldCutoff) { + pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency); } } -FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *rcSmoothingData) +FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData) { - rcSmoothingData->training.sum = 0; - rcSmoothingData->training.count = 0; - rcSmoothingData->training.min = UINT16_MAX; - rcSmoothingData->training.max = 0; + smoothingData->training.sum = 0; + smoothingData->training.count = 0; + smoothingData->training.min = UINT16_MAX; + smoothingData->training.max = 0; } -FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *rcSmoothingData, int rxFrameTimeUs) +// Accumulate the rx frame time samples. Once we've collected enough samples calculate the +// average and return true. +FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs) { - rcSmoothingData->training.sum += rxFrameTimeUs; - rcSmoothingData->training.count++; - rcSmoothingData->training.max = MAX(rcSmoothingData->training.max, rxFrameTimeUs); - rcSmoothingData->training.min = MIN(rcSmoothingData->training.min, rxFrameTimeUs); - if (rcSmoothingData->training.count >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { - rcSmoothingData->training.sum = rcSmoothingData->training.sum - rcSmoothingData->training.min - rcSmoothingData->training.max; // Throw out high and low samples - rcSmoothingData->averageFrameTimeUs = lrintf(rcSmoothingData->training.sum / (rcSmoothingData->training.count - 2)); - rcSmoothingResetAccumulation(rcSmoothingData); + smoothingData->training.sum += rxFrameTimeUs; + smoothingData->training.count++; + smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs); + smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs); + + // if we've collected enough samples then calculate the average and reset the accumulation + if (smoothingData->training.count >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { + smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples + smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2)); + rcSmoothingResetAccumulation(smoothingData); return true; } return false; } +// Determine if we need to caclulate filter cutoffs. If not then we can avoid +// examining the rx frame times completely FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) { bool ret = false; + + // if the input cutoff is 0 (auto) then we need to calculate cutoffs if (rxConfig()->rc_smoothing_input_cutoff == 0) { ret = true; } + + // if the derivative type isn't OFF and the cutoff is 0 then we need to calculate if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { ret = true; @@ -370,16 +393,22 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs; static FAST_RAM_ZERO_INIT bool calculateCutoffs; + // first call initialization if (!initialized) { initialized = true; rcSmoothingData.filterInitialized = false; rcSmoothingData.averageFrameTimeUs = 0; rcSmoothingResetAccumulation(&rcSmoothingData); + rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; + if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; } + calculateCutoffs = rcSmoothingAutoCalculate(); + + // if we don't need to calculate cutoffs dynamically then the filters can be initialized now if (!calculateCutoffs) { rcSmoothingSetFilterCutoffs(&rcSmoothingData); rcSmoothingData.filterInitialized = true; @@ -387,11 +416,15 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) } if (isRXDataNew) { + + // store the new raw channel values for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { if ((1 << i) & interpolationChannels) { lastRxData[i] = rcCommand[i]; } } + + // for dynamically calculated filters we need to examine each rx frame interval if (calculateCutoffs) { const timeMs_t currentTimeMs = millis(); int sampleState = 0; @@ -400,39 +433,55 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) // 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)) { + + // set the guard time expiration if it's not set if (validRxFrameTimeMs == 0) { validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS); } else { sampleState = 1; } + + // if the guard time has expired then process the rx frame time if (currentTimeMs > validRxFrameTimeMs) { sampleState = 2; bool accumulateSample = true; + + // During initial training process all samples. + // During retraining check samples to determine if they vary by more than the limit percentage. if (rcSmoothingData.filterInitialized) { const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100; if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) { + // We received a sample that wasn't more than the limit percent so reset the accumulation + // During retraining we need a contiguous block of samples that are all significantly different than the current average rcSmoothingResetAccumulation(&rcSmoothingData); accumulateSample = false; } } + + // accumlate the sample into the average if (accumulateSample) { if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) { + // the required number of samples were collected so set the filter cutoffs rcSmoothingSetFilterCutoffs(&rcSmoothingData); rcSmoothingData.filterInitialized = true; validRxFrameTimeMs = 0; } } + } } else { + // we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation validRxFrameTimeMs = 0; rcSmoothingResetAccumulation(&rcSmoothingData); } } + + // rx frame rate training blackbox debugging if (debugMode == DEBUG_RC_SMOOTHING_RATE) { - DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate); // log each rx frame interval + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate); // log each rx frame interval DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count); // log the training step count - DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs); - DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); // indicates whether guard time is active } } } @@ -444,16 +493,18 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs); } + // each pid loop continue to apply the last received channel value to the filter for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { - if ((1 << updatedChannel) & interpolationChannels) { + if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value if (rcSmoothingData.filterInitialized) { switch (rxConfig()->rc_smoothing_input_type) { case RC_SMOOTHING_INPUT_PT1: - rcCommand[updatedChannel] = pt1FilterApply(&rcSmoothingData.filterPt1[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; + case RC_SMOOTHING_INPUT_BIQUAD: default: - rcCommand[updatedChannel] = biquadFilterApplyDF1(&rcSmoothingData.filterBiquad[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; } } else { diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index a01bc8c83d..f1e60843f0 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -114,10 +114,14 @@ typedef struct rcSmoothingFilterTraining_s { uint16_t max; } rcSmoothingFilterTraining_t; +typedef union rcSmoothingFilterTypes_u { + pt1Filter_t pt1Filter; + biquadFilter_t biquadFilter; +} rcSmoothingFilterTypes_t; + typedef struct rcSmoothingFilter_s { bool filterInitialized; - biquadFilter_t filterBiquad[4]; - pt1Filter_t filterPt1[4]; + rcSmoothingFilterTypes_t filter[4]; uint16_t inputCutoffFrequency; uint16_t derivativeCutoffFrequency; int averageFrameTimeUs;