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

Dual-stage Gyro Filtering: PT1, Biquad, Butterworth, Denoise (FIR), FKF (fixed K), and Biquad RC+FIR2 (#5391)

Browse source 
* Dual-stage Gyro Filtering: PT1, FKF, and Biquad RC+FIR2

* Builds on the previous work of apocolipse.
* Fixes 'stage2'/'stage1' mis-naming to reflect where it is applied in the loop.
  That is, the older Biquad, PT1, Denoise (FIR) filters are 'stage2' - applied
  after dynamic and static notches (if enabled), and the controversial PT1,
  'fast Kalman' filter, and Biquad RC+FIR2 filters, are 'stage1'. e.g. before
  dynamic notch.
* FKF bruteforce Kalman gain removed. Calculate from half of PT1 RC constant,
  automatically taking loop-time into account.
* New union type definition for stage1 filtering.
* New gyro sensor members for stage1 filter application function and states for
  all three supported filter types
* New enum types for stage1 v. stage2. dterm lowpass type references 'stage2'.
* updates to CMS/MSP/FC to allow compilation (untested, probably breaks
  MSP, Lua, and ~comms with BFC~).
* Refactors FKF initialization, update and associated structures to be faster by
  not continuously calculating 'k'. Filter gain is calculated once during
  initialization from RC constant as per PT1 and Biquad RC+FIR2. It was
  discovered this converges to static value within 100 samples at 32kHz, so can
  be removed. Remove related interface (CLI) settings.
* update dterm_lowpass_type to use new 'TABLE_LOWPASS2_TYPE' (biquad/pt1/FIR)
* Stage 1 defaults to PT1, 763Hz (equivalent to Q400 / R88 from quasi-kalman
  filter) - suitable for 32kHz sampling modes. Can be switched to Biquad
  RC+FIR2, and FKF.
* Update `#if defined(USE_GYRO_SLEW_LIMITER) to `#ifdef`.
* Includes optional Lagged Moving Average 'smoothing' pipeline step, applied (in
  code) after the output of stage1.
* (diehertz): Removed redundant pointers from gyro filtering

* blackbox: fix indentation

* cms IMU menu: fix indentation

* filters: remove USE_GYRO_FIR_FILTER_DENOISE in filter type enum

* gyro sensors: go back to `if defined()` form. for slew limiter

* gyro sensors: increment parameter group version

* due to non-appending changes, the version must be bumped.
This commit is contained in:
AJ Christensen 2018-03-21 15:52:59 +13:00 • committed by Michael Keller
parent a579fc5b22
commit 46291a8374
11 changed files with 303 additions and 188 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -59,30 +59,46 @@ typedef enum {
#define GYRO_CONFIG_USE_GYRO_2 1
#define GYRO_CONFIG_USE_GYRO_BOTH 2
typedef enum {
FILTER_LOWPASS = 0,
FILTER_LOWPASS2
} filterSlots;
#define GYRO_LPF_ORDER_MAX 6
typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_type;
uint8_t gyro_soft_lpf_hz;
bool gyro_high_fsr;
bool gyro_use_32khz;
uint8_t gyro_to_use;
uint16_t gyro_soft_lpf2_hz;
// Lagged Moving Average smoother
uint8_t gyro_lma_depth;
uint8_t gyro_lma_weight;
// Lowpass primary/secondary
uint8_t gyro_lowpass_type;
uint8_t gyro_lowpass2_type;
// Order is used for the 'higher ordering' of cascaded butterworth/biquad sections
uint8_t gyro_lowpass_order;
uint8_t gyro_lowpass2_order;
uint16_t gyro_lowpass_hz;
uint16_t gyro_lowpass2_hz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
gyroOverflowCheck_e checkOverflow;
uint16_t gyro_filter_q;
uint16_t gyro_filter_r;
uint16_t gyro_filter_p;
int16_t gyro_offset_yaw;
uint8_t gyro_soft_lpf2_order;
} gyroConfig_t;
#define GYRO_LPF2_ORDER_MAX 6
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);