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

Cascaded notch filters.

This commit is contained in:
Kenneth Mitchell 2018-10-27 09:16:55 -04:00
parent 3736a2486d
commit 011eae93c6
No known key found for this signature in database
GPG Key ID: E27133AAF586AB21
10 changed files with 128 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -76,4 +76,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RX_SIGNAL_LOSS", "RX_SIGNAL_LOSS",
"RC_SMOOTHING_RATE", "RC_SMOOTHING_RATE",
"ANTI_GRAVITY", "ANTI_GRAVITY",
"DYN_LPF",
}; };

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

@ -94,6 +94,7 @@ typedef enum {
DEBUG_RX_SIGNAL_LOSS, DEBUG_RX_SIGNAL_LOSS,
DEBUG_RC_SMOOTHING_RATE, DEBUG_RC_SMOOTHING_RATE,
DEBUG_ANTI_GRAVITY, DEBUG_ANTI_GRAVITY,
DEBUG_DYN_LPF,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

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

@ -167,8 +167,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_limit = 90, .abs_control_limit = 90,
.abs_control_error_limit = 20, .abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH, .antiGravityMode = ANTI_GRAVITY_SMOOTH,
.dyn_lpf_dterm_max_hz = 200, .dyn_lpf_dterm_max_hz = 250,
.dyn_lpf_dterm_idle = 20,
.dterm_lowpass_hz = 100, // dual PT1 filtering ON by default .dterm_lowpass_hz = 100, // dual PT1 filtering ON by default
.dterm_lowpass2_hz = 200, // second Dterm LPF ON by default .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default
.dterm_filter_type = FILTER_PT1, .dterm_filter_type = FILTER_PT1,
@ -180,8 +179,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.launchControlAllowTriggerReset = true, .launchControlAllowTriggerReset = true,
); );
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
pidProfile->dterm_lowpass_hz = 120; pidProfile->dterm_lowpass_hz = 150;
pidProfile->dterm_lowpass2_hz = 180; pidProfile->dterm_lowpass2_hz = 150;
pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_filter2_type = FILTER_BIQUAD; pidProfile->dterm_filter2_type = FILTER_BIQUAD;
#endif #endif
@ -464,11 +463,9 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
} }
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
#endif #endif
void pidInitConfig(const pidProfile_t *pidProfile) void pidInitConfig(const pidProfile_t *pidProfile)
@ -547,9 +544,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) { if (pidProfile->dterm_lowpass_hz > 0 && pidProfile->dyn_lpf_dterm_max_hz > pidProfile->dterm_lowpass_hz) {
if (pidProfile->dterm_lowpass_hz > 0 ) {
dynLpfMin = pidProfile->dterm_lowpass_hz;
switch (pidProfile->dterm_filter_type) { switch (pidProfile->dterm_filter_type) {
case FILTER_PT1: case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1; dynLpfFilter = DYN_LPF_PT1;
@ -561,13 +556,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
dynLpfFilter = DYN_LPF_NONE; dynLpfFilter = DYN_LPF_NONE;
break; break;
} }
}
} else { } else {
dynLpfFilter = DYN_LPF_NONE; dynLpfFilter = DYN_LPF_NONE;
} }
dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f; dynLpfMin = pidProfile->dterm_lowpass_hz;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
#endif #endif
#ifdef USE_LAUNCH_CONTROL #ifdef USE_LAUNCH_CONTROL
@ -1299,17 +1292,13 @@ bool pidAntiGravityEnabled(void)
void dynLpfDTermUpdate(float throttle) void dynLpfDTermUpdate(float throttle)
{ {
if (dynLpfFilter != DYN_LPF_NONE) { if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin; const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
if (dynLpfFilter == DYN_LPF_PT1) { if (dynLpfFilter == DYN_LPF_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT)); pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
} }
} else { } else if (dynLpfFilter == DYN_LPF_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime); biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
} }

1
src/main/flight/pid.h
View File

