Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 13:55:18 +03:00
Code Issues Wiki Activity

Merge pull request #7655 from kmitchel/dyn_notch_binoffset

Browse source 
Automatically adjust bin offset based on dyn notch minimum.
This commit is contained in:
Michael Keller 2019-03-04 14:41:08 +13:00 • committed by GitHub
commit 8fff3cae09
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 5 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -604,7 +604,7 @@ const clivalue_t valueTable[] = {
{ "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) }, { "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) },
{ "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) }, { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) }, { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, { "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) }, { "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },

11
src/main/sensors/gyroanalyse.c
View file

@ -49,8 +49,6 @@
// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide // for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide
// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h // NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// start to compare 3rd bin to 2nd bin, ie start comparing from 77Hz, 100Hz, or 150Hz centres
#define FFT_BIN_OFFSET 2
// smoothing frequency for FFT centre frequency // smoothing frequency for FFT centre frequency
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 #define DYN_NOTCH_SMOOTH_FREQ_HZ 50
// we need 4 steps for each axis // we need 4 steps for each axis
@ -60,7 +58,7 @@
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
static float FAST_RAM_ZERO_INIT fftResolution; static float FAST_RAM_ZERO_INIT fftResolution;
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; static uint8_t FAST_RAM_ZERO_INIT fftStartBin;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz; static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz;
static uint8_t dynamicFilterRange; static uint8_t dynamicFilterRange;
static float FAST_RAM_ZERO_INIT dynNotchQ; static float FAST_RAM_ZERO_INIT dynNotchQ;
@ -85,7 +83,6 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
dynamicFilterRange = gyroConfig()->dyn_notch_range; dynamicFilterRange = gyroConfig()->dyn_notch_range;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW; fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
fftBinOffset = FFT_BIN_OFFSET;
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f; dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f; dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f; dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
@ -101,12 +98,10 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
} }
if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) { if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH; fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
fftBinOffset = 1;
} }
} else { } else {
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) { if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH; fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
fftBinOffset = 1;
} }
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) { else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM; fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
@ -120,6 +115,8 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
fftStartBin = dynNotchMinHz / lrintf(fftResolution);
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
@ -276,7 +273,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
uint8_t binStart = 0; uint8_t binStart = 0;
uint8_t binMax = 0; uint8_t binMax = 0;
//for bins after initial decline, identify start bin and max bin //for bins after initial decline, identify start bin and max bin
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
if (!fftIncreased) { if (!fftIncreased) {
binStart = i; // first up-step bin binStart = i; // first up-step bin