rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

* 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View File

@ -1290,8 +1290,10 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_lowpass_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz", "%d", gyroConfig()->gyro_lowpass2_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,

13
src/main/cms/cms_menu_imu.c
View File

@ -304,7 +304,9 @@ static CMS_Menu cmsx_menuProfileOther = {
.entries = cmsx_menuProfileOtherEntries,
};
static uint8_t gyroConfig_gyro_soft_lpf_hz;
static uint16_t gyroConfig_gyro_lowpass_hz;
static uint16_t gyroConfig_gyro_lowpass2_hz;
static uint16_t gyroConfig_gyro_soft_notch_hz_1;
static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
static uint16_t gyroConfig_gyro_soft_notch_hz_2;
@ -312,7 +314,8 @@ static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
static long cmsx_menuGyro_onEnter(void)
{
gyroConfig_gyro_soft_lpf_hz = gyroConfig()->gyro_soft_lpf_hz;
gyroConfig_gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
gyroConfig_gyro_lowpass2_hz = gyroConfig()->gyro_lowpass2_hz;
gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
@ -325,7 +328,8 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self)
{
UNUSED(self);
gyroConfigMutable()->gyro_soft_lpf_hz = gyroConfig_gyro_soft_lpf_hz;
gyroConfigMutable()->gyro_lowpass_hz = gyroConfig_gyro_lowpass_hz;
gyroConfigMutable()->gyro_lowpass2_hz = gyroConfig_gyro_lowpass2_hz;
gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
@ -338,7 +342,8 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
{
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz, 0, 16000, 1 }, 0 },
{ "GYRO LPF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz, 0, 16000, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },

80
src/main/common/filter.c
View File

@ -42,10 +42,15 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input)
// PT1 Low Pass filter
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
float pt1FilterGain(uint8_t f_cut, float dT)
{
float RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->k = dT / (RC + dT);
float RC = 1 / ( 2 * M_PI_FLOAT * f_cut);
return dT / (RC + dT);
}
void pt1FilterInit(pt1Filter_t *filter, float k)
{
filter->k = k;
}
FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
@ -245,6 +250,14 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
return result;
}
FAST_CODE float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input)
{
for (int i = 0; i < filter->sections; i++ ) {
input = biquadFilterApply(&filter->biquad[i], input);
}
return input;
}
/*
* FIR filter
*/
@ -342,6 +355,7 @@ float firFilterLastInput(const firFilter_t *filter)
return filter->buf[index];
}
#if defined(USE_FIR_FILTER_DENOISE)
void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
{
memset(filter, 0, sizeof(firFilterDenoise_t));
@ -364,36 +378,54 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
return filter->movingSum / ++filter->filledCount + 1;
}
}
#endif
#if defined(USE_GYRO_FAST_KALMAN)
// Fast two-state Kalman
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p)
// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k)
{
filter->q = q * 0.000001f; // add multiplier to make tuning easier
filter->r = r * 0.001f; // add multiplier to make tuning easier
filter->p = p * 0.001f; // add multiplier to make tuning easier
filter->x = 0.0f; // set initial value, can be zero if unknown
filter->lastX = 0.0f; // set initial value, can be zero if unknown
filter->k = 0.0f; // kalman gain
filter->b0 = k / 2;
filter->b1 = k / 2;
filter->b2 = 0;
filter->a1 = -(1 - k);
filter->a2 = 0;
}
// rs2k's fast "kalman" filter
void fastKalmanInit(fastKalman_t *filter, float k)
{
filter->x = 0.0f; // set initial value, can be zero if unknown
filter->lastX = 0.0f; // set initial value, can be zero if unknown
filter->k = k; // "kalman" gain - half of RC coefficient, calculated externally
}
FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input)
{
// project the state ahead using acceleration
filter->x += (filter->x - filter->lastX);
// update last state
filter->lastX = filter->x;
// prediction update
filter->p = filter->p + filter->q;
// measurement update
filter->k = filter->p / (filter->p + filter->r);
filter->x += filter->k * (input - filter->x);
filter->p = (1.0f - filter->k) * filter->p;
return filter->x;
}
#endif
void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight)
{
filter->movingWindowIndex = 0;
filter->windowSize = windowSize;
filter->weight = weight;
}
FAST_CODE float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input)
{
filter->movingSum -= filter->buf[filter->movingWindowIndex];
filter->buf[filter->movingWindowIndex] = input;
filter->movingSum += input;
uint8_t windowIndex = filter->movingWindowIndex;
if (++windowIndex >= filter->windowSize) {
filter->movingWindowIndex = 0;
} else {
filter->movingWindowIndex = windowIndex;
}
return input + (((filter->movingSum / filter->windowSize) - input) * filter->weight);
}

41
src/main/common/filter.h
View File

@ -25,6 +25,8 @@
#define MAX_FIR_DENOISE_WINDOW_SIZE 120
#endif
#define MAX_LMA_WINDOW_SIZE 12
struct filter_s;
typedef struct filter_s filter_t;
@ -45,6 +47,13 @@ typedef struct biquadFilter_s {
float x1, x2, y1, y2;
} biquadFilter_t;
#define BIQUAD_LPF_ORDER_MAX 6
typedef struct biquadFilterCascade_s {
int sections;
biquadFilter_t biquad[(BIQUAD_LPF_ORDER_MAX + 1) / 2]; // each section is of second order
} biquadFilterCascade_t;
typedef struct firFilterDenoise_s {
int filledCount;
int targetCount;
@ -54,20 +63,27 @@ typedef struct firFilterDenoise_s {
} firFilterDenoise_t;
typedef struct fastKalman_s {
float q; // process noise covariance
float r; // measurement noise covariance
float p; // estimation error covariance matrix
float k; // kalman gain
float k; // "kalman" gain
float x; // state
float lastX; // previous state
} fastKalman_t;
typedef struct laggedMovingAverage_s {
uint8_t movingWindowIndex;
uint8_t windowSize;
float weight;
float movingSum;
float buf[MAX_LMA_WINDOW_SIZE];
} laggedMovingAverage_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_FIR,
FILTER_SLEW
} filterType_e;
FILTER_BUTTERWORTH,
FILTER_BIQUAD_RC_FIR2,
FILTER_FAST_KALMAN
} lowpassFilterType_e;
typedef enum {
FILTER_LPF, // 2nd order Butterworth section
@ -90,7 +106,7 @@ typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
float nullFilterApply(filter_t *filter, float input);
#define BIQUAD_LPF_ORDER_MAX 6
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
@ -99,15 +115,22 @@ int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filter
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p);
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k);
void fastKalmanInit(fastKalman_t *filter, float k);
float fastKalmanUpdate(fastKalman_t *filter, float input);
void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight);
float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input);
// not exactly correct, but very very close and much much faster
#define filterGetNotchQApprox(centerFreq, cutoff) ((float)(cutoff * centerFreq) / ((float)(centerFreq - cutoff) * (float)(centerFreq + cutoff)))
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
float pt1FilterGain(uint8_t f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);

12
src/main/flight/pid.c
View File

@ -219,15 +219,15 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} else {
dtermNotchApplyFn = nullFilterApply;
}
//2nd Dterm Lowpass Filter
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
dtermLowpass2ApplyFn = nullFilterApply;
} else {
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
pt1FilterInit(&dtermLowpass2[axis], pidProfile->dterm_lowpass2_hz, dT);
}
pt1FilterInit(&dtermLowpass2[axis], pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
}
}
if (pidProfile->dterm_lowpass_hz == 0 || pidProfile->dterm_lowpass_hz > pidFrequencyNyquist) {
@ -240,7 +240,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
case FILTER_PT1:
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pidProfile->dterm_lowpass_hz, dT);
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass_hz, dT));
}
break;
case FILTER_BIQUAD:
@ -264,7 +264,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
ptermYawLowpassApplyFn = nullFilterApply;
} else {
ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
pt1FilterInit(&ptermYawLowpass, pidProfile->yaw_lowpass_hz, dT);
pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT));
}
}

4
src/main/interface/msp.c
View File

