mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
Automatically adjust bin offset based on dyn notch minimum.
Changes per review. Drop min HZ to allow for 7". Put alignment back. Fix mistakenly delete variable.
This commit is contained in:
parent
dc6aad22c5
commit
832306743e
2 changed files with 5 additions and 8 deletions
|
@ -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) },
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue