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

Merge pull request #10832 from daleckystepan/rx_lower_rates

Browse source
This commit is contained in:
Michael Keller 2021-09-10 02:24:52 +12:00 • committed by GitHub
commit 760e08e7cd
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 19 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

29
src/main/fc/rc.c
View file

@ -70,6 +70,7 @@ static bool reverseMotors = false;
static applyRatesFn *applyRates; static applyRatesFn *applyRates;
static uint16_t currentRxRefreshRate; static uint16_t currentRxRefreshRate;
static bool isRxDataNew = false; static bool isRxDataNew = false;
static bool isRxRateValid = false;
static float rcCommandDivider = 500.0f; static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f; static float rcCommandYawDivider = 500.0f;
@ -83,14 +84,15 @@ enum {
}; };
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_CUTOFF_MIN_HZ 15 // Minimum rc smoothing cutoff frequency
#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_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 during initial training #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training
#define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes #define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes
#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_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_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_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 1000 // 1ms #define RC_SMOOTHING_RX_RATE_MIN_US 950 // 0.950ms to fit 1kHz without an issue
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz #define RC_SMOOTHING_RX_RATE_MAX_US 65500 // 65.5ms or 15.26hz
#define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled #define RC_SMOOTHING_FEEDFORWARD_INITIAL_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData; static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
@ -144,6 +146,11 @@ float getRcCommandDelta(int axis)
{ {
return rcCommandDelta[axis]; return rcCommandDelta[axis];
} }
bool getRxRateValid(void)
{
return isRxRateValid;
}
#endif #endif
#define THROTTLE_LOOKUP_LENGTH 12 #define THROTTLE_LOOKUP_LENGTH 12
@ -301,7 +308,8 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
} }
lastRxTimeUs = currentTimeUs; lastRxTimeUs = currentTimeUs;
currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000); isRxRateValid = (refreshRateUs >= RC_SMOOTHING_RX_RATE_MIN_US && refreshRateUs <= RC_SMOOTHING_RX_RATE_MAX_US);
currentRxRefreshRate = constrain(refreshRateUs, RC_SMOOTHING_RX_RATE_MIN_US, RC_SMOOTHING_RX_RATE_MAX_US);
} }
uint16_t getCurrentRxRefreshRate(void) uint16_t getCurrentRxRefreshRate(void)
@ -323,13 +331,6 @@ FAST_CODE_NOINLINE int calcAutoSmoothingCutoff(int avgRxFrameTimeUs, uint8_t aut
} }
} }
// Preforms a reasonableness check on the rx frame time to avoid bad data
// skewing the average.
static 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 // Initialize or update the filters base on either the manually selected cutoff, or
// the auto-calculated cutoff frequency based on detected rx frame rate. // the auto-calculated cutoff frequency based on detected rx frame rate.
FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData) FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
@ -338,10 +339,10 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
uint16_t oldCutoff = smoothingData->setpointCutoffFrequency; uint16_t oldCutoff = smoothingData->setpointCutoffFrequency;
if (smoothingData->setpointCutoffSetting == 0) { if (smoothingData->setpointCutoffSetting == 0) {
smoothingData->setpointCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint); smoothingData->setpointCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
} }
if (smoothingData->throttleCutoffSetting == 0) { if (smoothingData->throttleCutoffSetting == 0) {
smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle); smoothingData->throttleCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle));
} }
// initialize or update the Setpoint filter // initialize or update the Setpoint filter
@ -375,7 +376,7 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
// update or initialize the FF filter // update or initialize the FF filter
oldCutoff = smoothingData->feedforwardCutoffFrequency; oldCutoff = smoothingData->feedforwardCutoffFrequency;
if (rcSmoothingData.ffCutoffSetting == 0) { if (rcSmoothingData.ffCutoffSetting == 0) {
smoothingData->feedforwardCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint); smoothingData->feedforwardCutoffFrequency = MAX(RC_SMOOTHING_CUTOFF_MIN_HZ, calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorSetpoint));
} }
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis); pidInitFeedforwardLpf(smoothingData->feedforwardCutoffFrequency, smoothingData->debugAxis);
@ -473,7 +474,7 @@ static FAST_CODE void processRcSmoothingFilter(void)
// If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate // If the filter cutoffs in auto mode, and we have good rx data, then determine the average rx frame rate
// and use that to calculate the filter cutoff frequencies // and use that to calculate the filter cutoff frequencies
if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
if (rxIsReceivingSignal() && rcSmoothingRxRateValid(currentRxRefreshRate)) { if (rxIsReceivingSignal() && isRxRateValid) {
// set the guard time expiration if it's not set // set the guard time expiration if it's not set
if (validRxFrameTimeMs == 0) { if (validRxFrameTimeMs == 0) {

1
src/main/fc/rc.h
View file

@ -47,3 +47,4 @@ float applyCurve(int axis, float deflection);
bool getShouldUpdateFeedforward(); bool getShouldUpdateFeedforward();
void updateRcRefreshRate(timeUs_t currentTimeUs); void updateRcRefreshRate(timeUs_t currentTimeUs);
uint16_t getCurrentRxRefreshRate(void); uint16_t getCurrentRxRefreshRate(void);
bool getRxRateValid(void);

2
src/main/flight/feedforward.c
View file

@ -95,7 +95,7 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
// interpolate setpoint if necessary // interpolate setpoint if necessary
if (rcCommandDelta == 0.0f) { if (rcCommandDelta == 0.0f) {
// duplicate packet data on this axis // duplicate packet data on this axis
if (goodPacketCount[axis] >= 2 && setpointPercent < 0.95f) { if (getRxRateValid() && goodPacketCount[axis] >= 2 && setpointPercent < 0.95f) {
// interpolate if two or more preceding valid packets, or if sticks near max // interpolate if two or more preceding valid packets, or if sticks near max
setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator) * rxInterval * jitterAttenuator; setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator) * rxInterval * jitterAttenuator;
// setpoint interpolation includes previous acceleration and attenuation // setpoint interpolation includes previous acceleration and attenuation

3
src/main/rx/rx.c
View file

@ -120,6 +120,7 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define DELAY_50_HZ (1000000 / 50) #define DELAY_50_HZ (1000000 / 50)
#define DELAY_33_HZ (1000000 / 33) #define DELAY_33_HZ (1000000 / 33)
#define DELAY_15_HZ (1000000 / 15)
#define DELAY_10_HZ (1000000 / 10) #define DELAY_10_HZ (1000000 / 10)
#define DELAY_5_HZ (1000000 / 5) #define DELAY_5_HZ (1000000 / 5)
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent) #define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
@ -695,7 +696,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
} }
rxDataProcessingRequired = false; rxDataProcessingRequired = false;
rxNextUpdateAtUs = currentTimeUs + DELAY_33_HZ; rxNextUpdateAtUs = currentTimeUs + DELAY_15_HZ;
// only proceed when no more samples to skip and suspend period is over // only proceed when no more samples to skip and suspend period is over
if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) { if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {