Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Merge pull request #7597 from iNavFlight/dzikuvx-matrix-filter-rework

Browse source 
Matrix filter rework
This commit is contained in:
Paweł Spychalski 2021-12-23 12:41:51 +01:00 • committed by GitHub
commit c58ae7ed9d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 122 additions and 70 deletions
Download patch file Download diff file Expand all files Collapse all files

46
src/main/flight/dynamic_gyro_notch.c
View file

@ -26,6 +26,8 @@
#ifdef USE_DYNAMIC_FILTERS
FILE_COMPILE_FOR_SPEED
#include <stdint.h>
#include "dynamic_gyro_notch.h"
#include "fc/config.h"
@ -33,7 +35,12 @@
#include "sensors/gyro.h"
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
state->filtersApplyFn = nullFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
state->filtersApplyFn[axis][i] = nullFilterApply;
}
}
state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f;
state->enabled = gyroConfig()->dynamicGyroNotchEnabled;
@ -45,27 +52,32 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
*/
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
//Any initial notch Q is valid sice it will be updated immediately after
biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
biquadFilterInit(&state->filters[axis][i], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[axis][i] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) {
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]) {
state->frequency[axis] = frequency;
DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency);
if (state->enabled) {
biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
if (axis == FD_ROLL) {
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, i, frequency[i]);
}
}
if (state->enabled) {
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
// Filter update happens only if peak was detected
if (frequency[i] > 0.0f) {
biquadFilterUpdate(&state->filters[axis][i], frequency[i], state->looptime, state->dynNotchQ, FILTER_NOTCH);
}
}
}
}
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) {
@ -75,9 +87,9 @@ float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, flo
* We always apply all filters. If a filter dimension is disabled, one of
* the function pointers will be a null apply function
*/
output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output);
output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output);
output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output);
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
output = state->filtersApplyFn[axis][i]((filter_t *)&state->filters[axis][i], output);
}
return output;
}

13
src/main/flight/dynamic_gyro_notch.h
View file

@ -27,23 +27,22 @@
#include <stdint.h>
#include "common/axis.h"
#include "common/filter.h"
#include "sensors/gyro.h"
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
typedef struct dynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT];
// uint16_t frequency[XYZ_AXIS_COUNT];
float dynNotchQ;
float dynNotch1Ctr;
float dynNotch2Ctr;
uint32_t looptime;
uint8_t enabled;
/*
* Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type
*/
biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
filterApplyFnPtr filtersApplyFn;
biquadFilter_t filters[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
} dynamicGyroNotchState_t;
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state);
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency);
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]);
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input);

109
src/main/flight/gyroanalyse.c
View file

