Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +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

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

@ -132,8 +132,8 @@ typedef struct gyroSensor_s {
filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr dynFilterApplyFn;
gyroDynamicFilter_t dynFilter[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
// overflow and recovery
timeUs_t overflowTimeUs;
@ -144,9 +144,8 @@ typedef struct gyroSensor_s {
#endif // USE_YAW_SPIN_RECOVERY
#ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ 200
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 400
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
gyroAnalyseState_t gyroAnalyseState;
#endif
@ -207,7 +206,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
.dyn_filter_type = FILTER_BIQUAD,
.dyn_filter_width_percent = 40,
.dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS,
.dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
@ -749,35 +747,15 @@ static bool isDynamicFilterActive(void)
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()) {
switch (gyroConfig()->dyn_filter_type) {
case FILTER_PT1: {
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++) {
pt1FilterInit(&gyroSensor->dynFilter[axis].pt1FilterState, gain);
}
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;
}
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
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++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
}
@ -807,7 +785,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
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);
#ifdef USE_GYRO_DATA_ANALYSE
gyroInitFilterDynamic(gyroSensor);
gyroInitFilterDynamicNotch(gyroSensor);
#endif
}
@ -1102,7 +1080,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->dynFilter);
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
}
#endif
}