@ -1185,7 +1185,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
sbufWriteU16(dst, currentPidProfile->yaw_lowpass_hz);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
@ -1657,7 +1657,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_FILTER_CONFIG:
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
currentPidProfile->yaw_lowpass_hz = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 8) {

33
src/main/interface/settings.c
View File

@ -253,7 +253,14 @@ static const char * const lookupTableRcInterpolationChannels[] = {
};
static const char * const lookupTableLowpassType[] = {
"PT1", "BIQUAD", "FIR"
"PT1",
"BIQUAD",
#if defined(USE_FIR_FILTER_DENOISE)
"FIR",
#endif
"BUTTERWORTH",
"BIQUAD_RC_FIR2",
"FAST_KALMAN"
};
static const char * const lookupTableFailsafe[] = {
@ -370,21 +377,23 @@ const clivalue_t valueTable[] = {
{ "gyro_high_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_high_fsr) },
#endif
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_sync_denom) },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_type) },
{ "gyro_lowpass_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz) },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_type) },
{ "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz) },
{ "gyro_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_order) },
{ "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) },
{ "gyro_lowpass2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz) },
{ "gyro_lowpass2_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_order) },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_1) },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
#if defined(USE_GYRO_FAST_KALMAN)
{ "gyro_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_q) },
{ "gyro_filter_r", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_r) },
{ "gyro_filter_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_p) },
#endif
#if defined(USE_GYRO_LPF2)
{ "gyro_stage2_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf2_hz) },
{ "gyro_stage2_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF2_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf2_order) },
#endif
{ "gyro_lma_depth", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 11}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_depth)},
{ "gyro_lma_weight", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 100}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lma_weight)},
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
{ "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
#ifdef USE_GYRO_OVERFLOW_CHECK

262
src/main/sensors/gyro.c
View File

@ -98,21 +98,34 @@ typedef struct gyroCalibration_s {
bool firstArmingCalibrationWasStarted = false;
typedef union gyroSoftFilter_u {
biquadFilter_t gyroFilterLpfState[XYZ_AXIS_COUNT];
pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT];
#if defined(USE_FIR_FILTER_DENOISE)
firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
#if GYRO_LPF_ORDER_MAX > BIQUAD_LPF_ORDER_MAX
#error "GYRO_LPF_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX"
#endif
} gyroSoftLpfFilter_t;
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
biquadFilterCascade_t biquadFilterCascadeState;
firFilterDenoise_t denoiseFilterState;
fastKalman_t fastKalmanFilterState;
} gyroLowpassFilter_t;
typedef struct gyroSensor_s {
gyroDev_t gyroDev;
gyroCalibration_t calibration;
// gyro soft filter
filterApplyFnPtr softLpfFilterApplyFn;
gyroSoftLpfFilter_t softLpfFilter;
filter_t *softLpfFilterPtr[XYZ_AXIS_COUNT];
// lowpass gyro soft filter
filterApplyFnPtr lowpassFilterApplyFn;
gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
// lowpass2 gyro soft filter
filterApplyFnPtr lowpass2FilterApplyFn;
gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
// lagged moving average smoothing
filterApplyFnPtr lmaSmoothingApplyFn;
laggedMovingAverage_t lmaSmoothingFilter[XYZ_AXIS_COUNT];
// notch filters
filterApplyFnPtr notchFilter1ApplyFn;
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
@ -120,18 +133,9 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
timeUs_t overflowTimeUs;
bool overflowDetected;
#if defined(USE_GYRO_FAST_KALMAN)
// gyro kalman filter
filterApplyFnPtr fastKalmanApplyFn;
fastKalman_t fastKalman[XYZ_AXIS_COUNT];
#endif
#if defined(USE_GYRO_LPF2)
// lowpass filter, cascaded biquad sections
int biquadLpf2Sections;
biquadFilter_t biquadLpf2[XYZ_AXIS_COUNT][(GYRO_LPF2_ORDER_MAX + 1) / 2]; // each section is of second order
#endif
} gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1;
@ -144,13 +148,8 @@ STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#endif
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_q, uint16_t gyro_filter_r, uint16_t gyro_filter_p);
#endif
#if defined (USE_GYRO_LPF2)
static void gyroInitFilterLpf2(gyroSensor_t *gyroSensor, int order, int lpfHz);
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order);
#define DEBUG_GYRO_CALIBRATION 3
@ -166,7 +165,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#ifdef USE_DUAL_GYRO
@ -181,8 +180,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyroMovementCalibrationThreshold = 48,
.gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
.gyro_lpf = GYRO_LPF_256HZ,
.gyro_soft_lpf_type = FILTER_PT1,
.gyro_soft_lpf_hz = 90,
.gyro_lowpass_type = FILTER_PT1,
.gyro_lowpass_hz = 90,
.gyro_lowpass_order = 1,
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz = 0,
.gyro_lowpass2_order = 1,
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
@ -191,12 +194,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_hz_2 = 200,
.gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.gyro_soft_lpf2_hz = 0,
.gyro_filter_q = 0,
.gyro_filter_r = 0,
.gyro_filter_p = 0,
.gyro_offset_yaw = 0,
.gyro_soft_lpf2_order = 1,
.gyro_lma_depth = 0,
.gyro_lma_weight = 100,
);
@ -559,38 +559,97 @@ bool gyroInit(void)
return ret;
}
void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order)
{
gyroSensor->softLpfFilterApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
filterApplyFnPtr *lowpassFilterApplyFn;
gyroLowpassFilter_t *lowpassFilter = NULL;
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
switch (gyroConfig()->gyro_soft_lpf_type) {
case FILTER_BIQUAD:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroFilterLpfState[axis];
biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime);
}
break;
switch (slot) {
case FILTER_LOWPASS:
lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn;
lowpassFilter = gyroSensor->lowpassFilter;
break;
case FILTER_LOWPASS2:
lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
lowpassFilter = gyroSensor->lowpass2Filter;
break;
default:
return;
}
// Establish some common constants
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
const float gyroDt = gyro.targetLooptime * 1e-6f;
// Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
const float gain = pt1FilterGain(lpfHz, gyroDt);
// Dereference the pointer to null before checking valid cutoff and filter
// type. It will be overridden for positive cases.
*lowpassFilterApplyFn = &nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
switch (type) {
case FILTER_PT1:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
*lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroFilterPt1State[axis];
pt1FilterInit(&gyroSensor->softLpfFilter.gyroFilterPt1State[axis], lpfHz, gyroDt);
pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
}
break;
case FILTER_BIQUAD:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
}
break;
case FILTER_BIQUAD_RC_FIR2:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadRCFIR2FilterInit(&lowpassFilter[axis].biquadFilterState, gain);
}
break;
case FILTER_BUTTERWORTH:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadCascadeFilterApply;
if (order <= GYRO_LPF_ORDER_MAX) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
int *sections = &lowpassFilter[axis].biquadFilterCascadeState.sections;
biquadFilter_t *biquadSections = lowpassFilter[axis].biquadFilterCascadeState.biquad;
*sections = biquadFilterLpfCascadeInit(biquadSections, order, lpfHz, gyro.targetLooptime);
}
}
break;
case FILTER_FAST_KALMAN:
*lowpassFilterApplyFn = (filterApplyFnPtr) fastKalmanUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fastKalmanInit(&lowpassFilter[axis].fastKalmanFilterState, gain / 2);
}
break;
default:
#if defined(USE_FIR_FILTER_DENOISE)
// this should be case FILTER_FIR:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
case FILTER_FIR:
*lowpassFilterApplyFn = (filterApplyFnPtr) firFilterDenoiseUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroDenoiseState[axis];
firFilterDenoiseInit(&gyroSensor->softLpfFilter.gyroDenoiseState[axis], lpfHz, gyro.targetLooptime);
firFilterDenoiseInit(&lowpassFilter[axis].denoiseFilterState, lpfHz, gyro.targetLooptime);
}
#endif
break;
#endif
}
}
}
static void gyroInitLmaSmoothing(gyroSensor_t *gyroSensor, uint8_t depth, uint8_t weight)
{
gyroSensor->lmaSmoothingApplyFn = nullFilterApply;
if (depth && weight) {
const uint8_t windowSize = depth + 1;
const float lmaWeight = weight * 0.01f;
gyroSensor->lmaSmoothingApplyFn = (filterApplyFnPtr)lmaSmoothingUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
lmaSmoothingInit(&gyroSensor->lmaSmoothingFilter[axis], windowSize, lmaWeight);
}
}
}
@ -668,54 +727,31 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
}
#endif
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_q, uint16_t gyro_filter_r, uint16_t gyro_filter_p)
{
gyroSensor->fastKalmanApplyFn = nullFilterApply;
// If Kalman Filter noise covariances for Process and Measurement are non-zero, we treat as enabled
if (gyro_filter_q != 0 && gyro_filter_r != 0) {
gyroSensor->fastKalmanApplyFn = (filterApplyFnPtr)fastKalmanUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fastKalmanInit(&gyroSensor->fastKalman[axis], gyro_filter_q, gyro_filter_r, gyro_filter_p);
}
}
}
#endif
#if defined(USE_GYRO_LPF2)
#if GYRO_LPF2_ORDER_MAX > BIQUAD_LPF_ORDER_MAX
# error "GYRO_LPF2_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX"
#endif
static void gyroInitFilterLpf2(gyroSensor_t *gyroSensor, int order, int lpfHz)
{
const int gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
int sections = 0;
if (lpfHz && lpfHz <= gyroFrequencyNyquist && order <= GYRO_LPF2_ORDER_MAX) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
const int axisSections = biquadFilterLpfCascadeInit(gyroSensor->biquadLpf2[axis], order, lpfHz, gyro.targetLooptime);
sections = MAX(sections, axisSections);
}
}
gyroSensor->biquadLpf2Sections = sections;
}
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{
#if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor);
#endif
#if defined(USE_GYRO_FAST_KALMAN)
gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_r, gyroConfig()->gyro_filter_p);
#endif
#if defined(USE_GYRO_LPF2)
gyroInitFilterLpf2(gyroSensor, gyroConfig()->gyro_soft_lpf2_order, gyroConfig()->gyro_soft_lpf2_hz);
#endif
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type,
gyroConfig()->gyro_lowpass_hz,
gyroConfig()->gyro_lowpass_order
);
gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type,
gyroConfig()->gyro_lowpass2_hz,
gyroConfig()->gyro_lowpass2_order
);
gyroInitLmaSmoothing(gyroSensor, gyroConfig()->gyro_lma_depth, gyroConfig()->gyro_lma_weight);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE
@ -948,21 +984,18 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
#if defined(USE_GYRO_FAST_KALMAN)
gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
#endif
#if defined(USE_GYRO_LPF2)
for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) {
gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf);
}
#endif
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
#endif
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
@ -977,15 +1010,8 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
// DEBUG_GYRO_NOTCH records the unfiltered gyro output
DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf));
#if defined(USE_GYRO_FAST_KALMAN)
// apply fast kalman
gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
#endif
#if defined(USE_GYRO_LPF2)
for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) {
gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf);
}
#endif
// apply lowpass2 filter
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter
@ -1004,9 +1030,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
// apply LPF
// apply lowpass2 filter
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) {

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

1
src/main/target/YUPIF7/target.h
View File

@ -127,7 +127,6 @@
#define SERIALRX_UART SERIAL_PORT_USART6
#define TELEMETRY_UART SERIAL_PORT_USART1
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD)
#define USE_GYRO_FAST_KALMAN
// Target IO and timers
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

5
src/test/unit/sensor_gyro_unittest.cc
View File

@ -112,7 +112,10 @@ TEST(SensorGyro, Update)
{
pgResetAll();
// turn off filters
gyroConfigMutable()->gyro_soft_lpf_hz = 0;
gyroConfigMutable()->gyro_lowpass_hz = 0;
gyroConfigMutable()->gyro_lowpass2_hz = 0;
gyroConfigMutable()->gyro_lma_depth = 0;
gyroConfigMutable()->gyro_lma_weight = 0;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
gyroInit();