Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Drop ABG filtering

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-10-21 09:22:33 +02:00
parent dead2cdfd9
commit d2daec8222
7 changed files with 3 additions and 132 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

@ -82,7 +82,6 @@ typedef enum {
DEBUG_AUTOLEVEL,
DEBUG_IMU2,
DEBUG_ALTITUDE,
DEBUG_GYRO_ALPHA_BETA_GAMMA,
DEBUG_SMITH_PREDICTOR,
DEBUG_AUTOTRIM,
DEBUG_AUTOTUNE,

66
src/main/common/filter.c
View file

@ -317,72 +317,6 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
filter->y2 = y2;
}
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
// beta, gamma, and eta gains all derived from
// http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
filter->xk = 0.0f;
filter->vk = 0.0f;
filter->ak = 0.0f;
filter->jk = 0.0f;
filter->a = alpha;
filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
filter->dT = dT;
filter->dT2 = dT * dT;
filter->dT3 = dT * dT * dT;
pt1FilterInit(&filter->boostFilter, 100, dT);
const float boost = boostGain * 100;
filter->boost = (boost * boost / 10000) * 0.003;
filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
}
FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
//xk - current system state (ie: position)
//vk - derivative of system state (ie: velocity)
//ak - derivative of system velociy (ie: acceleration)
//jk - derivative of system acceleration (ie: jerk)
float rk; // residual error
// give the filter limited history
filter->xk *= filter->halfLife;
filter->vk *= filter->halfLife;
filter->ak *= filter->halfLife;
filter->jk *= filter->halfLife;
// update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
// update (estimated) velocity (also estimated dterm from measurement)
filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
filter->ak += filter->dT * filter->jk;
// what is our residual error (measured - estimated)
rk = input - filter->xk;
// artificially boost the error to increase the response of the filter
rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
rk = (input - filter->xk) / filter->a;
}
filter->rk = rk; // for logging
// update our estimates given the residual error.
filter->xk += filter->a * rk;
filter->vk += filter->b / filter->dT * rk;
filter->ak += filter->g / (2.0f * filter->dT2) * rk;
filter->jk += filter->e / (6.0f * filter->dT3) * rk;
return filter->xk;
}
#endif
FUNCTION_COMPILE_FOR_SIZE
void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
const float dT = refreshRate * 1e-6f;

2
src/main/fc/config.c
View file

@ -107,7 +107,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES
);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 4);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 5);
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.current_profile_index = 0,

23
src/main/fc/settings.yaml
View file

@ -94,7 +94,7 @@ tables:
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE",
"GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
"SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -309,27 +309,6 @@ groups:
min: 0
max: 2
default_value: 0
- name: gyro_abg_alpha
description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
default_value: 0
field: alphaBetaGammaAlpha
condition: USE_ALPHA_BETA_GAMMA_FILTER
min: 0
max: 1
- name: gyro_abg_boost
description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
default_value: 0.35
field: alphaBetaGammaBoost
condition: USE_ALPHA_BETA_GAMMA_FILTER
min: 0
max: 2
- name: gyro_abg_half_life
description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
default_value: 0.5
field: alphaBetaGammaHalfLife
condition: USE_ALPHA_BETA_GAMMA_FILTER
min: 0
max: 10
- name: setpoint_kalman_enabled
description: "Enable Kalman filter on the gyro data"
default_value: ON

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

@ -103,14 +103,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn;
STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 15);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 16);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -136,11 +129,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
.dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
#endif
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
.alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT,
.alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
.alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
#endif
#ifdef USE_GYRO_KALMAN
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
.kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
@ -313,23 +301,6 @@ static void gyroInitFilters(void)
}
}
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply;
if (gyroConfig()->alphaBetaGammaAlpha > 0) {
abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply;
for (int axis = 0; axis < 3; axis++) {
alphaBetaGammaFilterInit(
&abgFilter[axis],
gyroConfig()->alphaBetaGammaAlpha,
gyroConfig()->alphaBetaGammaBoost,
gyroConfig()->alphaBetaGammaHalfLife,
getLooptime() * 1e-6f
);
}
}
#endif
#ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) {
gyroKalmanInitialize(gyroConfig()->kalman_q);
@ -503,12 +474,6 @@ void FAST_CODE NOINLINE gyroFilter()
gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf);
gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf);
DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf);
#endif
#ifdef USE_DYNAMIC_FILTERS
if (dynamicGyroNotchState.enabled) {
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);

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

@ -83,11 +83,6 @@ typedef struct gyroConfig_s {
uint16_t dynamicGyroNotchMinHz;
uint8_t dynamicGyroNotchEnabled;
#endif
#ifdef USE_ALPHA_BETA_GAMMA_FILTER
float alphaBetaGammaAlpha;
float alphaBetaGammaBoost;
float alphaBetaGammaHalfLife;
#endif
#ifdef USE_GYRO_KALMAN
uint16_t kalman_q;
uint8_t kalmanEnabled;

1
src/main/target/common.h
View file

@ -87,7 +87,6 @@
#define USE_PITOT
#define USE_PITOT_ADC
#define USE_ALPHA_BETA_GAMMA_FILTER
#define USE_DYNAMIC_FILTERS
#define USE_GYRO_KALMAN
#define USE_SMITH_PREDICTOR