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

Merge pull request #6421 from mikeller/add_dynamic_filter_parameters

Browse source 
Added parameters for dynamic notch filter.
This commit is contained in:
Michael Keller 2018-07-23 07:31:20 +12:00 • committed by GitHub
commit 179d01fb3f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 15 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/interface/settings.c
View file

@ -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) },

2
src/main/sensors/gyro.c
View file

@ -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,
);

2
src/main/sensors/gyro.h
View file

@ -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);

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

@ -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(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);