Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

remove PT1

Browse source 
warning:
In file included from ./src/main/sensors/gyro.c:1022:0:
./src/main/sensors/gyro_filter_impl.h: In function 'filterGyro':
./src/main/sensors/gyro_filter_impl.h:18:15: warning: variable 'gyroDataForAnalysis' set but not used
This commit is contained in:
ctzsnooze 2018-08-20 11:34:13 +10:00
parent 3ddc1c5be6
commit 759007214b
6 changed files with 23 additions and 69 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -521,7 +521,6 @@ const clivalue_t valueTable[] = {
#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_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_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_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_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) },
#endif #endif

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

@ -132,8 +132,8 @@ typedef struct gyroSensor_s {
filterApplyFnPtr notchFilter2ApplyFn; filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr dynFilterApplyFn; filterApplyFnPtr notchFilterDynApplyFn;
gyroDynamicFilter_t dynFilter[XYZ_AXIS_COUNT]; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
// overflow and recovery // overflow and recovery
timeUs_t overflowTimeUs; timeUs_t overflowTimeUs;
@ -144,9 +144,8 @@ typedef struct gyroSensor_s {
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ 200 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 400 #define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 350
gyroAnalyseState_t gyroAnalyseState; gyroAnalyseState_t gyroAnalyseState;
#endif #endif
@ -207,7 +206,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0, .gyro_offset_yaw = 0,
.yaw_spin_recovery = true, .yaw_spin_recovery = true,
.yaw_spin_threshold = 1950, .yaw_spin_threshold = 1950,
.dyn_filter_type = FILTER_BIQUAD,
.dyn_filter_width_percent = 40, .dyn_filter_width_percent = 40,
.dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS, .dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS,
.dyn_filter_range = DYN_FILTER_RANGE_MEDIUM, .dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
@ -749,35 +747,15 @@ static bool isDynamicFilterActive(void)
return feature(FEATURE_DYNAMIC_FILTER); return feature(FEATURE_DYNAMIC_FILTER);
} }
static void gyroInitFilterDynamic(gyroSensor_t *gyroSensor) static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
{ {
gyroSensor->dynFilterApplyFn = nullFilterApply; gyroSensor->notchFilterDynApplyFn = nullFilterApply;
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
switch (gyroConfig()->dyn_filter_type) { gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
case FILTER_PT1: { const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
const float gyroDt = gyro.targetLooptime * 1e-6f;
const float gain = pt1FilterGain(DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ, gyroDt);
gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&gyroSensor->dynFilter[axis].pt1FilterState, gain); biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
break;
}
case FILTER_BIQUAD: {
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ);
gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->dynFilter[axis].biquadFilterState, DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
break;
}
} }
} }
} }
@ -807,7 +785,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); 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); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroInitFilterDynamic(gyroSensor); gyroInitFilterDynamicNotch(gyroSensor);
#endif #endif
} }
@ -1102,7 +1080,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->dynFilter); gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
} }
#endif #endif
} }

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

@ -29,11 +29,6 @@
#include "pg/pg.h" #include "pg/pg.h"
typedef union gyroDynamicFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
} gyroDynamicFilter_t;
typedef enum { typedef enum {
GYRO_NONE = 0, GYRO_NONE = 0,
GYRO_DEFAULT, GYRO_DEFAULT,
@ -116,7 +111,6 @@ 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_type;
uint8_t dyn_filter_width_percent; uint8_t dyn_filter_width_percent;
uint8_t dyn_fft_location; // before or after static filters uint8_t dyn_fft_location; // before or after static filters
uint8_t dyn_filter_range; // ignore any FFT bin below this threshold uint8_t dyn_filter_range; // ignore any FFT bin below this threshold

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

@ -35,8 +35,8 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis));
} }
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis); gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
} }
#endif #endif

24
src/main/sensors/gyroanalyse.c
View file

@ -67,7 +67,6 @@ 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 uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz;
static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor; static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor;
static uint8_t FAST_RAM_ZERO_INIT dynamicFilterType;
static uint8_t dynamicFilterRange; static uint8_t dynamicFilterRange;
// 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
@ -83,7 +82,6 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true; gyroAnalyseInitialized = true;
#endif #endif
dynamicFilterType = gyroConfig()->dyn_filter_type;
dynamicFilterRange = gyroConfig()->dyn_filter_range; dynamicFilterRange = gyroConfig()->dyn_filter_range;
fftSamplingRateHz = 1000; fftSamplingRateHz = 1000;
@ -142,12 +140,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, gyroDynamicFilter_t *dynFilter); static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn);
/* /*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/ */
void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
{ {
// 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
@ -176,7 +174,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter)
// calculate FFT and update filters // calculate FFT and update filters
if (state->updateTicks > 0) { if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state, dynFilter); gyroDataAnalyseUpdate(state, notchFilterDyn);
--state->updateTicks; --state->updateTicks;
} }
} }
@ -190,7 +188,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, gyroDynamicFilter_t *dynFilter) static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
{ {
enum { enum {
STEP_ARM_CFFT_F32, STEP_ARM_CFFT_F32,
@ -329,22 +327,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
case STEP_UPDATE_FILTERS: case STEP_UPDATE_FILTERS:
{ {
// 7us // 7us
switch (dynamicFilterType) {
case FILTER_PT1: {
const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor;
const float gyroDt = gyro.targetLooptime * 1e-6f;
const float gain = pt1FilterGain(cutoffFreq, gyroDt);
pt1FilterUpdateCutoff(&dynFilter[state->updateAxis].pt1FilterState, gain);
break;
}
case FILTER_BIQUAD: {
// calculate cutoffFreq and notch Q, update notch filter // calculate cutoffFreq and notch Q, update notch filter
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz);
const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
biquadFilterUpdate(&dynFilter[state->updateAxis].biquadFilterState, state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
break;
}
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);

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

@ -36,9 +36,6 @@ typedef struct gyroAnalyseState_s {
float maxSampleCountRcp; float maxSampleCountRcp;
float oversampledGyroAccumulator[XYZ_AXIS_COUNT]; float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
// filter for downsampled accumulated gyro
biquadFilter_t gyroBandpassFilter[XYZ_AXIS_COUNT];
// downsampled gyro data circular buffer for frequency analysis // downsampled gyro data circular buffer for frequency analysis
uint8_t circularBufferIdx; uint8_t circularBufferIdx;
float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
@ -61,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, gyroDynamicFilter_t *dynFilter); void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn);