From 1982c94780009ec50679864af69d53dae23ecc20 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 14 Jun 2018 16:46:33 -0400 Subject: [PATCH 1/4] RC smoothing retraining update - adds full support for CRSF Adds in flight monitoring of the rx frame rate and adapts the filters if the frame rate changes. Primarily to add support for Crossfire with its ability to switch from 150hz to 50hz (and back) under some circumstances. Will work with any protocol - not CRSF specific. So if future receivers add the ability to switch frame rates dynamically the logic should support them. If the current rx frame rate is more than +-20% from the previously detected rate, then the process will retrain for the next 50 samples as long as the rate continues to be outside the 20% tolerance. Once 50 samples are collected the new frame rate is updated and the filter cutoffs are adjusted. Only filters set with their cutoffs = 0 (auto) will be adjusted. There is a 2 second guard time after a successful update before retraining can start again to prevent rapid switching back and forth. The logic is optimized to not perform any training if the filters are set to manual cutoffs. So there is an opportunity for advanced users to choose specific cutoffs and reduce the PID loop load slightly. However this is not recommended for Crossfire or other protocols that might change their rx frame rate. Updated the output of the `rc_smoothing_info` cli command to match the revised logic. --- src/main/blackbox/blackbox.c | 1 + src/main/build/debug.c | 3 +- src/main/build/debug.h | 1 + src/main/common/filter.c | 10 ++ src/main/common/filter.h | 2 + src/main/fc/fc_rc.c | 241 +++++++++++++++++++++++------------ src/main/fc/fc_rc.h | 1 + src/main/fc/rc_controls.h | 2 - src/main/flight/pid.c | 18 ++- src/main/flight/pid.h | 1 + src/main/interface/cli.c | 18 ++- 11 files changed, 207 insertions(+), 91 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f6bda3af3..037ee8d24 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1333,6 +1333,7 @@ static bool blackboxWriteSysinfo(void) 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)); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME)); #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 10c59fe9d..947aeae29 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -72,5 +72,6 @@ const char * const debugModeNames[DEBUG_COUNT] = { "ITERM_RELAX", "ACRO_TRAINER", "RC_SMOOTHING", - "RX_SIGNAL_LOSS", + "RX_SIGNAL_LOSS", + "RC_SMOOTHING_RATE", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 43db3ee9b..83bfc9f0f 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -91,6 +91,7 @@ typedef enum { DEBUG_ACRO_TRAINER, DEBUG_RC_SMOOTHING, DEBUG_RX_SIGNAL_LOSS, + DEBUG_RC_SMOOTHING_RATE, DEBUG_COUNT } debugType_e; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index e61f1fcd2..9b729cd5c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -56,6 +56,11 @@ void pt1FilterInit(pt1Filter_t *filter, float k) filter->k = k; } +void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) +{ + filter->k = k; +} + FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input) { filter->state = filter->state + filter->k * (input - filter->state); @@ -167,6 +172,11 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint filter->y2 = y2; } +FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) +{ + biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); +} + /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */ FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index c6befb153..f4de43d17 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -67,6 +67,7 @@ float nullFilterApply(filter_t *filter, float input); void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); +void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input); @@ -77,6 +78,7 @@ float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input); float pt1FilterGain(uint16_t f_cut, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); +void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input); void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 68f66240e..b17d3e56b 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -68,16 +68,26 @@ 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_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 +#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1 +#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 +#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 5000 // 5ms or 200hz +#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz + +static FAST_RAM_ZERO_INIT uint16_t rcSmoothingFilterCutoffFrequency; +static FAST_RAM_ZERO_INIT uint16_t rcSmoothingDerivativeCutoffFrequency; +static FAST_RAM_ZERO_INIT int rcSmoothingAvgRxFrameTimeUs; +static FAST_RAM_ZERO_INIT pt1Filter_t rcSmoothingFilterPt1[4]; +static FAST_RAM_ZERO_INIT biquadFilter_t rcSmoothingFilterBiquad[4]; +static FAST_RAM_ZERO_INIT float rcSmoothingFrameTimeSum; +static FAST_RAM_ZERO_INIT int rcSmoothingFrameCount; +static FAST_RAM_ZERO_INIT uint16_t rcSmoothingMinFrameInterval; +static FAST_RAM_ZERO_INIT uint16_t rcSmoothingMaxFrameInterval; +static FAST_RAM_ZERO_INIT bool rcSmoothingFilterInitialized; -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) @@ -263,10 +273,10 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) +FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1) { - if (avgRxFrameTime > 0) { - float cutoff = (1 / avgRxFrameTime) / 2; // calculate the nyquist frequency + 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,25 +287,112 @@ int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) } } +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(void) +{ + const float dT = targetPidLooptime * 1e-6f; + uint16_t oldCutoff = rcSmoothingFilterCutoffFrequency; + if (rxConfig()->rc_smoothing_input_cutoff == 0) { + rcSmoothingFilterCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingAvgRxFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + } + if ((rcSmoothingFilterCutoffFrequency != oldCutoff) || !rcSmoothingFilterInitialized) { + for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { + if ((1 << i) & interpolationChannels) { + switch (rxConfig()->rc_smoothing_input_type) { + case RC_SMOOTHING_INPUT_PT1: + if (!rcSmoothingFilterInitialized) { + pt1FilterInit(&rcSmoothingFilterPt1[i], pt1FilterGain(rcSmoothingFilterCutoffFrequency, dT)); + } else { + pt1FilterUpdateCutoff(&rcSmoothingFilterPt1[i], pt1FilterGain(rcSmoothingFilterCutoffFrequency, dT)); + } + break; + case RC_SMOOTHING_INPUT_BIQUAD: + default: + if (!rcSmoothingFilterInitialized) { + biquadFilterInitLPF(&rcSmoothingFilterBiquad[i], rcSmoothingFilterCutoffFrequency, targetPidLooptime); + } else { + biquadFilterUpdateLPF(&rcSmoothingFilterBiquad[i], rcSmoothingFilterCutoffFrequency, targetPidLooptime); + } + break; + } + } + } + } + oldCutoff = rcSmoothingDerivativeCutoffFrequency; + if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) { + rcSmoothingDerivativeCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingAvgRxFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + } + if ((rcSmoothingDerivativeCutoffFrequency != oldCutoff) || !rcSmoothingFilterInitialized) { + if (!rcSmoothingFilterInitialized) { + pidInitSetpointDerivativeLpf(rcSmoothingDerivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); + } else { + pidUpdateSetpointDerivativeLpf(rcSmoothingDerivativeCutoffFrequency); + } + } +} + +FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(void) +{ + rcSmoothingFrameTimeSum = 0; + rcSmoothingFrameCount = 0; + rcSmoothingMinFrameInterval = UINT16_MAX; + rcSmoothingMaxFrameInterval = 0; +} + +FAST_CODE bool rcSmoothingAccumulateSample(int rxFrameTimeUs) +{ + rcSmoothingFrameTimeSum += rxFrameTimeUs; + rcSmoothingFrameCount++; + rcSmoothingMaxFrameInterval = MAX(rcSmoothingMaxFrameInterval, rxFrameTimeUs); + rcSmoothingMinFrameInterval = MIN(rcSmoothingMinFrameInterval, rxFrameTimeUs); + if (rcSmoothingFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { + rcSmoothingFrameTimeSum = rcSmoothingFrameTimeSum - rcSmoothingMinFrameInterval - rcSmoothingMaxFrameInterval; // Throw out high and low samples + rcSmoothingAvgRxFrameTimeUs = lrintf(rcSmoothingFrameTimeSum / (rcSmoothingFrameCount - 2)); + rcSmoothingResetAccumulation(); + return true; + } + return false; +} + +FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) +{ + bool ret = false; + if (rxConfig()->rc_smoothing_input_cutoff == 0) { + ret = true; + } + if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { + if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { + ret = true; + } + } + return ret; +} + FAST_CODE uint8_t processRcSmoothingFilter(void) { uint8_t updatedChannel = 0; static FAST_RAM_ZERO_INIT float lastRxData[4]; - static FAST_RAM_ZERO_INIT pt1Filter_t rcCommandFilterPt1[4]; - static FAST_RAM_ZERO_INIT biquadFilter_t rcCommandFilterBiquad[4]; static FAST_RAM_ZERO_INIT bool initialized; - static FAST_RAM_ZERO_INIT bool filterInitialized; - static FAST_RAM_ZERO_INIT float rxFrameTimeSum; - 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; + static FAST_RAM_ZERO_INIT bool calculateCutoffs; if (!initialized) { initialized = true; - filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; - derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; + rcSmoothingResetAccumulation(); + rcSmoothingFilterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; + if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { + rcSmoothingDerivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; + } + calculateCutoffs = rcSmoothingAutoCalculate(); + if (!calculateCutoffs) { + rcSmoothingSetFilterCutoffs(); + rcSmoothingFilterInitialized = true; + } } if (isRXDataNew) { @@ -304,80 +401,68 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) lastRxData[i] = rcCommand[i]; } } - // 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 (calculateCutoffs) { 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; + int sampleState = 0; - 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; - } + // 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 ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization + if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) { + if (validRxFrameTimeMs == 0) { + validRxFrameTimeMs = currentTimeMs + (rcSmoothingFilterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS); + } else { + sampleState = 1; + } + if (currentTimeMs > validRxFrameTimeMs) { + sampleState = 2; + bool accumulateSample = true; + if (rcSmoothingFilterInitialized) { + const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingAvgRxFrameTimeUs) / (float)rcSmoothingAvgRxFrameTimeUs) * 100; + if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) { + rcSmoothingResetAccumulation(); + accumulateSample = false; + } + } + if (accumulateSample) { + if (rcSmoothingAccumulateSample(currentRxRefreshRate)) { + rcSmoothingSetFilterCutoffs(); + rcSmoothingFilterInitialized = true; + validRxFrameTimeMs = 0; } } - pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); - filterInitialized = true; } + } else { + validRxFrameTimeMs = 0; + rcSmoothingResetAccumulation(); } - } else { - rxFrameTimeSum = 0; - rxFrameCount = 0; - validRxFrameTimeMs = 0; - minRxFrameInterval = UINT16_MAX; - maxRxFrameInterval = 0; + } + 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, 1, rcSmoothingFrameCount); // log the training step count + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingAvgRxFrameTimeUs); + DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState); } } } - if (filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { + if (rcSmoothingFilterInitialized && (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); + DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingAvgRxFrameTimeUs); } for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { if ((1 << updatedChannel) & interpolationChannels) { - if (filterInitialized) { + if (rcSmoothingFilterInitialized) { switch (rxConfig()->rc_smoothing_input_type) { case RC_SMOOTHING_INPUT_PT1: - rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = pt1FilterApply(&rcSmoothingFilterPt1[updatedChannel], lastRxData[updatedChannel]); break; case RC_SMOOTHING_INPUT_BIQUAD: default: - rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = biquadFilterApplyDF1(&rcSmoothingFilterBiquad[updatedChannel], lastRxData[updatedChannel]); break; } } else { @@ -594,16 +679,12 @@ void initRcProcessing(void) 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; + return rcSmoothingFilterCutoffFrequency; case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: - return derivativeCutoffFrequency; + return rcSmoothingDerivativeCutoffFrequency; case RC_SMOOTHING_VALUE_AVERAGE_FRAME: - return calculatedFrameTimeAverageUs; + return rcSmoothingAvgRxFrameTimeUs; default: return 0; } diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 64567da0e..a829ba698 100644 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -40,3 +40,4 @@ void resetYawAxis(void); void initRcProcessing(void); bool isMotorsReversed(void); int rcSmoothingGetValue(int whichValue); +bool rcSmoothingAutoCalculate(void); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 8820de868..be5039950 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -77,9 +77,7 @@ typedef enum { } 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; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7c2ab9e07..b42663402 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -325,6 +325,22 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint } } } + +void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff) +{ + if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { + switch (rcSmoothingFilterType) { + case RC_SMOOTHING_DERIVATIVE_PT1: + pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT)); + break; + case RC_SMOOTHING_DERIVATIVE_BIQUAD: + biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime); + break; + } + } + } +} #endif // USE_RC_SMOOTHING_FILTER typedef struct pidCoefficient_s { @@ -921,7 +937,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta); break; case RC_SMOOTHING_DERIVATIVE_BIQUAD: - pidSetpointDelta = biquadFilterApply(&setpointDerivativeBiquad[axis], pidSetpointDelta); + pidSetpointDelta = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta); break; } if (axis == rcSmoothingDebugAxis) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d99e87082..2fcba4889 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -183,3 +183,4 @@ bool crashRecoveryModeActive(void); void pidAcroTrainerInit(void); void pidSetAcroTrainerState(bool newState); void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType); +void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index 175e37899..817e96c25 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -3662,20 +3662,24 @@ static void cliRcSmoothing(char *cmdline) 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); + if (rcSmoothingAutoCalculate()) { + 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)); + cliPrint("# Input filter type: "); + cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rxConfig()->rc_smoothing_input_type]); 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)); + cliPrint("# Derivative filter type: "); + cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rxConfig()->rc_smoothing_derivative_type]); cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { cliPrintLine("off)"); From e9b086a8793677200966d33e42ab4dccf072d138 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 23 Jun 2018 10:16:54 -0400 Subject: [PATCH 2/4] Refactor static variables into a structure Cleans up the many static variables into a single structure. Saves ~60 bytes on F3 but far more importantly will allow the support functions to be easily moved to a size optimized file at some later date. --- src/main/fc/fc_rc.c | 121 ++++++++++++++++++-------------------- src/main/fc/rc_controls.h | 18 ++++++ 2 files changed, 74 insertions(+), 65 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index b17d3e56b..ad1b7e0b9 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -77,17 +77,7 @@ enum { #define RC_SMOOTHING_RX_RATE_MIN_US 5000 // 5ms or 200hz #define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz -static FAST_RAM_ZERO_INIT uint16_t rcSmoothingFilterCutoffFrequency; -static FAST_RAM_ZERO_INIT uint16_t rcSmoothingDerivativeCutoffFrequency; -static FAST_RAM_ZERO_INIT int rcSmoothingAvgRxFrameTimeUs; -static FAST_RAM_ZERO_INIT pt1Filter_t rcSmoothingFilterPt1[4]; -static FAST_RAM_ZERO_INIT biquadFilter_t rcSmoothingFilterBiquad[4]; -static FAST_RAM_ZERO_INIT float rcSmoothingFrameTimeSum; -static FAST_RAM_ZERO_INIT int rcSmoothingFrameCount; -static FAST_RAM_ZERO_INIT uint16_t rcSmoothingMinFrameInterval; -static FAST_RAM_ZERO_INIT uint16_t rcSmoothingMaxFrameInterval; -static FAST_RAM_ZERO_INIT bool rcSmoothingFilterInitialized; - +static FAST_RAM rcSmoothingFilter_t rcSmoothingData; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis) @@ -292,67 +282,67 @@ 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(void) +FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *rcSmoothingData) { const float dT = targetPidLooptime * 1e-6f; - uint16_t oldCutoff = rcSmoothingFilterCutoffFrequency; + uint16_t oldCutoff = rcSmoothingData->inputCutoffFrequency; if (rxConfig()->rc_smoothing_input_cutoff == 0) { - rcSmoothingFilterCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingAvgRxFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + rcSmoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); } - if ((rcSmoothingFilterCutoffFrequency != oldCutoff) || !rcSmoothingFilterInitialized) { + if ((rcSmoothingData->inputCutoffFrequency != oldCutoff) || !rcSmoothingData->filterInitialized) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { if ((1 << i) & interpolationChannels) { switch (rxConfig()->rc_smoothing_input_type) { case RC_SMOOTHING_INPUT_PT1: - if (!rcSmoothingFilterInitialized) { - pt1FilterInit(&rcSmoothingFilterPt1[i], pt1FilterGain(rcSmoothingFilterCutoffFrequency, dT)); + if (!rcSmoothingData->filterInitialized) { + pt1FilterInit(&rcSmoothingData->filterPt1[i], pt1FilterGain(rcSmoothingData->inputCutoffFrequency, dT)); } else { - pt1FilterUpdateCutoff(&rcSmoothingFilterPt1[i], pt1FilterGain(rcSmoothingFilterCutoffFrequency, dT)); + pt1FilterUpdateCutoff(&rcSmoothingData->filterPt1[i], pt1FilterGain(rcSmoothingData->inputCutoffFrequency, dT)); } break; case RC_SMOOTHING_INPUT_BIQUAD: default: - if (!rcSmoothingFilterInitialized) { - biquadFilterInitLPF(&rcSmoothingFilterBiquad[i], rcSmoothingFilterCutoffFrequency, targetPidLooptime); + if (!rcSmoothingData->filterInitialized) { + biquadFilterInitLPF(&rcSmoothingData->filterBiquad[i], rcSmoothingData->inputCutoffFrequency, targetPidLooptime); } else { - biquadFilterUpdateLPF(&rcSmoothingFilterBiquad[i], rcSmoothingFilterCutoffFrequency, targetPidLooptime); + biquadFilterUpdateLPF(&rcSmoothingData->filterBiquad[i], rcSmoothingData->inputCutoffFrequency, targetPidLooptime); } break; } } } } - oldCutoff = rcSmoothingDerivativeCutoffFrequency; + oldCutoff = rcSmoothingData->derivativeCutoffFrequency; if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) { - rcSmoothingDerivativeCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingAvgRxFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + rcSmoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(rcSmoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); } - if ((rcSmoothingDerivativeCutoffFrequency != oldCutoff) || !rcSmoothingFilterInitialized) { - if (!rcSmoothingFilterInitialized) { - pidInitSetpointDerivativeLpf(rcSmoothingDerivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); + if ((rcSmoothingData->derivativeCutoffFrequency != oldCutoff) || !rcSmoothingData->filterInitialized) { + if (!rcSmoothingData->filterInitialized) { + pidInitSetpointDerivativeLpf(rcSmoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); } else { - pidUpdateSetpointDerivativeLpf(rcSmoothingDerivativeCutoffFrequency); + pidUpdateSetpointDerivativeLpf(rcSmoothingData->derivativeCutoffFrequency); } } } -FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(void) +FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *rcSmoothingData) { - rcSmoothingFrameTimeSum = 0; - rcSmoothingFrameCount = 0; - rcSmoothingMinFrameInterval = UINT16_MAX; - rcSmoothingMaxFrameInterval = 0; + rcSmoothingData->training.sum = 0; + rcSmoothingData->training.count = 0; + rcSmoothingData->training.min = UINT16_MAX; + rcSmoothingData->training.max = 0; } -FAST_CODE bool rcSmoothingAccumulateSample(int rxFrameTimeUs) +FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *rcSmoothingData, int rxFrameTimeUs) { - rcSmoothingFrameTimeSum += rxFrameTimeUs; - rcSmoothingFrameCount++; - rcSmoothingMaxFrameInterval = MAX(rcSmoothingMaxFrameInterval, rxFrameTimeUs); - rcSmoothingMinFrameInterval = MIN(rcSmoothingMinFrameInterval, rxFrameTimeUs); - if (rcSmoothingFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { - rcSmoothingFrameTimeSum = rcSmoothingFrameTimeSum - rcSmoothingMinFrameInterval - rcSmoothingMaxFrameInterval; // Throw out high and low samples - rcSmoothingAvgRxFrameTimeUs = lrintf(rcSmoothingFrameTimeSum / (rcSmoothingFrameCount - 2)); - rcSmoothingResetAccumulation(); + 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); return true; } return false; @@ -375,7 +365,6 @@ FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) FAST_CODE uint8_t processRcSmoothingFilter(void) { uint8_t updatedChannel = 0; - static FAST_RAM_ZERO_INIT float lastRxData[4]; static FAST_RAM_ZERO_INIT bool initialized; static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs; @@ -383,15 +372,17 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) if (!initialized) { initialized = true; - rcSmoothingResetAccumulation(); - rcSmoothingFilterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; + 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) { - rcSmoothingDerivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; + rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; } calculateCutoffs = rcSmoothingAutoCalculate(); if (!calculateCutoffs) { - rcSmoothingSetFilterCutoffs(); - rcSmoothingFilterInitialized = true; + rcSmoothingSetFilterCutoffs(&rcSmoothingData); + rcSmoothingData.filterInitialized = true; } } @@ -410,59 +401,59 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) { if (validRxFrameTimeMs == 0) { - validRxFrameTimeMs = currentTimeMs + (rcSmoothingFilterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS); + validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS); } else { sampleState = 1; } if (currentTimeMs > validRxFrameTimeMs) { sampleState = 2; bool accumulateSample = true; - if (rcSmoothingFilterInitialized) { - const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingAvgRxFrameTimeUs) / (float)rcSmoothingAvgRxFrameTimeUs) * 100; + if (rcSmoothingData.filterInitialized) { + const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100; if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) { - rcSmoothingResetAccumulation(); + rcSmoothingResetAccumulation(&rcSmoothingData); accumulateSample = false; } } if (accumulateSample) { - if (rcSmoothingAccumulateSample(currentRxRefreshRate)) { - rcSmoothingSetFilterCutoffs(); - rcSmoothingFilterInitialized = true; + if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) { + rcSmoothingSetFilterCutoffs(&rcSmoothingData); + rcSmoothingData.filterInitialized = true; validRxFrameTimeMs = 0; } } } } else { validRxFrameTimeMs = 0; - rcSmoothingResetAccumulation(); + rcSmoothingResetAccumulation(&rcSmoothingData); } } 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, 1, rcSmoothingFrameCount); // log the training step count - DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingAvgRxFrameTimeUs); + 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); } } } - if (rcSmoothingFilterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { + if (rcSmoothingData.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, rcSmoothingAvgRxFrameTimeUs); + DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs); } for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { if ((1 << updatedChannel) & interpolationChannels) { - if (rcSmoothingFilterInitialized) { + if (rcSmoothingData.filterInitialized) { switch (rxConfig()->rc_smoothing_input_type) { case RC_SMOOTHING_INPUT_PT1: - rcCommand[updatedChannel] = pt1FilterApply(&rcSmoothingFilterPt1[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = pt1FilterApply(&rcSmoothingData.filterPt1[updatedChannel], lastRxData[updatedChannel]); break; case RC_SMOOTHING_INPUT_BIQUAD: default: - rcCommand[updatedChannel] = biquadFilterApplyDF1(&rcSmoothingFilterBiquad[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = biquadFilterApplyDF1(&rcSmoothingData.filterBiquad[updatedChannel], lastRxData[updatedChannel]); break; } } else { @@ -680,11 +671,11 @@ int rcSmoothingGetValue(int whichValue) { switch (whichValue) { case RC_SMOOTHING_VALUE_INPUT_ACTIVE: - return rcSmoothingFilterCutoffFrequency; + return rcSmoothingData.inputCutoffFrequency; case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: - return rcSmoothingDerivativeCutoffFrequency; + return rcSmoothingData.derivativeCutoffFrequency; case RC_SMOOTHING_VALUE_AVERAGE_FRAME: - return rcSmoothingAvgRxFrameTimeUs; + return rcSmoothingData.averageFrameTimeUs; default: return 0; } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index be5039950..a01bc8c83 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -22,6 +22,7 @@ #include +#include "common/filter.h" #include "pg/pg.h" typedef enum rc_alias { @@ -106,6 +107,23 @@ typedef enum { extern float rcCommand[4]; +typedef struct rcSmoothingFilterTraining_s { + float sum; + int count; + uint16_t min; + uint16_t max; +} rcSmoothingFilterTraining_t; + +typedef struct rcSmoothingFilter_s { + bool filterInitialized; + biquadFilter_t filterBiquad[4]; + pt1Filter_t filterPt1[4]; + uint16_t inputCutoffFrequency; + uint16_t derivativeCutoffFrequency; + int averageFrameTimeUs; + rcSmoothingFilterTraining_t training; +} rcSmoothingFilter_t; + typedef struct rcControlsConfig_s { uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. From d663c478ab453ac3da4680718599e082b5c84e32 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 23 Jun 2018 15:01:14 -0400 Subject: [PATCH 3/4] 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 ad1b7e0b9..2cba9c01d 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 a01bc8c83..f1e60843f 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; From dbdbc4819a7bb96e072862b3fc9392a5d63bbbe7 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 23 Jun 2018 15:46:58 -0400 Subject: [PATCH 4/4] Fix FAST_RAM_ZERO_INIT for static variable. --- src/main/fc/fc_rc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 2cba9c01d..d634c8982 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -77,7 +77,7 @@ enum { #define RC_SMOOTHING_RX_RATE_MIN_US 5000 // 5ms or 200hz #define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz -static FAST_RAM rcSmoothingFilter_t rcSmoothingData; +static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis)