mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
remove PT1
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:
parent
3ddc1c5be6
commit
759007214b
6 changed files with 23 additions and 69 deletions
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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(¬chFilterDyn[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);
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue