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

Fix small bugs

Browse source
This commit is contained in:
Quick-Flash 2025-05-27 12:07:23 -06:00
parent 82bc2f9c1c
commit 86fd501d5e
1 changed files with 9 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -347,7 +347,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0; const bool autoSetpointSmoothing = smoothingData->setpointCutoffSetting == 0;
const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0; const bool autoThrottleSmoothing = smoothingData->throttleCutoffSetting == 0;
const bool autoFeedforwardSmoothing = smoothingData->throttleCutoffSetting == 0; const bool autoFeedforwardSmoothing = smoothingData->feedforwardCutoffSetting == 0;
const bool autoSetpointAndFeedforward = autoSetpointSmoothing && autoFeedforwardSmoothing; const bool autoSetpointAndFeedforward = autoSetpointSmoothing && autoFeedforwardSmoothing;
if (autoSetpointAndFeedforward) { if (autoSetpointAndFeedforward) {
@ -374,11 +374,13 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency; const float throttleCutoffFrequency = smoothingData->throttleCutoffFrequency;
if (!smoothingData->filterInitialized) { if (!smoothingData->filterInitialized) {
for (int i = FD_ROLL; i < FD_YAW; i++) { for (int i = FD_ROLL; i <= FD_YAW; i++) {
pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filterSetpoint[i], pt3FilterGain(setpointCutoffFrequency, dT));
pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT)); pt3FilterInit(&smoothingData->filterFeedforward[i], pt3FilterGain(feedforwardCutoffFrequency, dT));
} }
for (int i = FD_ROLL; i < FD_YAW; i++) {
pt3FilterInit(&smoothingData->filterRcDeflection[i], pt3FilterGain(setpointCutoffFrequency, dT));
}
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[3], pt3FilterGain(throttleCutoffFrequency, dT));
} }
@ -388,7 +390,7 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
// all cutoffs will be the same, we can optimize :) // all cutoffs will be the same, we can optimize :)
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT));
const float pt3K = smoothingData->filterSetpoint[0].k; const float pt3K = smoothingData->filterSetpoint[0].k;
for (int i = FD_ROLL; i < FD_YAW; i++) { for (int i = FD_ROLL; i <= FD_YAW; i++) {
smoothingData->filterSetpoint[i].k = pt3K; smoothingData->filterSetpoint[i].k = pt3K;
smoothingData->filterRcDeflection[i].k = pt3K; smoothingData->filterRcDeflection[i].k = pt3K;
smoothingData->filterFeedforward[i].k = pt3K; smoothingData->filterFeedforward[i].k = pt3K;
@ -397,8 +399,10 @@ static FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *
if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) { if (smoothingData->setpointCutoffFrequency != oldSetpointCutoff) {
pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT)); pt3FilterUpdateCutoff(&smoothingData->filterSetpoint[0], pt3FilterGain(setpointCutoffFrequency, dT));
const float pt3K = smoothingData->filterSetpoint[0].k; const float pt3K = smoothingData->filterSetpoint[0].k;
for (int i = FD_ROLL; i < FD_YAW; i++) { for (int i = FD_ROLL; i <= FD_YAW; i++) {
smoothingData->filterSetpoint[i].k = pt3K; smoothingData->filterSetpoint[i].k = pt3K;
}
for (int i = FD_ROLL; i < FD_YAW; i++) {
smoothingData->filterRcDeflection[i].k = pt3K; smoothingData->filterRcDeflection[i].k = pt3K;
} }
} }