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

Automatically adjust bin offset based on dyn notch minimum.

Browse source 
Changes per review.

Drop min HZ to allow for 7".

Put alignment back.

Fix mistakenly delete variable.
This commit is contained in:
Kenneth Mitchell 2019-02-10 19:04:55 -05:00
parent dc6aad22c5
commit 832306743e
No known key found for this signature in database
GPG key ID: E27133AAF586AB21
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