@ -61,7 +61,7 @@ enum {
// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// smoothing frequency for FFT centre frequency
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
#define DYN_NOTCH_SMOOTH_FREQ_HZ 25
/*
* Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution.
@ -93,11 +93,12 @@ void gyroDataAnalyseStateInit(
const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value
state->centerFreq[axis] = state->maxFrequency;
state->prevCenterFreq[axis] = state->maxFrequency;
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
state->centerFrequency[axis][i] = state->maxFrequency;
pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f);
}
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs);
}
}
@ -135,18 +136,6 @@ void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut
void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) {
uint8_t peakBinIndex = state->fftStartBin;
float peakValue = 0;
for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) {
if (state->fftData[i] > peakValue) {
peakValue = state->fftData[i];
peakBinIndex = i;
}
}
return peakBinIndex;
}
static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) {
float preciseBin = peakBinIndex;
@ -190,38 +179,78 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
// 8us
arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
//Find peak frequency
uint8_t peakBin = findPeakBinIndex(state);
//Zero the data structure
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
state->peaks[i].bin = 0;
state->peaks[i].value = 0.0f;
}
// Failsafe to ensure the last bin is not a peak bin
peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1);
// Find peaks
for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT - 1; bin++) {
/*
* Peak is defined if the current bin is greater than the previous bin and the next bin
*/
if (
state->fftData[bin] > state->fftData[bin - 1] &&
state->fftData[bin] > state->fftData[bin + 1]
) {
/*
* We are only interested in N biggest peaks
* Check previously found peaks and update the structure if necessary
*/
for (int p = 0; p < DYN_NOTCH_PEAK_COUNT; p++) {
if (state->fftData[bin] > state->peaks[p].value) {
for (int k = DYN_NOTCH_PEAK_COUNT - 1; k > p; k--) {
state->peaks[k] = state->peaks[k - 1];
}
state->peaks[p].bin = bin;
state->peaks[p].value = state->fftData[bin];
break;
}
}
bin++; // If bin is peak, next bin can't be peak => jump it
}
}
/*
* Calculate center frequency using the parabola method
*/
float preciseBin = computeParabolaMean(state, peakBin);
float peakFrequency = preciseBin * state->fftResolution;
// Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0)
for (int p = DYN_NOTCH_PEAK_COUNT - 1; p > 0; p--) {
for (int k = 0; k < p; k++) {
// Swap peaks but ignore swapping void peaks (bin = 0). This leaves
// void peaks at the end of peaks array without moving them
if (state->peaks[k].bin > state->peaks[k + 1].bin && state->peaks[k + 1].bin != 0) {
peak_t temp = state->peaks[k];
state->peaks[k] = state->peaks[k + 1];
state->peaks[k + 1] = temp;
}
}
}
peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency);
peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency);
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
state->centerFreq[state->updateAxis] = peakFrequency;
break;
}
case STEP_UPDATE_FILTERS_AND_HANNING:
{
// 7us
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
/*
* Filters will be updated inside dynamicGyroNotchFiltersUpdate()
*/
state->filterUpdateExecute = true;
state->filterUpdateAxis = state->updateAxis;
state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
/*
* Update frequencies
*/
for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
if (state->peaks[i].bin > 0) {
const int bin = constrain(state->peaks[i].bin, state->fftStartBin, FFT_BIN_COUNT - 1);
float frequency = computeParabolaMean(state, bin) * state->fftResolution;
state->centerFrequency[state->updateAxis][i] = pt1FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency);
} else {
state->centerFrequency[state->updateAxis][i] = 0.0f;
}
}
/*
* Filters will be updated inside dynamicGyroNotchFiltersUpdate()
*/
state->filterUpdateExecute = true;
state->filterUpdateAxis = state->updateAxis;
//Switch to the next axis
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;

15
src/main/flight/gyroanalyse.h
View file

@ -31,6 +31,11 @@
*/
#define FFT_WINDOW_SIZE 64
typedef struct peak_s {
int bin;
float value;
} peak_t;
typedef struct gyroAnalyseState_s {
// accumulator for oversampled data => no aliasing and less noise
float currentSample[XYZ_AXIS_COUNT];
@ -40,7 +45,6 @@ typedef struct gyroAnalyseState_s {
float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
// update state machine step information
uint8_t updateTicks;
uint8_t updateStep;
uint8_t updateAxis;
@ -48,12 +52,15 @@ typedef struct gyroAnalyseState_s {
float fftData[FFT_WINDOW_SIZE];
float rfftData[FFT_WINDOW_SIZE];
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
uint16_t centerFreq[XYZ_AXIS_COUNT];
uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
pt1Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
peak_t peaks[DYN_NOTCH_PEAK_COUNT];
bool filterUpdateExecute;
uint8_t filterUpdateAxis;
uint16_t filterUpdateFrequency;
uint16_t fftSamplingRateHz;
uint8_t fftStartBin;
float fftResolution;

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

@ -458,8 +458,8 @@ void FAST_CODE NOINLINE gyroFilter()
if (gyroAnalyseState.filterUpdateExecute) {
dynamicGyroNotchFiltersUpdate(
&dynamicGyroNotchState,
gyroAnalyseState.filterUpdateAxis,
gyroAnalyseState.filterUpdateFrequency
gyroAnalyseState.filterUpdateAxis,
gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
);
}
}

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

@ -24,6 +24,11 @@
#include "config/parameter_group.h"
#include "drivers/sensor.h"
/*
* Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters
*/
#define DYN_NOTCH_PEAK_COUNT 3
typedef enum {
GYRO_NONE = 0,
GYRO_AUTODETECT,