mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
Merge pull request #10767 from KarateBrot/dynamicFilter
This commit is contained in:
Michael Keller
GitHub
commit
74739ab82f
10 changed files with 96 additions and 119 deletions
|
|
@ -1422,7 +1422,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_count", "%d", gyroConfig()->dyn_notch_count);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_bandwidth_hz", "%d", gyroConfig()->dyn_notch_bandwidth_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
|
||||
#endif
|
||||
#ifdef USE_DSHOT_TELEMETRY
|
||||
|
|
|
|||
|
|
@ -662,7 +662,7 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
{ "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
|
||||
{ "dyn_notch_bandwidth_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_bandwidth_hz) },
|
||||
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
|
||||
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
|
||||
{ "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -757,7 +757,7 @@ static CMS_Menu cmsx_menuFilterGlobal = {
|
|||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
static uint16_t dynFiltNotchMaxHz;
|
||||
static uint8_t dynFiltCount;
|
||||
static uint16_t dynFiltBandwidthHz;
|
||||
static uint16_t dynFiltNotchQ;
|
||||
static uint16_t dynFiltNotchMinHz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
|
|
@ -776,7 +776,7 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
|
|||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz;
|
||||
dynFiltCount = gyroConfig()->dyn_notch_count;
|
||||
dynFiltBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
|
||||
dynFiltNotchQ = gyroConfig()->dyn_notch_q;
|
||||
dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
|
|
@ -800,7 +800,7 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
|
|||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz;
|
||||
gyroConfigMutable()->dyn_notch_count = dynFiltCount;
|
||||
gyroConfigMutable()->dyn_notch_bandwidth_hz = dynFiltBandwidthHz;
|
||||
gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
|
||||
gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
|
|
@ -822,7 +822,7 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
|
|||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
{ "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 },
|
||||
{ "NOTCH WIDTH HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltBandwidthHz, 1, 1000, 1 }, 0 },
|
||||
{ "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 1, 1000, 1 }, 0 },
|
||||
{ "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 },
|
||||
{ "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 0, 1000, 1 }, 0 },
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -35,13 +35,13 @@ static FAST_DATA_ZERO_INIT complex_t twiddle[SDFT_BIN_COUNT];
|
|||
static void applySqrt(const sdft_t *sdft, float *data);
|
||||
|
||||
|
||||
void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches)
|
||||
void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches)
|
||||
{
|
||||
if (!isInitialized) {
|
||||
rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE);
|
||||
const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
|
||||
float phi = 0.0f;
|
||||
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
|
||||
for (int i = 0; i < SDFT_BIN_COUNT; i++) {
|
||||
phi = c * i;
|
||||
twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi));
|
||||
}
|
||||
|
|
@ -54,47 +54,47 @@ void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const
|
|||
sdft->numBatches = numBatches;
|
||||
sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1;
|
||||
|
||||
for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) {
|
||||
for (int i = 0; i < SDFT_SAMPLE_SIZE; i++) {
|
||||
sdft->samples[i] = 0.0f;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
|
||||
for (int i = 0; i < SDFT_BIN_COUNT; i++) {
|
||||
sdft->data[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Add new sample to frequency spectrum
|
||||
FAST_CODE void sdftPush(sdft_t *sdft, const float *sample)
|
||||
FAST_CODE void sdftPush(sdft_t *sdft, const float sample)
|
||||
{
|
||||
const float delta = *sample - rPowerN * sdft->samples[sdft->idx];
|
||||
const float delta = sample - rPowerN * sdft->samples[sdft->idx];
|
||||
|
||||
sdft->samples[sdft->idx] = *sample;
|
||||
sdft->samples[sdft->idx] = sample;
|
||||
sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
|
||||
|
||||
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
for (int i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Add new sample to frequency spectrum in parts
|
||||
FAST_CODE void sdftPushBatch(sdft_t* sdft, const float *sample, const uint8_t *batchIdx)
|
||||
FAST_CODE void sdftPushBatch(sdft_t* sdft, const float sample, const int batchIdx)
|
||||
{
|
||||
const uint8_t batchStart = sdft->batchSize * *batchIdx;
|
||||
uint8_t batchEnd = batchStart;
|
||||
const int batchStart = sdft->batchSize * batchIdx;
|
||||
int batchEnd = batchStart;
|
||||
|
||||
const float delta = *sample - rPowerN * sdft->samples[sdft->idx];
|
||||
const float delta = sample - rPowerN * sdft->samples[sdft->idx];
|
||||
|
||||
if (*batchIdx == sdft->numBatches - 1) {
|
||||
sdft->samples[sdft->idx] = *sample;
|
||||
if (batchIdx == sdft->numBatches - 1) {
|
||||
sdft->samples[sdft->idx] = sample;
|
||||
sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
|
||||
batchEnd += sdft->endBin - batchStart + 1;
|
||||
} else {
|
||||
batchEnd += sdft->batchSize;
|
||||
}
|
||||
|
||||
for (uint8_t i = batchStart; i < batchEnd; i++) {
|
||||
for (int i = batchStart; i < batchEnd; i++) {
|
||||
sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
|
||||
}
|
||||
}
|
||||
|
|
@ -106,7 +106,7 @@ FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
|
|||
float re;
|
||||
float im;
|
||||
|
||||
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
for (int i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
re = crealf(sdft->data[i]);
|
||||
im = cimagf(sdft->data[i]);
|
||||
output[i] = re * re + im * im;
|
||||
|
|
@ -130,7 +130,7 @@ FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
|
|||
float re;
|
||||
float im;
|
||||
|
||||
for (uint8_t i = (sdft->startBin + 1); i < sdft->endBin; i++) {
|
||||
for (int i = (sdft->startBin + 1); i < sdft->endBin; i++) {
|
||||
val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication
|
||||
re = crealf(val);
|
||||
im = cimagf(val);
|
||||
|
|
@ -150,7 +150,7 @@ FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
|
|||
// Apply square root to the whole sdft range
|
||||
static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
|
||||
{
|
||||
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
for (int i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
data[i] = sqrtf(data[i]);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,21 +33,19 @@ typedef float complex complex_t; // Better readability for type "float complex"
|
|||
|
||||
typedef struct sdft_s {
|
||||
|
||||
uint8_t idx; // circular buffer index
|
||||
uint8_t startBin;
|
||||
uint8_t endBin;
|
||||
uint8_t batchSize;
|
||||
uint8_t numBatches;
|
||||
int idx; // circular buffer index
|
||||
int startBin;
|
||||
int endBin;
|
||||
int batchSize;
|
||||
int numBatches;
|
||||
float samples[SDFT_SAMPLE_SIZE]; // circular buffer
|
||||
complex_t data[SDFT_BIN_COUNT]; // complex frequency spectrum
|
||||
|
||||
} sdft_t;
|
||||
|
||||
STATIC_ASSERT(SDFT_SAMPLE_SIZE <= (uint8_t)-1, window_size_greater_than_underlying_type);
|
||||
|
||||
void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches);
|
||||
void sdftPush(sdft_t *sdft, const float *sample);
|
||||
void sdftPushBatch(sdft_t *sdft, const float *sample, const uint8_t *batchIdx);
|
||||
void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches);
|
||||
void sdftPush(sdft_t *sdft, const float sample);
|
||||
void sdftPushBatch(sdft_t *sdft, const float sample, const int batchIdx);
|
||||
void sdftMagSq(const sdft_t *sdft, float *output);
|
||||
void sdftMagnitude(const sdft_t *sdft, float *output);
|
||||
void sdftWinSq(const sdft_t *sdft, float *output);
|
||||
|
|
|
|||
|
|
@ -29,7 +29,6 @@
|
|||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -98,7 +97,7 @@ typedef enum {
|
|||
|
||||
typedef struct peak_s {
|
||||
|
||||
uint8_t bin;
|
||||
int bin;
|
||||
float value;
|
||||
|
||||
} peak_t;
|
||||
|
|
@ -106,27 +105,27 @@ typedef struct peak_s {
|
|||
static sdft_t FAST_DATA_ZERO_INIT sdft[XYZ_AXIS_COUNT];
|
||||
static peak_t FAST_DATA_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX];
|
||||
static float FAST_DATA_ZERO_INIT sdftData[SDFT_BIN_COUNT];
|
||||
static uint16_t FAST_DATA_ZERO_INIT sdftSampleRateHz;
|
||||
static float FAST_DATA_ZERO_INIT sdftSampleRateHz;
|
||||
static float FAST_DATA_ZERO_INIT sdftResolutionHz;
|
||||
static uint8_t FAST_DATA_ZERO_INIT sdftStartBin;
|
||||
static uint8_t FAST_DATA_ZERO_INIT sdftEndBin;
|
||||
static int FAST_DATA_ZERO_INIT sdftStartBin;
|
||||
static int FAST_DATA_ZERO_INIT sdftEndBin;
|
||||
static float FAST_DATA_ZERO_INIT sdftMeanSq;
|
||||
static uint16_t FAST_DATA_ZERO_INIT dynNotchBandwidthHz;
|
||||
static uint16_t FAST_DATA_ZERO_INIT dynNotchMinHz;
|
||||
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxHz;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchQ;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchMinHz;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchMaxHz;
|
||||
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT;
|
||||
static float FAST_DATA_ZERO_INIT smoothFactor;
|
||||
static uint8_t FAST_DATA_ZERO_INIT numSamples;
|
||||
static float FAST_DATA_ZERO_INIT gain;
|
||||
static int FAST_DATA_ZERO_INIT numSamples;
|
||||
|
||||
void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
|
||||
void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs)
|
||||
{
|
||||
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
|
||||
dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
|
||||
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
|
||||
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
|
||||
|
||||
// gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs)
|
||||
const int32_t targetLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
|
||||
const float targetLoopRateHz = 1.0f / targetLooptimeUs * 1e6f;
|
||||
numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00
|
||||
|
||||
sdftSampleRateHz = targetLoopRateHz / numSamples;
|
||||
|
|
@ -137,20 +136,20 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
|
|||
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) sdftSampleRateHz = 1000hz, range 500Hz
|
||||
// the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2)
|
||||
|
||||
sdftResolutionHz = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
|
||||
sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolutionHz + 0.5f)); // can't use bin 0 because it is DC.
|
||||
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotchMaxHz / sdftResolutionHz + 0.5f)); // can't use more than SDFT_BIN_COUNT bins.
|
||||
smoothFactor = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / (float)targetLoopRateHz); // minimum PT1 k value
|
||||
sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
|
||||
sdftStartBin = MAX(2, dynNotchMinHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC.
|
||||
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotchMaxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins.
|
||||
gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / targetLoopRateHz); // minimum PT1 k value
|
||||
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
|
||||
}
|
||||
|
||||
state->maxSampleCount = numSamples;
|
||||
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
|
||||
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
// any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker
|
||||
state->centerFreq[axis][p] = (p + 0.5f) * (dynNotchMaxHz - dynNotchMinHz) / (float)gyro.notchFilterDynCount + dynNotchMinHz;
|
||||
}
|
||||
|
|
@ -158,7 +157,7 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
|
|||
}
|
||||
|
||||
// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample)
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
|
||||
{
|
||||
state->oversampledGyroAccumulator[axis] += sample;
|
||||
}
|
||||
|
|
@ -174,7 +173,7 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
|
|||
state->sampleCount = 0;
|
||||
|
||||
// calculate mean value of accumulated samples
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
|
||||
state->downsampledGyroData[axis] = sample;
|
||||
if (axis == 0) {
|
||||
|
|
@ -192,8 +191,8 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
|
|||
|
||||
// 2us @ F722
|
||||
// SDFT processing in batches to synchronize with incoming downsampled data
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
sdftPushBatch(&sdft[axis], &state->downsampledGyroData[axis], &state->sampleCount);
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
sdftPushBatch(&sdft[axis], state->downsampledGyroData[axis], state->sampleCount);
|
||||
}
|
||||
state->sampleCount++;
|
||||
|
||||
|
|
@ -222,7 +221,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
|
||||
// Calculate mean square over frequency range (= average power of vibrations)
|
||||
sdftMeanSq = 0.0f;
|
||||
for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
|
||||
for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
|
||||
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq)
|
||||
}
|
||||
sdftMeanSq /= sdftEndBin - sdftStartBin - 1;
|
||||
|
|
@ -234,20 +233,20 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
case STEP_DETECT_PEAKS: // 6us @ F722
|
||||
{
|
||||
// Get memory ready for new peak data on current axis
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
peaks[p].bin = 0;
|
||||
peaks[p].value = 0.0f;
|
||||
}
|
||||
|
||||
// Search for N biggest peaks in frequency spectrum
|
||||
for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) {
|
||||
for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) {
|
||||
// Check if bin is peak
|
||||
if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) {
|
||||
// Check if peak is big enough to be one of N biggest peaks.
|
||||
// If so, insert peak and sort peaks in descending height order
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
if (sdftData[bin] > peaks[p].value) {
|
||||
for (uint8_t k = gyro.notchFilterDynCount - 1; k > p; k--) {
|
||||
for (int k = gyro.notchFilterDynCount - 1; k > p; k--) {
|
||||
peaks[k] = peaks[k - 1];
|
||||
}
|
||||
peaks[p].bin = bin;
|
||||
|
|
@ -260,8 +259,8 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
|
||||
// Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0)
|
||||
for (uint8_t p = gyro.notchFilterDynCount - 1; p > 0; p--) {
|
||||
for (uint8_t k = 0; k < p; k++) {
|
||||
for (int p = gyro.notchFilterDynCount - 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 (peaks[k].bin > peaks[k + 1].bin && peaks[k + 1].bin != 0) {
|
||||
|
|
@ -278,61 +277,44 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
case STEP_CALC_FREQUENCIES: // 4us @ F722
|
||||
{
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
|
||||
// Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
|
||||
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
|
||||
|
||||
// accumulate sdftSum and sdftWeightedSum from peak bin, and shoulder bins either side of peak
|
||||
float squaredData = peaks[p].value; // peak data already squared (see sdftWinSq)
|
||||
float sdftSum = squaredData;
|
||||
float sdftWeightedSum = squaredData * peaks[p].bin;
|
||||
float meanBin = peaks[p].bin;
|
||||
|
||||
// accumulate upper shoulder unless it would be sdftEndBin
|
||||
uint8_t shoulderBin = peaks[p].bin + 1;
|
||||
if (shoulderBin < sdftEndBin) {
|
||||
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
|
||||
sdftSum += squaredData;
|
||||
sdftWeightedSum += squaredData * shoulderBin;
|
||||
// Height of peak bin (y1) and shoulder bins (y0, y2)
|
||||
const float y0 = sdftData[peaks[p].bin - 1];
|
||||
const float y1 = sdftData[peaks[p].bin];
|
||||
const float y2 = sdftData[peaks[p].bin + 1];
|
||||
|
||||
// Estimate true peak position aka. meanBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x)
|
||||
const float denom = 2.0f * (y0 - 2 * y1 + y2);
|
||||
if (denom != 0.0f) {
|
||||
meanBin += (y0 - y2) / denom;
|
||||
}
|
||||
|
||||
// accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
|
||||
if (peaks[p].bin > 1) {
|
||||
shoulderBin = peaks[p].bin - 1;
|
||||
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
|
||||
sdftSum += squaredData;
|
||||
sdftWeightedSum += squaredData * shoulderBin;
|
||||
}
|
||||
// Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
|
||||
const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotchMinHz, dynNotchMaxHz);
|
||||
|
||||
// get centerFreq in Hz from weighted bins
|
||||
float centerFreq = dynNotchMaxHz;
|
||||
float sdftMeanBin = 0;
|
||||
// PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 8x faster
|
||||
// DYN_NOTCH_SMOOTH_HZ = 4 & gainMultiplier = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
|
||||
const float gainMultiplier = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
|
||||
|
||||
if (sdftSum > 0) {
|
||||
sdftMeanBin = (sdftWeightedSum / sdftSum);
|
||||
centerFreq = sdftMeanBin * sdftResolutionHz;
|
||||
centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
|
||||
// In theory, the index points to the centre frequency of the bin.
|
||||
// at 1333hz, bin widths are 13.3Hz, so bin 2 (26.7Hz) has the range 20Hz to 33.3Hz
|
||||
// Rav feels that maybe centerFreq = (sdftMeanBin + 0.5) * sdftResolutionHz is better
|
||||
// empirical checking shows that not adding 0.5 works better
|
||||
|
||||
// PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster
|
||||
// DYN_NOTCH_SMOOTH_HZ = 4 & dynamicFactor = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
|
||||
const float dynamicFactor = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
|
||||
state->centerFreq[state->updateAxis][p] += smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis][p]);
|
||||
}
|
||||
// Finally update notch center frequency p on current axis
|
||||
state->centerFreq[state->updateAxis][p] += gain * gainMultiplier * (centerFreq - state->centerFreq[state->updateAxis][p]);
|
||||
}
|
||||
}
|
||||
|
||||
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
|
||||
}
|
||||
}
|
||||
|
||||
if (state->updateAxis == gyro.gyroDebugAxis) {
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount && p < 3; p++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount && p < 3; p++) {
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
|
||||
}
|
||||
DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0]));
|
||||
|
|
@ -344,13 +326,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
|||
}
|
||||
case STEP_UPDATE_FILTERS: // 7us @ F722
|
||||
{
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
for (int p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
|
||||
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
|
||||
// Choose notch Q in such a way that notch bandwidth stays constant (improves prop wash handling)
|
||||
float dynamicQ = state->centerFreq[state->updateAxis][p] / (float)dynNotchBandwidthHz;
|
||||
dynamicQ = constrainf(dynamicQ, 2.0f, 10.0f);
|
||||
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynamicQ, FILTER_NOTCH);
|
||||
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
|
||||
#define DYN_NOTCH_COUNT_MAX 5
|
||||
|
||||
typedef struct gyroAnalyseState_s {
|
||||
|
|
@ -42,8 +46,8 @@ typedef struct gyroAnalyseState_s {
|
|||
|
||||
} gyroAnalyseState_t;
|
||||
|
||||
void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
|
||||
void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs);
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample);
|
||||
void gyroDataAnalyse(gyroAnalyseState_t *state);
|
||||
uint16_t getMaxFFT(void);
|
||||
void resetMaxFFT(void);
|
||||
|
|
|
|||
|
|
@ -1780,7 +1780,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent
|
||||
sbufWriteU16(dst, 0); // DEPRECATED 1.44: dyn_notch_q
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
|
|
@ -1809,10 +1809,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufWriteU8(dst, gyroConfig()->dyn_notch_count);
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_bandwidth_hz);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
|
@ -2656,7 +2654,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
|
||||
sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent
|
||||
sbufReadU16(src); // DEPRECATED 1.44: dyn_notch_q
|
||||
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
|
||||
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
|
|
@ -2680,7 +2678,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU16(src);
|
||||
#endif
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 4) {
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
// Added in MSP API 1.44
|
||||
#if defined(USE_DYN_LPF)
|
||||
currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src);
|
||||
|
|
@ -2689,10 +2687,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
gyroConfigMutable()->dyn_notch_count = sbufReadU8(src);
|
||||
gyroConfigMutable()->dyn_notch_bandwidth_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -129,8 +129,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
|||
gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT;
|
||||
gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT;
|
||||
gyroConfig->dyn_notch_max_hz = 600;
|
||||
gyroConfig->dyn_notch_count = 1;
|
||||
gyroConfig->dyn_notch_bandwidth_hz = 45;
|
||||
gyroConfig->dyn_notch_count = 3;
|
||||
gyroConfig->dyn_notch_q = 300;
|
||||
gyroConfig->dyn_notch_min_hz = 150;
|
||||
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
|
||||
gyroConfig->dyn_lpf_curve_expo = 5;
|
||||
|
|
|
|||
|
|
@ -200,7 +200,7 @@ typedef struct gyroConfig_s {
|
|||
|
||||
uint16_t dyn_notch_max_hz;
|
||||
uint8_t dyn_notch_count;
|
||||
uint16_t dyn_notch_bandwidth_hz;
|
||||
uint16_t dyn_notch_q;
|
||||
uint16_t dyn_notch_min_hz;
|
||||
|
||||
uint8_t gyro_filter_debug_axis;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue