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

Updates per review

Browse source 
Stylistic fixes.
Rearrange initilization code to only run when isRXDataNew.
Corrected use of FAST_CODE and FAST_CODE_NOINLINE.
This commit is contained in:
Bruce Luckcuck 2018-06-01 09:57:58 -04:00
parent 826609e703
commit 8b42c80790
1 changed files with 15 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -188,7 +188,7 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
} }
} }
FAST_CODE FAST_CODE_NOINLINE uint8_t processRcInterpolation(void) FAST_CODE uint8_t processRcInterpolation(void)
{ {
static float rcCommandInterp[4]; static float rcCommandInterp[4];
static float rcStepSize[4]; static float rcStepSize[4];
@ -244,7 +244,7 @@ FAST_CODE FAST_CODE_NOINLINE uint8_t processRcInterpolation(void)
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
FAST_CODE FAST_CODE_NOINLINE uint8_t processRcSmoothingFilter(void) FAST_CODE uint8_t processRcSmoothingFilter(void)
{ {
uint8_t updatedChannel = 0; uint8_t updatedChannel = 0;
@ -273,15 +273,22 @@ FAST_CODE FAST_CODE_NOINLINE 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 // 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 // and use that to calculate the filter cutoff frequencies
if ((filterCutoffFrequency == 0) || (derivativeCutoffFrequency == 0)) { if (!filterInitialized) {
if (rxIsReceivingSignal()) { if (rxIsReceivingSignal() && (targetPidLooptime > 0)) {
rxFrameTimeSum += currentRxRefreshRate; rxFrameTimeSum += currentRxRefreshRate;
rxFrameCount++; rxFrameCount++;
if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) { if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
const float avgRxFrameRate = rxFrameTimeSum / rxFrameCount / 1000.0f; const float avgRxFrameRate = rxFrameTimeSum / rxFrameCount / 1000;
defaultCutoffFrequency = lrintf(RC_SMOOTHING_FILTER_AUTO_HZ / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS)); defaultCutoffFrequency = lrintf(RC_SMOOTHING_FILTER_AUTO_HZ / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS));
filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultCutoffFrequency : filterCutoffFrequency; filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultCutoffFrequency : filterCutoffFrequency;
derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultCutoffFrequency : derivativeCutoffFrequency; derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultCutoffFrequency : derivativeCutoffFrequency;
const float dT = targetPidLooptime * 1e-6f;
for (int i = 0; i < interpolationChannels; i++) {
pt1FilterInit(&rcCommandFilter[i], pt1FilterGain(filterCutoffFrequency, dT));
}
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis);
filterInitialized = true;
} }
} else { } else {
rxFrameTimeSum = 0; rxFrameTimeSum = 0;
@ -293,16 +300,6 @@ FAST_CODE FAST_CODE_NOINLINE uint8_t processRcSmoothingFilter(void)
DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis])); DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultCutoffFrequency); DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultCutoffFrequency);
// Once we've determined the filter cutoff frequencies then initialize the filters
if (!filterInitialized && (targetPidLooptime > 0) && (filterCutoffFrequency != 0) && (derivativeCutoffFrequency != 0)) {
const float dT = targetPidLooptime * 0.000001f;
for (int i = 0; i < interpolationChannels; i++) {
pt1FilterInit(&rcCommandFilter[i], pt1FilterGain(filterCutoffFrequency, dT));
}
pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis);
filterInitialized = true;
}
for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) { for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
if (filterInitialized) { if (filterInitialized) {
rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilter[updatedChannel], lastRxData[updatedChannel]); rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilter[updatedChannel], lastRxData[updatedChannel]);
@ -316,7 +313,7 @@ FAST_CODE FAST_CODE_NOINLINE uint8_t processRcSmoothingFilter(void)
} }
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
FAST_CODE FAST_CODE_NOINLINE void processRcCommand(void) FAST_CODE void processRcCommand(void)
{ {
uint8_t updatedChannel; uint8_t updatedChannel;