diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9e27e31fbc..853b7318e2 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -499,14 +499,16 @@ const clivalue_t valueTable[] = { { "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 500, 1950 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) }, #endif -#if defined(GYRO_USES_SPI) -#ifdef USE_32K_CAPABLE_GYRO +#if defined(GYRO_USES_SPI) && defined(USE_32K_CAPABLE_GYRO) { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) }, #endif -#endif #ifdef USE_DUAL_GYRO { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif +#if defined(USE_GYRO_DATA_ANALYSE) + { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, + { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) }, +#endif // PG_ACCELEROMETER_CONFIG { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 1e288e9b95..42f0562113 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -197,6 +197,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, + .dyn_notch_quality = 70, + .dyn_notch_width_percent = 50, ); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 415412aac2..b726620343 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -96,6 +96,8 @@ typedef struct gyroConfig_s { int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second + uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass + uint8_t dyn_notch_width_percent; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index fd7fc21809..7e8fd5c6c6 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -52,14 +52,11 @@ #define FFT_BIN_OFFSET 1 // compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz #define FFT_SAMPLING_RATE_HZ 1333 // analyse up to 666Hz, 16 bins each 41.655z wide #define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) // centre frequency of bandpass that constrains input to FFT -#define FFT_BIQUAD_Q 0.05f // bandpass quality factor, 0.1 for steep sided bandpass #define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin #define FFT_MIN_BIN_RISE 2 // following bin must be at least 2 times previous to indicate start of peak #define DYN_NOTCH_SMOOTH_FREQ_HZ 60 // lowpass frequency for smoothing notch centre point #define DYN_NOTCH_MIN_CENTRE_HZ 125 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin #define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist -#define DYN_NOTCH_WIDTH_PERCENT 25 // maybe adjustable via CLI? -#define DYN_NOTCH_CUTOFF ((float)(100 - DYN_NOTCH_WIDTH_PERCENT) / 100) #define DYN_NOTCH_MIN_CUTOFF_HZ 105 // lowest allowed notch cutoff frequency #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis @@ -85,6 +82,8 @@ static FAST_RAM_ZERO_INIT biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT]; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; +static FAST_RAM_ZERO_INIT float dynamicNotchCutoff; + void initHanning(void) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) { @@ -118,8 +117,10 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { fftResult[axis].centerFreq = 200; // any init value biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); - biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, FFT_BIQUAD_Q, FILTER_BPF); + biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, 0.001f * gyroConfig()->dyn_notch_quality, FILTER_BPF); } + + dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100; } // used in OSD @@ -296,7 +297,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) { // 7us // calculate cutoffFreq and notch Q, update notch filter - float cutoffFreq = fmax(fftResult[axis].centerFreq * DYN_NOTCH_CUTOFF, DYN_NOTCH_MIN_CUTOFF_HZ); + float cutoffFreq = fmax(fftResult[axis].centerFreq * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ); float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq); biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);