Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +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
#ifdef USE_GYRO_DATA_ANALYSE
static const char * const lookupTableDynamicFilterLocation[] = {
static const char * const lookupTableDynamicFftLocation[] = {
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
};
#endif // USE_GYRO_DATA_ANALYSE
@ -468,7 +468,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterLocation),
LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation),
#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) },
#endif
#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_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_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
// PG_ACCELEROMETER_CONFIG

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

@ -113,7 +113,7 @@ typedef enum {
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
TABLE_DYNAMIC_FILTER_LOCATION,
TABLE_DYNAMIC_FFT_LOCATION,
#endif // USE_GYRO_DATA_ANALYSE
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_width_percent = 40,
.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_ignore = 20,
);

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

@ -59,8 +59,8 @@ typedef enum {
} gyroOverflowCheck_e;
enum {
DYN_FILTER_BEFORE_STATIC_FILTERS = 0,
DYN_FILTER_AFTER_STATIC_FILTERS
DYN_FFT_BEFORE_STATIC_FILTERS = 0,
DYN_FFT_AFTER_STATIC_FILTERS
} ;
#define GYRO_CONFIG_USE_GYRO_1 0
@ -104,7 +104,7 @@ typedef struct gyroConfig_s {
uint8_t dyn_filter_type;
uint8_t dyn_filter_width_percent;
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_ignore; // ignore any FFT bin below this threshold
} gyroConfig_t;

31
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
if (isDynamicFilterActive()) {
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data in 3
}
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
}
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
}
}
float gyroDataForAnalysis = gyroADCf;
#endif
// apply static notch filters and software lowpass filters
@ -32,14 +26,17 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
if (gyroConfig()->dyn_filter_location == DYN_FILTER_AFTER_STATIC_FILTERS){
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(gyroADCf)); // store data after statics
}
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf);
if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) {
gyroDataForAnalysis = gyroADCf;
}
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis));
}
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis);
gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf);
}
#endif