@ -150,7 +150,6 @@ typedef struct pidProfile_s {
uint8_t abs_control_error_limit; // Limit to the accumulated error uint8_t abs_control_error_limit; // Limit to the accumulated error
uint8_t dterm_filter2_type; // Filter selection for 2nd dterm uint8_t dterm_filter2_type; // Filter selection for 2nd dterm
uint16_t dyn_lpf_dterm_max_hz; uint16_t dyn_lpf_dterm_max_hz;
uint8_t dyn_lpf_dterm_idle;
uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery) uint8_t launchControlMode; // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
uint8_t launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control uint8_t launchControlThrottlePercent; // Throttle percentage to trigger launch for launch control
uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC) uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC)

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

@ -386,7 +386,7 @@ static const char * const lookupTableDynamicFftLocation[] = {
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
}; };
static const char * const lookupTableDynamicFilterRange[] = { static const char * const lookupTableDynamicFilterRange[] = {
"HIGH", "MEDIUM", "LOW" "HIGH", "MEDIUM", "LOW", "AUTO"
}; };
#endif // USE_GYRO_DATA_ANALYSE #endif // USE_GYRO_DATA_ANALYSE
@ -565,15 +565,14 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
{ "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) }, { "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) },
{ "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
{ "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) }, { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) }, { "dyn_lpf_gyro_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
{ "dyn_lpf_gyro_idle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) },
{ "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) }, { "dyn_lpf_dterm_max_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
{ "dyn_lpf_dterm_idle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) },
#endif #endif
// PG_ACCELEROMETER_CONFIG // PG_ACCELEROMETER_CONFIG

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

@ -141,6 +141,7 @@ typedef struct gyroSensor_s {
filterApplyFnPtr notchFilterDynApplyFn; filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
// overflow and recovery // overflow and recovery
timeUs_t overflowTimeUs; timeUs_t overflowTimeUs;
@ -185,7 +186,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #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) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -214,13 +215,15 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyro_offset_yaw = 0; gyroConfig->gyro_offset_yaw = 0;
gyroConfig->yaw_spin_recovery = true; gyroConfig->yaw_spin_recovery = true;
gyroConfig->yaw_spin_threshold = 1950; gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_filter_width_percent = 40; gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_AUTO;
gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS; gyroConfig->dyn_notch_width_percent = 8;
gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM; gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_lpf_gyro_max_hz = 400; gyroConfig->dyn_notch_min_hz = 150;
gyroConfig->dyn_lpf_gyro_idle = 20; gyroConfig->dyn_lpf_gyro_max_hz = 450;
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
gyroConfig->gyro_lowpass_hz = 120; gyroConfig->gyro_lowpass_hz = 150;
gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
gyroConfig->gyro_lowpass2_hz = 0;
#endif #endif
} }
@ -499,6 +502,7 @@ bool gyroInit(void)
case DEBUG_GYRO_RAW: case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED: case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED: case DEBUG_GYRO_FILTERED:
case DEBUG_DYN_LPF:
gyroDebugMode = debugMode; gyroDebugMode = debugMode;
break; break;
case DEBUG_DUAL_GYRO: case DEBUG_DUAL_GYRO:
@ -547,17 +551,13 @@ bool gyroInit(void)
} }
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE; static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
static FAST_RAM_ZERO_INIT float dynLpfIdle;
static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin; static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
static void dynLpfFilterInit() static void dynLpfFilterInit()
{ {
if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) { if (gyroConfig()->gyro_lowpass_hz > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > gyroConfig()->gyro_lowpass_hz) {
if (gyroConfig()->gyro_lowpass_hz > 0 ) {
dynLpfMin = gyroConfig()->gyro_lowpass_hz;
switch (gyroConfig()->gyro_lowpass_type) { switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1: case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1; dynLpfFilter = DYN_LPF_PT1;
@ -569,13 +569,11 @@ static void dynLpfFilterInit()
dynLpfFilter = DYN_LPF_NONE; dynLpfFilter = DYN_LPF_NONE;
break; break;
} }
}
} else { } else {
dynLpfFilter = DYN_LPF_NONE; dynLpfFilter = DYN_LPF_NONE;
} }
dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f; dynLpfMin = gyroConfig()->gyro_lowpass_hz;
dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f; dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin);
} }
#endif #endif
@ -701,6 +699,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
} }
} }
} }
@ -1022,7 +1021,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2);
} }
#endif #endif
@ -1211,14 +1210,17 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
#endif // USE_GYRO_REGISTER_DUMP #endif // USE_GYRO_REGISTER_DUMP
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
float dynThrottle(float throttle) {
return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
}
void dynLpfGyroUpdate(float throttle) void dynLpfGyroUpdate(float throttle)
{ {
if (dynLpfFilter != DYN_LPF_NONE) { if (dynLpfFilter != DYN_LPF_NONE) {
uint16_t cutoffFreq = dynLpfMin; const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
if (throttle > dynLpfIdle) {
const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f; DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
}
if (dynLpfFilter == DYN_LPF_PT1) { if (dynLpfFilter == DYN_LPF_PT1) {
const float gyroDt = gyro.targetLooptime * 1e-6f; const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -1233,7 +1235,7 @@ void dynLpfGyroUpdate(float throttle)
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
#endif #endif
} }
} else { } else if (dynLpfFilter == DYN_LPF_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) { if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {

23
src/main/sensors/gyro.h
View File

@ -43,15 +43,15 @@ typedef enum {
} gyroOverflowCheck_e; } gyroOverflowCheck_e;
enum { enum {
DYN_FFT_BEFORE_STATIC_FILTERS = 0, DYN_NOTCH_RANGE_HIGH = 0,
DYN_FFT_AFTER_STATIC_FILTERS DYN_NOTCH_RANGE_MEDIUM,
DYN_NOTCH_RANGE_LOW,
DYN_NOTCH_RANGE_AUTO
} ; } ;
enum { #define DYN_NOTCH_RANGE_HZ_HIGH 2000
DYN_FILTER_RANGE_HIGH = 0, #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
DYN_FILTER_RANGE_MEDIUM, #define DYN_NOTCH_RANGE_HZ_LOW 1000
DYN_FILTER_RANGE_LOW
} ;
enum { enum {
DYN_LPF_NONE = 0, DYN_LPF_NONE = 0,
@ -98,11 +98,11 @@ typedef struct gyroConfig_s {
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
uint8_t dyn_filter_width_percent; uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
uint8_t dyn_fft_location; // before or after static filters
uint8_t dyn_filter_range; // ignore any FFT bin below this threshold
uint16_t dyn_lpf_gyro_max_hz; uint16_t dyn_lpf_gyro_max_hz;
uint8_t dyn_lpf_gyro_idle; uint8_t dyn_notch_width_percent;
uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz;
} gyroConfig_t; } gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig); PG_DECLARE(gyroConfig_t, gyroConfig);
@ -126,5 +126,6 @@ bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis); uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg); uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
float dynThrottle(float throttle);
void dynLpfGyroUpdate(float throttle); void dynLpfGyroUpdate(float throttle);
#endif #endif

16
src/main/sensors/gyro_filter_impl.h
View File

@ -12,10 +12,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
} }
} }
float gyroDataForAnalysis = gyroADCf;
#endif #endif
// apply static notch filters and software lowpass filters // apply static notch filters and software lowpass filters
@ -26,17 +25,14 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) {
gyroDataForAnalysis = gyroADCf;
}
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
} }
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn2[axis], gyroADCf);
} }
#endif #endif

71
src/main/sensors/gyroanalyse.c
View File

@ -51,23 +51,18 @@
#define FFT_BIN_OFFSET 2 #define FFT_BIN_OFFSET 2
// smoothing frequency for FFT centre frequency // smoothing frequency for FFT centre frequency
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 #define DYN_NOTCH_SMOOTH_FREQ_HZ 50
// notch centre point will not go below sample rate divided by these dividers, resulting in range limits:
// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz
#define DYN_NOTCH_MIN_CENTRE_DIV 12
// divider to get lowest allowed notch cutoff frequency
// otherwise cutoff is user configured percentage below centre frequency
#define DYN_NOTCH_MIN_CUTOFF_DIV 15
// we need 4 steps for each axis // we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
static float FAST_RAM_ZERO_INIT fftResolution; static float FAST_RAM_ZERO_INIT fftResolution;
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz;
static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor;
static uint8_t dynamicFilterRange; static uint8_t dynamicFilterRange;
static float FAST_RAM_ZERO_INIT dynamicFilterNotchQ;
static float FAST_RAM_ZERO_INIT dynamicFilterNotch1Center;
static float FAST_RAM_ZERO_INIT dynamicFilterNotch2Center;
static uint16_t FAST_RAM_ZERO_INIT dynFilterNotchMinHz;
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window // 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 hanningWindow[FFT_WINDOW_SIZE];
@ -82,14 +77,30 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true; gyroAnalyseInitialized = true;
#endif #endif
dynamicFilterRange = gyroConfig()->dyn_filter_range; dynamicFilterRange = gyroConfig()->dyn_notch_range;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
fftBinOffset = FFT_BIN_OFFSET;
dynamicFilterNotch1Center = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynamicFilterNotch2Center = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynamicFilterNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynFilterNotchMinHz = gyroConfig()->dyn_notch_min_hz;
fftSamplingRateHz = 1000; if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) { if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
fftSamplingRateHz = 2000; fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
}
if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
fftBinOffset = 1;
}
} else {
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
fftBinOffset = 1;
}
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
} }
else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) {
fftSamplingRateHz = 1333;
} }
// If we get at least 3 samples then use the default FFT sample frequency // If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
@ -98,13 +109,8 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz); fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
fftBinOffset = FFT_BIN_OFFSET;
dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist
dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV;
dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV;
dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100;
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
@ -140,12 +146,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float
state->oversampledGyroAccumulator[axis] += sample; state->oversampledGyroAccumulator[axis] += sample;
} }
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn); static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
/* /*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/ */
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{ {
// samples should have been pushed by `gyroDataAnalysePush` // samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples // if gyro sampling is > 1kHz, accumulate multiple samples
@ -174,7 +180,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
// calculate FFT and update filters // calculate FFT and update filters
if (state->updateTicks > 0) { if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state, notchFilterDyn); gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
--state->updateTicks; --state->updateTicks;
} }
} }
@ -188,7 +194,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t
/* /*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/ */
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{ {
enum { enum {
STEP_ARM_CFFT_F32, STEP_ARM_CFFT_F32,
@ -307,15 +313,15 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
} else { } else {
centerFreq = state->prevCenterFreq[state->updateAxis]; centerFreq = state->prevCenterFreq[state->updateAxis];
} }
// constrain and low-pass smooth centre frequency centerFreq = fmax(centerFreq, dynFilterNotchMinHz);
centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
state->centerFreq[state->updateAxis] = centerFreq; state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) { if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]); DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]);
} }
if (state->updateAxis == 1) { if (state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
@ -327,11 +333,11 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
case STEP_UPDATE_FILTERS: case STEP_UPDATE_FILTERS:
{ {
// 7us // 7us
// calculate cutoffFreq and notch Q, update notch filter // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch1Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynamicFilterNotch2Center, gyro.targetLooptime, dynamicFilterNotchQ, FILTER_NOTCH);
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
@ -355,4 +361,5 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
state->updateStep = (state->updateStep + 1) % STEP_COUNT; state->updateStep = (state->updateStep + 1) % STEP_COUNT;
} }
#endif // USE_GYRO_DATA_ANALYSE #endif // USE_GYRO_DATA_ANALYSE

2
src/main/sensors/gyroanalyse.h
View File

@ -58,4 +58,4 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi
void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);