Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +03:00
Code Issues Wiki Activity

Merge pull request #6067 from etracer65/rc_smoohting_crsf_init

Browse source 
RC smoothing - add rx frame training delay to deal with CRSF initialization
This commit is contained in:
Michael Keller 2018-06-09 21:40:37 +12:00 • committed by GitHub
commit 0210be305a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 43 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/fc/fc_rc.c
View file

@ -70,7 +70,8 @@ enum {
#ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1
#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 5000 // Wait 5 seconds after power to let the PID loop stabilize before starting average frame rate calculation
#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before training starts
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50
static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
@ -290,6 +291,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
static FAST_RAM_ZERO_INIT int rxFrameCount;
static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX;
static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval;
static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs;
if (!initialized) {
initialized = true;
@ -306,7 +308,11 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
// If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate
// and use that to calculate the filter cutoff frequencies
if (!filterInitialized) {
if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (millis() > RC_SMOOTHING_FILTER_TRAINING_DELAY_MS)) {
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);
@ -345,9 +351,13 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
filterInitialized = true;
}
}
} else {
rxFrameTimeSum = 0;
rxFrameCount = 0;
validRxFrameTimeMs = 0;
minRxFrameInterval = UINT16_MAX;
maxRxFrameInterval = 0;
}
}
}