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

Merge pull request #7583 from joelucid/fix_7582

Browse source 
Fixing missing upper notch limit #7582
This commit is contained in:
Michael Keller 2019-02-18 02:10:33 +13:00 • committed by GitHub
commit fe1a06730b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 3 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/sensors/rpm_filter.c
View file

@ -61,6 +61,7 @@ static float pidLooptime;
static rpmNotchFilter_t filters[2]; static rpmNotchFilter_t filters[2];
static rpmNotchFilter_t* gyroFilter; static rpmNotchFilter_t* gyroFilter;
static rpmNotchFilter_t* dtermFilter; static rpmNotchFilter_t* dtermFilter;
static float maxFrequency;
PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3); PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3);
@ -124,6 +125,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics); numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate; const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f); filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
maxFrequency = 0.5f / (gyro.targetLooptime * 1e-6f);
} }
static float applyFilter(rpmNotchFilter_t* filter, int axis, float value) static float applyFilter(rpmNotchFilter_t* filter, int axis, float value)
@ -171,10 +173,7 @@ void rpmFilterUpdate()
for (int i = 0; i < filterUpdatesPerIteration; i++) { for (int i = 0; i < filterUpdatesPerIteration; i++) {
float frequency = (harmonic + 1) * motorFrequency[motor]; float frequency = constrainf((harmonic + 1) * motorFrequency[motor], currentFilter->minHz, maxFrequency);
if (frequency < currentFilter->minHz) {
frequency = currentFilter->minHz;
}
biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic]; biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
/* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */