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

Simplified FFT location

Browse source
This commit is contained in:
Andrey Mironov 2018-08-08 11:24:16 +03:00 • committed by ctzsnooze
parent 1e960c95eb
commit 3f001295f7
5 changed files with 23 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -373,7 +373,7 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
static const char * const lookupTableDynamicFilterLocation[] = { static const char * const lookupTableDynamicFftLocation[] = {
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
}; };
#endif // USE_GYRO_DATA_ANALYSE #endif // USE_GYRO_DATA_ANALYSE
@ -468,7 +468,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterLocation), LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation),
#endif // USE_GYRO_DATA_ANALYSE #endif // USE_GYRO_DATA_ANALYSE
}; };
@ -516,12 +516,12 @@ 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_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) }, { "dyn_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) },
{ "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) },
{ "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) },
{ "dyn_filter_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_location) },
{ "dyn_filter_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) }, { "dyn_filter_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) },
{ "dyn_filter_ignore", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) }, { "dyn_filter_ignore", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) },
{ "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) },
#endif #endif
// PG_ACCELEROMETER_CONFIG // PG_ACCELEROMETER_CONFIG

2
src/main/interface/settings.h
View file

@ -113,7 +113,7 @@ typedef enum {
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
TABLE_DYNAMIC_FILTER_LOCATION, TABLE_DYNAMIC_FFT_LOCATION,
#endif // USE_GYRO_DATA_ANALYSE #endif // USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_COUNT LOOKUP_TABLE_COUNT

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

@ -210,7 +210,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.dyn_filter_type = FILTER_BIQUAD, .dyn_filter_type = FILTER_BIQUAD,
.dyn_filter_width_percent = 40, .dyn_filter_width_percent = 40,
.dyn_notch_quality = 20, .dyn_notch_quality = 20,
.dyn_filter_location = DYN_FILTER_BEFORE_STATIC_FILTERS, .dyn_fft_location = DYN_FFT_BEFORE_STATIC_FILTERS,
.dyn_filter_threshold = 30, .dyn_filter_threshold = 30,
.dyn_filter_ignore = 20, .dyn_filter_ignore = 20,
); );

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

@ -59,8 +59,8 @@ typedef enum {
} gyroOverflowCheck_e; } gyroOverflowCheck_e;
enum { enum {
DYN_FILTER_BEFORE_STATIC_FILTERS = 0, DYN_FFT_BEFORE_STATIC_FILTERS = 0,
DYN_FILTER_AFTER_STATIC_FILTERS DYN_FFT_AFTER_STATIC_FILTERS
} ; } ;
#define GYRO_CONFIG_USE_GYRO_1 0 #define GYRO_CONFIG_USE_GYRO_1 0
@ -104,7 +104,7 @@ typedef struct gyroConfig_s {
uint8_t dyn_filter_type; uint8_t dyn_filter_type;
uint8_t dyn_filter_width_percent; uint8_t dyn_filter_width_percent;
uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass
uint8_t dyn_filter_location; // before or after static filters uint8_t dyn_fft_location; // before or after static filters
uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak
uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold
} gyroConfig_t; } gyroConfig_t;

27
src/main/sensors/gyro_filter_impl.h
View file

@ -10,18 +10,12 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data in 3 GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
}
if (gyroConfig()->dyn_filter_location == DYN_FILTER_BEFORE_STATIC_FILTERS) {
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf);
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-notch data in 2
}
} }
} }
float gyroDataForAnalysis = gyroADCf;
#endif #endif
// apply static notch filters and software lowpass filters // apply static notch filters and software lowpass filters
@ -32,15 +26,18 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
if (gyroConfig()->dyn_filter_location == DYN_FILTER_AFTER_STATIC_FILTERS){ if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) {
gyroDataForAnalysis = gyroADCf;
}
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-static data in debug 2 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after statics GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis));
} }
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis);
gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf);
} }
}
#endif #endif
// DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied. // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.