[go: nahoru, domu]
Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Auto Notch #785

Draft
wants to merge 7 commits into
base: emuflight-1.0.0-master
Choose a base branch
from
Draft

Auto Notch #785

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
5bf5c04
wip
Quick-Flash Feb 24, 2022
f10a2cc
autonotch wip
Quick-Flash Feb 25, 2022
7023d12
add auto notch to the dynamic notch
nerdCopter Nov 22, 2022
3d4d8e3
autonotch on rpm filter
Quick-Flash Feb 25, 2022
cb66e36
add frequency accounter
Quick-Flash Mar 1, 2022
765a5a0
update defaults
Quick-Flash Mar 4, 2022
0ee1886
improve cpu load, remove bandpass
Quick-Flash Mar 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
improve cpu load, remove bandpass
  • Loading branch information
@Quick-Flash @nerdCopter
Quick-Flash authored and nerdCopter committed Nov 22, 2022
commit 0ee188687103b3696a02569db4788fce3b6c91ac
10 changes: 4 additions & 6 deletions src/main/common/auto_notch.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,18 +43,17 @@ void initAutoNotch(autoNotch_t *autoNotch, float initial_frequency, int q, int n

autoNotch->preVariance = 0.0;
pt1FilterInit(&autoNotch->preVarianceFilter, pt1FilterGain(10.0, looptimeUs * 1e-6f));
biquadFilterInit(&autoNotch->preVarianceBandpass, initial_frequency, looptimeUs, q, FILTER_BPF, 1.0f);

biquadFilterInit(&autoNotch->notchFilter, initial_frequency, looptimeUs, q, FILTER_NOTCH, 1.0f);
}

FAST_CODE float applyAutoNotch(autoNotch_t *autoNotch, float input) {
float preNotchNoise = biquadFilterApplyDF1(&autoNotch->preVarianceBandpass, input);
float notchFilteredNoise = biquadFilterApplyDF1(&autoNotch->notchFilter, input);

float preNotchNoise = input - notchFilteredNoise;
// variance is approximately the noise squared and averaged
autoNotch->preVariance = pt1FilterApply(&autoNotch->preVarianceFilter, preNotchNoise * preNotchNoise);

float notchFilteredNoise = biquadFilterApplyDF1(&autoNotch->notchFilter, input);

return autoNotch->weight * notchFilteredNoise + autoNotch->invWeight * input;
}

Expand All @@ -63,15 +62,14 @@ FAST_CODE void updateWeight(autoNotch_t *autoNotch, float frequency, float weigh
arm_sqrt_f32(autoNotch->preVariance, &deviation);
// 1 / 360
// higher freq have less delay when filtering anyhow and make more dterm noise
float frequencyAccounter = 1.0f + frequency * 0.002777777777f;
float frequencyAccounter = 1.0f + frequency * 0.002777777777f; // the 0.0027 is 1/360
float weight = deviation * frequencyAccounter * autoNotch->noiseLimitInv;

autoNotch->weight = MIN(weight * weightMultiplier, 1.0);
autoNotch->invWeight = 1.0 - autoNotch->weight;
}

FAST_CODE void updateAutoNotch(autoNotch_t *autoNotch, float frequency, float q, float weightMultiplier, float looptimeUs) {
biquadFilterInit(&autoNotch->preVarianceBandpass, frequency, looptimeUs, q, FILTER_BPF, 1.0f);
biquadFilterUpdate(&autoNotch->notchFilter, frequency, looptimeUs, q, FILTER_NOTCH, 1.0f);

updateWeight(autoNotch, frequency, weightMultiplier);
Expand Down
1 change: 0 additions & 1 deletion src/main/common/auto_notch.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@
typedef struct autoNotch_s {
float preVariance;
pt1Filter_t preVarianceFilter; // used as an exponential average, setup k to act like exponential average
biquadFilter_t preVarianceBandpass;

float noiseLimitInv; // default of 50 allows 70 amplitude noise to be totally notched
float weight;
Expand Down
5 changes: 0 additions & 5 deletions src/main/flight/rpm_filter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -210,11 +210,6 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
clone->notchFilter.a1 = template->notchFilter.a1;
clone->notchFilter.a2 = template->notchFilter.a2;

clone->preVarianceBandpass.b0 = template->preVarianceBandpass.b0;
//clone->preVarianceBandpass.b1 = template->preVarianceBandpass.b1; // always 0
clone->preVarianceBandpass.b2 = template->preVarianceBandpass.b2;
clone->preVarianceBandpass.b0 = template->preVarianceBandpass.a1;
clone->preVarianceBandpass.b2 = template->preVarianceBandpass.a2;
updateWeight(clone, frequency, weight);
}

Expand Down