forked from emuflight/EmuFlight
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
update the Matrix filter, turn it into a dynamic notch filter (emufli…
…ght#500) * update the Matrix filter, turn it into a dynamic notch filter -- This filter is far more precise than the previous filter was. I've currently set this up with 36 bins (so accuracy is (max-min)/36 which for defaults is, 12.5hz, the old filter had a bin size of 16 which would have given an accuracy of 28.125hz, so this is over double as accurate.) This can also track up to 5 peaks and place 5 notch filters. It is also nicer on the cpu than the older version was, so all around really good. Using this filter + abg i was easily able to raise my gyro lpf filters to 300hz with dterm filters at around 150hz. I'm really excited to see what you all think about this filter :), P.S. all credit goes to @KarateBrot for coding this into betaflight, I may want to update this later to have dynamic widths and dynamic on the number of notches it uses, but so far, hey its awesome. Probably going to stay like this for 0.4.0. Only the defaults need testing right now. * fix emugravity * fix potential bug * the approximation is slower and less accurate * fixes * update q correctly * dterm dyn notch * dterm dyn notch can be tuned * Update gyro_filter_impl.h * OSD and BB headers for dterm_dyn_notch_enable & dterm_dyn_notch_q (emuflight#638) * OSD and BB headers for dterm_dyn_notch_enable & dterm_dyn_notch_q * fix OSD toggle * dynamicFilter/dynamicDtermNotch (emuflight#657) * fix Codacy; set dynDtermNotch disabled by default * update the Matrix filter, turn it into a dynamic notch filter * fix Codacy again * make codacy happy Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com>
- Loading branch information
1 parent
9030ccc
commit 75a10b7
Showing
16 changed files
with
561 additions
and
321 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,153 @@ | ||
| /* | ||
| * This file is part of Cleanflight and Betaflight. | ||
| * | ||
| * Cleanflight and Betaflight are free software. You can redistribute | ||
| * this software and/or modify this software under the terms of the | ||
| * GNU General Public License as published by the Free Software | ||
| * Foundation, either version 3 of the License, or (at your option) | ||
| * any later version. | ||
| * | ||
| * Cleanflight and Betaflight are distributed in the hope that they | ||
| * will be useful, but WITHOUT ANY WARRANTY; without even the implied | ||
| * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. | ||
| * See the GNU General Public License for more details. | ||
| * | ||
| * You should have received a copy of the GNU General Public License | ||
| * along with this software. | ||
| * | ||
| * If not, see <http://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| #include <stdbool.h> | ||
| #include <math.h> | ||
|
|
||
| #include "platform.h" | ||
|
|
||
| #include "common/maths.h" | ||
| #include "common/sdft.h" | ||
|
|
||
| #define SDFT_R 0.9999f // damping factor for guaranteed SDFT stability (r < 1.0f) | ||
|
|
||
| static FAST_RAM_ZERO_INIT float rPowerN; // SDFT_R to the power of SDFT_SAMPLE_SIZE | ||
| static FAST_RAM_ZERO_INIT bool isInitialized; | ||
| static FAST_RAM_ZERO_INIT complex_t twiddle[SDFT_BIN_COUNT]; | ||
|
|
||
| static void applySqrt(const sdft_t *sdft, float *data); | ||
|
|
||
|
|
||
| void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches) | ||
| { | ||
| if (!isInitialized) { | ||
| rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE); | ||
| const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE; | ||
| for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { | ||
| float phi = 0.0f; | ||
| phi = c * i; | ||
| twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi)); | ||
| } | ||
| isInitialized = true; | ||
| } | ||
|
|
||
| sdft->idx = 0; | ||
| sdft->startBin = startBin; | ||
| sdft->endBin = endBin; | ||
| sdft->numBatches = numBatches; | ||
| sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1; | ||
|
|
||
| for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) { | ||
| sdft->samples[i] = 0.0f; | ||
| } | ||
|
|
||
| for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { | ||
| sdft->data[i] = 0.0f; | ||
| } | ||
| } | ||
|
|
||
|
|
||
| // Add new sample to frequency spectrum | ||
| FAST_CODE void sdftPush(sdft_t *sdft, const float *sample) | ||
| { | ||
| const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; | ||
|
|
||
| sdft->samples[sdft->idx] = *sample; | ||
| sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; | ||
|
|
||
| for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { | ||
| sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); | ||
| } | ||
| } | ||
|
|
||
|
|
||
| // Add new sample to frequency spectrum in parts | ||
| FAST_CODE void sdftPushBatch(sdft_t* sdft, const float *sample, const uint8_t *batchIdx) | ||
| { | ||
| const uint8_t batchStart = sdft->batchSize * *batchIdx; | ||
| uint8_t batchEnd = batchStart; | ||
|
|
||
| const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; | ||
|
|
||
| if (*batchIdx == sdft->numBatches - 1) { | ||
| sdft->samples[sdft->idx] = *sample; | ||
| sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; | ||
| batchEnd += sdft->endBin - batchStart + 1; | ||
| } else { | ||
| batchEnd += sdft->batchSize; | ||
| } | ||
|
|
||
| for (uint8_t i = batchStart; i < batchEnd; i++) { | ||
| sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); | ||
| } | ||
| } | ||
|
|
||
|
|
||
| // Get squared magnitude of frequency spectrum | ||
| FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output) | ||
| { | ||
| for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { | ||
| float re = crealf(sdft->data[i]); | ||
| float im = cimagf(sdft->data[i]); | ||
| output[i] = re * re + im * im; | ||
| } | ||
| } | ||
|
|
||
|
|
||
| // Get magnitude of frequency spectrum (slower) | ||
| FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output) | ||
| { | ||
| sdftMagSq(sdft, output); | ||
| applySqrt(sdft, output); | ||
| } | ||
|
|
||
|
|
||
| // Get squared magnitude of frequency spectrum with Hann window applied | ||
| // Hann window in frequency domain: X[k] = -0.25 * X[k-1] +0.5 * X[k] -0.25 * X[k+1] | ||
| FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output) | ||
| { | ||
| complex_t val; | ||
|
|
||
| for (uint8_t i = (sdft->startBin + 1); i < sdft->endBin; i++) { | ||
| float re; | ||
| float im; | ||
| val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication | ||
| re = crealf(val); | ||
| im = cimagf(val); | ||
| output[i] = re * re + im * im; | ||
| } | ||
| } | ||
|
|
||
|
|
||
| // Get magnitude of frequency spectrum with Hann window applied (slower) | ||
| FAST_CODE void sdftWindow(const sdft_t *sdft, float *output) | ||
| { | ||
| sdftWinSq(sdft, output); | ||
| applySqrt(sdft, output); | ||
| } | ||
|
|
||
|
|
||
| // Apply fast square root approximation to the whole sdft range | ||
| static FAST_CODE void applySqrt(const sdft_t *sdft, float *data) | ||
| { | ||
| for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { | ||
| data[i] = sqrtf(data[i]); | ||
| } | ||
| } |
Oops, something went wrong.