diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f6bda3af3e..037ee8d249 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 10c59fe9df..947aeae291 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 43db3ee9b7..83bfc9f0fb 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 e61f1fcd2c..9b729cd5c3 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 c6befb1537..f4de43d17d 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 68f66240e5..d634c8982f 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -68,16 +68,16 @@ 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 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; +static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; #endif // USE_RC_SMOOTHING_FILTER float getSetpointRate(int axis) @@ -263,11 +263,14 @@ FAST_CODE uint8_t processRcInterpolation(void) } #ifdef USE_RC_SMOOTHING_FILTER -int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1) +// 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 (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,107 +280,231 @@ int calcRcSmoothingCutoff(float avgRxFrameTime, 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); +} + +// 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 = smoothingData->inputCutoffFrequency; + + if (rxConfig()->rc_smoothing_input_cutoff == 0) { + smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + } + + // 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) { // only update channels specified by rc_interp_ch + switch (rxConfig()->rc_smoothing_input_type) { + + case RC_SMOOTHING_INPUT_PT1: + if (!smoothingData->filterInitialized) { + pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); + } else { + pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT)); + } + break; + + case RC_SMOOTHING_INPUT_BIQUAD: + default: + if (!smoothingData->filterInitialized) { + biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); + } else { + biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime); + } + break; + } + } + } + } + + // 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)) { + smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + } + + 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 *smoothingData) +{ + smoothingData->training.sum = 0; + smoothingData->training.count = 0; + smoothingData->training.min = UINT16_MAX; + smoothingData->training.max = 0; +} + +// 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) +{ + 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; + } + } + 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; + // first call initialization if (!initialized) { initialized = true; - filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; - derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_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) { + 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; + } } 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]; } } - // 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) { + + // for dynamically calculated filters we need to examine each rx frame interval + 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 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 (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; - } + // 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; + } - 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 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; } } - pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); - filterInitialized = true; + + // 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); } - } else { - rxFrameTimeSum = 0; - rxFrameCount = 0; - validRxFrameTimeMs = 0; - minRxFrameInterval = UINT16_MAX; - maxRxFrameInterval = 0; + } + + // 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, 1, rcSmoothingData.training.count); // log the training step count + 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 } } } - if (filterInitialized && (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, calculatedFrameTimeAverageUs); + 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 (filterInitialized) { + 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(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; + case RC_SMOOTHING_INPUT_BIQUAD: default: - rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]); + rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; } } else { @@ -594,16 +721,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 rcSmoothingData.inputCutoffFrequency; case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: - return derivativeCutoffFrequency; + return rcSmoothingData.derivativeCutoffFrequency; case RC_SMOOTHING_VALUE_AVERAGE_FRAME: - return calculatedFrameTimeAverageUs; + return rcSmoothingData.averageFrameTimeUs; default: return 0; } diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index 64567da0ef..a829ba698b 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 8820de8683..f1e60843f0 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 { @@ -77,9 +78,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; @@ -108,6 +107,27 @@ typedef enum { extern float rcCommand[4]; +typedef struct rcSmoothingFilterTraining_s { + float sum; + int count; + uint16_t min; + uint16_t max; +} rcSmoothingFilterTraining_t; + +typedef union rcSmoothingFilterTypes_u { + pt1Filter_t pt1Filter; + biquadFilter_t biquadFilter; +} rcSmoothingFilterTypes_t; + +typedef struct rcSmoothingFilter_s { + bool filterInitialized; + rcSmoothingFilterTypes_t filter[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. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7c2ab9e077..b42663402a 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 d99e870824..2fcba48893 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 23d45bab89..968b583422 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)");