Merge pull request #10554 from KarateBrot/sdft
This commit is contained in:
commit
dc76125e69
|
@ -226,6 +226,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
common/encoding.c \
|
||||
common/filter.c \
|
||||
common/maths.c \
|
||||
common/sdft.c \
|
||||
common/typeconversion.c \
|
||||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/accgyro/accgyro_mpu3050.c \
|
||||
|
@ -397,16 +398,6 @@ ifneq ($(DSP_LIB),)
|
|||
|
||||
INCLUDE_DIRS += $(DSP_LIB)/Include
|
||||
|
||||
SRC += $(DSP_LIB)/Source/BasicMathFunctions/arm_mult_f32.c
|
||||
SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
|
||||
SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
|
||||
SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
|
||||
SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
|
||||
SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
|
||||
|
||||
SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
|
||||
SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
|
||||
|
||||
SRC += $(wildcard $(DSP_LIB)/Source/*/*.S)
|
||||
endif
|
||||
|
||||
|
|
|
@ -1419,8 +1419,8 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("gyro_to_use", "%d", gyroConfig()->gyro_to_use);
|
||||
#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_width_percent", "%d", gyroConfig()->dyn_notch_width_percent);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
|
||||
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_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
|
||||
#endif
|
||||
#ifdef USE_DSHOT_TELEMETRY
|
||||
|
|
|
@ -679,8 +679,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
{ "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
|
||||
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
|
||||
{ "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_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
|
||||
|
|
|
@ -25,6 +25,7 @@
|
|||
#include <ctype.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
|
@ -752,8 +752,8 @@ static CMS_Menu cmsx_menuFilterGlobal = {
|
|||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
static uint16_t dynFiltNotchMaxHz;
|
||||
static uint8_t dynFiltWidthPercent;
|
||||
static uint16_t dynFiltNotchQ;
|
||||
static uint8_t dynFiltCount;
|
||||
static uint16_t dynFiltBandwidthHz;
|
||||
static uint16_t dynFiltNotchMinHz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
|
@ -771,8 +771,8 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
|
|||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz;
|
||||
dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent;
|
||||
dynFiltNotchQ = gyroConfig()->dyn_notch_q;
|
||||
dynFiltCount = gyroConfig()->dyn_notch_count;
|
||||
dynFiltBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
|
||||
dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
|
@ -795,8 +795,8 @@ 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_width_percent = dynFiltWidthPercent;
|
||||
gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
|
||||
gyroConfigMutable()->dyn_notch_count = dynFiltCount;
|
||||
gyroConfigMutable()->dyn_notch_bandwidth_hz = dynFiltBandwidthHz;
|
||||
gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
|
@ -817,8 +817,8 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
|
|||
{ "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
{ "NOTCH WIDTH %", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltWidthPercent, 0, 20, 1 }, 0 },
|
||||
{ "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 0, 1000, 1 }, 0 },
|
||||
{ "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 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
|
||||
|
|
|
@ -29,8 +29,6 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#define M_LN2_FLOAT 0.69314718055994530942f
|
||||
#define M_PI_FLOAT 3.14159265358979323846f
|
||||
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/
|
||||
|
||||
// NULL filter
|
||||
|
@ -46,7 +44,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input)
|
|||
|
||||
float pt1FilterGain(float f_cut, float dT)
|
||||
{
|
||||
float RC = 1 / ( 2 * M_PI_FLOAT * f_cut);
|
||||
float RC = 1 / ( 2 * M_PIf * f_cut);
|
||||
return dT / (RC + dT);
|
||||
}
|
||||
|
||||
|
@ -107,7 +105,7 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
|
|||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
|
||||
{
|
||||
// setup variables
|
||||
const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
|
||||
const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
|
||||
const float sn = sin_approx(omega);
|
||||
const float cs = cos_approx(omega);
|
||||
const float alpha = sn / (2.0f * Q);
|
||||
|
|
|
@ -33,6 +33,9 @@
|
|||
|
||||
// Use floating point M_PI instead explicitly.
|
||||
#define M_PIf 3.14159265358979323846f
|
||||
#define M_EULERf 2.71828182845904523536f
|
||||
#define M_SQRT2f 1.41421356237309504880f
|
||||
#define M_LN2f 0.69314718055994530942f
|
||||
|
||||
#define RAD (M_PIf / 180.0f)
|
||||
#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
|
||||
|
|
|
@ -0,0 +1,156 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/sdft.h"
|
||||
|
||||
#define SDFT_R 0.9999f // damping factor for guaranteed SDFT stability (r < 1.0f)
|
||||
|
||||
static FAST_DATA_ZERO_INIT float rPowerN; // SDFT_R to the power of SDFT_SAMPLE_SIZE
|
||||
static FAST_DATA_ZERO_INIT bool isInitialized;
|
||||
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)
|
||||
{
|
||||
if (!isInitialized) {
|
||||
rPowerN = powerf(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++) {
|
||||
phi = c * i;
|
||||
twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi));
|
||||
}
|
||||
isInitialized = true;
|
||||
}
|
||||
|
||||
sdft->idx = 0;
|
||||
sdft->startBin = startBin;
|
||||
sdft->endBin = endBin;
|
||||
sdft->numBatches = numBatches;
|
||||
sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1;
|
||||
|
||||
for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) {
|
||||
sdft->samples[i] = 0.0f;
|
||||
}
|
||||
|
||||
for (uint8_t 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)
|
||||
{
|
||||
const float delta = *sample - rPowerN * sdft->samples[sdft->idx];
|
||||
|
||||
sdft->samples[sdft->idx] = *sample;
|
||||
sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
|
||||
|
||||
for (uint8_t 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)
|
||||
{
|
||||
const uint8_t batchStart = sdft->batchSize * *batchIdx;
|
||||
uint8_t batchEnd = batchStart;
|
||||
|
||||
const float delta = *sample - rPowerN * sdft->samples[sdft->idx];
|
||||
|
||||
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++) {
|
||||
sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Get squared magnitude of frequency spectrum
|
||||
FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
|
||||
{
|
||||
float re;
|
||||
float im;
|
||||
|
||||
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
|
||||
re = crealf(sdft->data[i]);
|
||||
im = cimagf(sdft->data[i]);
|
||||
output[i] = re * re + im * im;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Get magnitude of frequency spectrum (slower)
|
||||
FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output)
|
||||
{
|
||||
sdftMagSq(sdft, output);
|
||||
applySqrt(sdft, output);
|
||||
}
|
||||
|
||||
|
||||
// Get squared magnitude of frequency spectrum with Hann window applied
|
||||
// Hann window in frequency domain: X[k] = -0.25 * X[k-1] +0.5 * X[k] -0.25 * X[k+1]
|
||||
FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
|
||||
{
|
||||
complex_t val;
|
||||
float re;
|
||||
float im;
|
||||
|
||||
for (uint8_t 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);
|
||||
output[i] = re * re + im * im;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Get magnitude of frequency spectrum with Hann window applied (slower)
|
||||
FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
|
||||
{
|
||||
sdftWinSq(sdft, output);
|
||||
applySqrt(sdft, 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++) {
|
||||
data[i] = sqrtf(data[i]);
|
||||
}
|
||||
}
|
|
@ -0,0 +1,54 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// Implementation of a Sliding Discrete Fourier Transform (SDFT).
|
||||
// Complexity for calculating frequency spectrum with N bins is O(N).
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <complex.h>
|
||||
#undef I // avoid collision of imaginary unit I with variable I in pid.h
|
||||
typedef float complex complex_t; // Better readability for type "float complex"
|
||||
|
||||
#define SDFT_SAMPLE_SIZE 72
|
||||
#define SDFT_BIN_COUNT (SDFT_SAMPLE_SIZE / 2)
|
||||
|
||||
typedef struct sdft_s {
|
||||
|
||||
uint8_t idx; // circular buffer index
|
||||
uint8_t startBin;
|
||||
uint8_t endBin;
|
||||
uint8_t batchSize;
|
||||
uint8_t 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 sdftMagSq(const sdft_t *sdft, float *output);
|
||||
void sdftMagnitude(const sdft_t *sdft, float *output);
|
||||
void sdftWinSq(const sdft_t *sdft, float *output);
|
||||
void sdftWindow(const sdft_t *sdft, float *output);
|
|
@ -19,10 +19,16 @@
|
|||
*/
|
||||
|
||||
/* original work by Rav
|
||||
*
|
||||
* 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
|
||||
* coding assistance and advice from DieHertz, Rav, eTracer
|
||||
* test pilots icr4sh, UAV Tech, Flint723
|
||||
*
|
||||
* 2021_02 updated by KarateBrot: switched FFT with SDFT, multiple notches per axis
|
||||
* test pilots: Sugar K, bizmar
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
@ -32,6 +38,7 @@
|
|||
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/sdft.h"
|
||||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
|
@ -44,62 +51,72 @@
|
|||
|
||||
#include "gyroanalyse.h"
|
||||
|
||||
// FFT_WINDOW_SIZE defaults to 32 (gyroanalyse.h)
|
||||
// We get 16 frequency bins from 32 consecutive data values
|
||||
// SDFT_SAMPLE_SIZE defaults to 72 (common/sdft.h).
|
||||
// We get 36 frequency bins from 72 consecutive data values, called SDFT_BIN_COUNT (common/sdft.h)
|
||||
// Bin 0 is DC and can't be used.
|
||||
// Only bins 1 to 15 are usable.
|
||||
// Only bins 1 to 35 are usable.
|
||||
|
||||
// A gyro sample is collected every gyro loop
|
||||
// A gyro sample is collected every PID loop.
|
||||
// maxSampleCount recent gyro values are accumulated and averaged
|
||||
// to ensure that 32 samples are collected at the right rate for the required FFT bandwidth
|
||||
// to ensure that 72 samples are collected at the right rate for the required SDFT bandwidth.
|
||||
|
||||
// For an 8k gyro loop, at default 600hz max, 6 sequential gyro data points are averaged, FFT runs 1333Hz.
|
||||
// Upper limit of FFT is half that frequency, eg 666Hz by default.
|
||||
// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, fftSamplingRateHz = 615Hz, range 307Hz
|
||||
// For an 8k PID loop, at default 600hz max, 6 sequential gyro data points are averaged, SDFT runs 1333Hz.
|
||||
// Upper limit of SDFT is half that frequency, eg 666Hz by default.
|
||||
// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, sdftSampleRateHz = 615Hz, range 307Hz.
|
||||
// Note that lower max requires more samples to be averaged, increasing precision but taking longer to get enough samples.
|
||||
// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, fftSamplingRateHz = 1600, range to 800hz
|
||||
// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, fftSamplingRateHz = 640, range to 320Hz
|
||||
//
|
||||
// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the circular buffer of 32 samples
|
||||
// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms
|
||||
// Hence to completely replace all 32 samples of the FFT input buffer with clean new data takes 24ms
|
||||
// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, sdftSampleRateHz = 1600, range to 800hz.
|
||||
// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, sdftSampleRateHz = 640, range to 320Hz.
|
||||
|
||||
// The FFT code is split into steps. It takes 4 gyro loops to calculate the FFT for one axis
|
||||
// (gyroDataAnalyseUpdate has 8 steps, but only four breaks)
|
||||
// Since there are three axes, it takes 12 gyro loops to completely update all axes.
|
||||
// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the corresponding SDFT.
|
||||
// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms.
|
||||
// Hence to completely replace all 72 samples of the SDFT input buffer with clean new data takes 54ms.
|
||||
|
||||
// The SDFT code is split into steps. It takes 4 PID loops to calculate the SDFT, track peaks and update the filters for one axis.
|
||||
// Since there are three axes, it takes 12 PID loops to completely update all axes.
|
||||
// At 8k, any one axis gets updated at 8000 / 12 or 666hz or every 1.5ms
|
||||
// In this time, 2 points in the FFT buffer will have changed.
|
||||
// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms
|
||||
// In this time, 2 points in the SDFT buffer will have changed.
|
||||
// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms.
|
||||
// Four points in the buffer will have changed in that time, and each point will be the average of three samples.
|
||||
// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad.
|
||||
|
||||
// The Hanning window step loads gyro data (32 data points) for one axis from the circular buffer into fftData[i]
|
||||
// and applies the hanning window to the edge values
|
||||
// Calculation steps 1 and 2 then calculate the fft output (32 data points) and put that back into the same fftData[i] array.
|
||||
// We then use fftData[i] array for frequency centre calculations for that axis
|
||||
|
||||
// Each FFT output bin has width fftSamplingRateHz/32, ie 41.65Hz per bin at 1333Hz
|
||||
// Usable bandwidth is half this, ie 666Hz if fftSamplingRateHz is 1333Hz, i.e. bin 1 is 41.65hz, bin 2 83.3hz etc
|
||||
// Each SDFT output bin has width sdftSampleRateHz/72, ie 18.5Hz per bin at 1333Hz.
|
||||
// Usable bandwidth is half this, ie 666Hz if sdftSampleRateHz is 1333Hz, i.e. bin 1 is 18.5Hz, bin 2 is 37.0Hz etc.
|
||||
|
||||
#define DYN_NOTCH_SMOOTH_HZ 4
|
||||
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // 16
|
||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // 4 steps per axis
|
||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis
|
||||
#define DYN_NOTCH_OSD_MIN_THROTTLE 20
|
||||
|
||||
static uint16_t FAST_DATA_ZERO_INIT fftSamplingRateHz;
|
||||
static float FAST_DATA_ZERO_INIT fftResolution;
|
||||
static uint8_t FAST_DATA_ZERO_INIT fftStartBin;
|
||||
static float FAST_DATA_ZERO_INIT dynNotchQ;
|
||||
static float FAST_DATA_ZERO_INIT dynNotch1Ctr;
|
||||
static float FAST_DATA_ZERO_INIT dynNotch2Ctr;
|
||||
typedef enum {
|
||||
|
||||
STEP_WINDOW,
|
||||
STEP_DETECT_PEAKS,
|
||||
STEP_CALC_FREQUENCIES,
|
||||
STEP_UPDATE_FILTERS,
|
||||
STEP_COUNT
|
||||
|
||||
} step_e;
|
||||
|
||||
typedef struct peak_s {
|
||||
|
||||
uint8_t bin;
|
||||
float value;
|
||||
|
||||
} peak_t;
|
||||
|
||||
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 sdftResolutionHz;
|
||||
static uint8_t FAST_DATA_ZERO_INIT sdftStartBin;
|
||||
static uint8_t 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 bool FAST_DATA dualNotch = true;
|
||||
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT;
|
||||
static float FAST_DATA_ZERO_INIT smoothFactor;
|
||||
static uint8_t FAST_DATA_ZERO_INIT samples;
|
||||
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
|
||||
static FAST_DATA_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
|
||||
static uint8_t FAST_DATA_ZERO_INIT numSamples;
|
||||
|
||||
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
||||
{
|
||||
|
@ -111,33 +128,29 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
|||
gyroAnalyseInitialized = true;
|
||||
#endif
|
||||
|
||||
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
|
||||
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
|
||||
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
|
||||
dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
|
||||
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
|
||||
|
||||
if (gyroConfig()->dyn_notch_width_percent == 0) {
|
||||
dualNotch = false;
|
||||
}
|
||||
// gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs)
|
||||
const int32_t targetLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
|
||||
numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00
|
||||
|
||||
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
|
||||
samples = MAX(1, gyroLoopRateHz / (2 * dynNotchMaxHz)); //600hz, 8k looptime, 13.333
|
||||
sdftSampleRateHz = targetLoopRateHz / numSamples;
|
||||
// eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), sdftSampleRateHz = 1333hz, range 666Hz
|
||||
// eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), sdftSampleRateHz = 1333hz, range 666Hz
|
||||
// eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) sdftSampleRateHz = 2000hz, range 1000Hz
|
||||
// eg 2k, user max 400hz, int(2000/800) = 2 (2.5) sdftSampleRateHz = 1000hz, range 500Hz
|
||||
// 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)
|
||||
|
||||
fftSamplingRateHz = gyroLoopRateHz / samples;
|
||||
// eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), fftSamplingRateHz = 1333hz, range 666Hz
|
||||
// eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), fftSamplingRateHz = 1333hz, range 666Hz
|
||||
// eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) fftSamplingRateHz = 2000hz, range 1000Hz
|
||||
// eg 2k, user max 400hz, int(2000/800) = 2 (2.5) fftSamplingRateHz = 1000hz, range 500Hz
|
||||
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) fftSamplingRateHz = 1000hz, range 500Hz
|
||||
// the upper limit of DN is always going to be Nyquist
|
||||
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
|
||||
|
||||
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; // 41.65hz per bin for medium
|
||||
fftStartBin = MAX(2, dynNotchMinHz / lrintf(fftResolution)); // can't use bin 0 because it is DC.
|
||||
smoothFactor = 2 * M_PIf * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); // minimum PT1 k value
|
||||
|
||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
||||
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -145,254 +158,217 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
|
|||
{
|
||||
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
|
||||
gyroDataAnalyseInit(targetLooptimeUs);
|
||||
state->maxSampleCount = samples;
|
||||
state->maxSampleCount = numSamples;
|
||||
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
|
||||
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
// any init value
|
||||
state->centerFreq[axis] = dynNotchMaxHz;
|
||||
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (uint8_t 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
|
||||
// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample)
|
||||
{
|
||||
state->oversampledGyroAccumulator[axis] += sample;
|
||||
}
|
||||
|
||||
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
|
||||
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
|
||||
|
||||
/*
|
||||
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
|
||||
*/
|
||||
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
|
||||
// Downsample and analyse gyro data
|
||||
FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
|
||||
{
|
||||
// samples should have been pushed by `gyroDataAnalysePush`
|
||||
// if gyro sampling is > 1kHz, accumulate and average multiple gyro samples
|
||||
state->sampleCount++;
|
||||
|
||||
if (state->sampleCount == state->maxSampleCount) {
|
||||
state->sampleCount = 0;
|
||||
|
||||
// calculate mean value of accumulated samples
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
|
||||
state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
|
||||
state->downsampledGyroData[axis] = sample;
|
||||
if (axis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
|
||||
}
|
||||
|
||||
state->oversampledGyroAccumulator[axis] = 0;
|
||||
}
|
||||
|
||||
state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
|
||||
|
||||
// We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
|
||||
// We need DYN_NOTCH_CALC_TICKS ticks to update all axes with newly sampled value
|
||||
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
|
||||
// at 4kHz gyro loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms
|
||||
// at 4kHz gyro loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms
|
||||
// at 8kHz PID loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms
|
||||
// at 4kHz PID loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms
|
||||
state->updateTicks = DYN_NOTCH_CALC_TICKS;
|
||||
}
|
||||
|
||||
// calculate FFT and update filters
|
||||
// 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);
|
||||
}
|
||||
state->sampleCount++;
|
||||
|
||||
// Find frequency peaks and update filters
|
||||
if (state->updateTicks > 0) {
|
||||
gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
|
||||
gyroDataAnalyseUpdate(state);
|
||||
--state->updateTicks;
|
||||
}
|
||||
}
|
||||
|
||||
void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
|
||||
void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1);
|
||||
void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
|
||||
void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier);
|
||||
void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
|
||||
|
||||
/*
|
||||
* Analyse gyro data
|
||||
*/
|
||||
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
|
||||
// Find frequency peaks and update filters
|
||||
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
|
||||
{
|
||||
enum {
|
||||
STEP_ARM_CFFT_F32,
|
||||
STEP_BITREVERSAL,
|
||||
STEP_STAGE_RFFT_F32,
|
||||
STEP_ARM_CMPLX_MAG_F32,
|
||||
STEP_CALC_FREQUENCIES,
|
||||
STEP_UPDATE_FILTERS,
|
||||
STEP_HANNING,
|
||||
STEP_COUNT
|
||||
};
|
||||
|
||||
arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
|
||||
|
||||
uint32_t startTime = 0;
|
||||
if (debugMode == (DEBUG_FFT_TIME)) {
|
||||
startTime = micros();
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
|
||||
|
||||
switch (state->updateStep) {
|
||||
case STEP_ARM_CFFT_F32:
|
||||
|
||||
case STEP_WINDOW: // 6us @ F722
|
||||
{
|
||||
switch (FFT_BIN_COUNT) {
|
||||
case 16:
|
||||
// 16us
|
||||
arm_cfft_radix8by2_f32(Sint, state->fftData);
|
||||
break;
|
||||
case 32:
|
||||
// 35us
|
||||
arm_cfft_radix8by4_f32(Sint, state->fftData);
|
||||
break;
|
||||
case 64:
|
||||
// 70us
|
||||
arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
|
||||
break;
|
||||
sdftWinSq(&sdft[state->updateAxis], sdftData);
|
||||
|
||||
// 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
|
||||
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq)
|
||||
}
|
||||
sdftMeanSq /= sdftEndBin - sdftStartBin - 1;
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
break;
|
||||
}
|
||||
case STEP_BITREVERSAL:
|
||||
case STEP_DETECT_PEAKS: // 6us @ F722
|
||||
{
|
||||
// 6us
|
||||
arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
state->updateStep++;
|
||||
FALLTHROUGH;
|
||||
// Get memory ready for new peak data on current axis
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
peaks[p].bin = 0;
|
||||
peaks[p].value = 0.0f;
|
||||
}
|
||||
case STEP_STAGE_RFFT_F32:
|
||||
{
|
||||
// 14us
|
||||
// this does not work in place => fftData AND rfftData needed
|
||||
stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
|
||||
|
||||
// Search for N biggest peaks in frequency spectrum
|
||||
for (uint8_t 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++) {
|
||||
if (sdftData[bin] > peaks[p].value) {
|
||||
for (uint8_t k = gyro.notchFilterDynCount - 1; k > p; k--) {
|
||||
peaks[k] = peaks[k - 1];
|
||||
}
|
||||
peaks[p].bin = bin;
|
||||
peaks[p].value = sdftData[bin];
|
||||
break;
|
||||
}
|
||||
}
|
||||
bin++; // If bin is peak, next bin can't be peak => jump it
|
||||
}
|
||||
}
|
||||
|
||||
// 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++) {
|
||||
// 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) {
|
||||
peak_t temp = peaks[k];
|
||||
peaks[k] = peaks[k + 1];
|
||||
peaks[k + 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
break;
|
||||
}
|
||||
case STEP_ARM_CMPLX_MAG_F32:
|
||||
case STEP_CALC_FREQUENCIES: // 4us @ F722
|
||||
{
|
||||
// 8us
|
||||
arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
|
||||
state->updateStep++;
|
||||
FALLTHROUGH;
|
||||
}
|
||||
case STEP_CALC_FREQUENCIES:
|
||||
{
|
||||
// identify max bin and max/min heights
|
||||
float dataMax = 0.0f;
|
||||
float dataMin = 1.0f;
|
||||
uint8_t binMax = 0;
|
||||
float dataMinHi = 1.0f;
|
||||
for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
|
||||
if (state->fftData[i] > state->fftData[i - 1]) { // bin height increased
|
||||
if (state->fftData[i] > dataMax) {
|
||||
dataMax = state->fftData[i];
|
||||
binMax = i; // tallest bin so far
|
||||
}
|
||||
}
|
||||
}
|
||||
if (binMax == 0) { // no bin increase, hold prev max bin, dataMin = 1 dataMax = 0, ie move slow
|
||||
binMax = lrintf(state->centerFreq[state->updateAxis] / fftResolution);
|
||||
} else { // there was a max, find min
|
||||
for (int i = binMax - 1; i > 1; i--) { // look for min below max
|
||||
dataMin = state->fftData[i];
|
||||
if (state->fftData[i - 1] > state->fftData[i]) { // up step below this one
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int i = binMax + 1; i < (FFT_BIN_COUNT - 1); i++) { // // look for min above max
|
||||
dataMinHi = state->fftData[i];
|
||||
if (state->fftData[i] < state->fftData[i + 1]) { // up step above this one
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
dataMin = fminf(dataMin, dataMinHi);
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
|
||||
// accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak
|
||||
float squaredData = state->fftData[binMax] * state->fftData[binMax];
|
||||
float fftSum = squaredData;
|
||||
float fftWeightedSum = squaredData * binMax;
|
||||
// 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 upper shoulder unless it would be FFT_BIN_COUNT
|
||||
uint8_t shoulderBin = binMax + 1;
|
||||
if (shoulderBin < FFT_BIN_COUNT) {
|
||||
squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin];
|
||||
fftSum += squaredData;
|
||||
fftWeightedSum += squaredData * shoulderBin;
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
|
||||
if (binMax > 1) {
|
||||
shoulderBin = binMax - 1;
|
||||
squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin];
|
||||
fftSum += squaredData;
|
||||
fftWeightedSum += squaredData * shoulderBin;
|
||||
if (peaks[p].bin > 1) {
|
||||
shoulderBin = peaks[p].bin - 1;
|
||||
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
|
||||
sdftSum += squaredData;
|
||||
sdftWeightedSum += squaredData * shoulderBin;
|
||||
}
|
||||
|
||||
// get centerFreq in Hz from weighted bins
|
||||
float centerFreq = dynNotchMaxHz;
|
||||
float fftMeanIndex = 0;
|
||||
if (fftSum > 0) {
|
||||
fftMeanIndex = (fftWeightedSum / fftSum);
|
||||
centerFreq = fftMeanIndex * fftResolution;
|
||||
// In theory, the index points to the centre frequency of the bin.
|
||||
// at 1333hz, bin widths are 41.65Hz, so bin 2 has the range 83,3Hz to 124,95Hz
|
||||
// Rav feels that maybe centerFreq = (fftMeanIndex + 0.5) * fftResolution; is better
|
||||
// empirical checking shows that not adding 0.5 works better
|
||||
} else {
|
||||
centerFreq = state->centerFreq[state->updateAxis];
|
||||
}
|
||||
float sdftMeanBin = 0;
|
||||
|
||||
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
|
||||
float dynamicFactor = constrainf(dataMax / dataMin, 1.0f, 8.0f);
|
||||
state->centerFreq[state->updateAxis] = state->centerFreq[state->updateAxis] + smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis]);
|
||||
// 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]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
|
||||
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
|
||||
}
|
||||
}
|
||||
|
||||
if (state->updateAxis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, 1, lrintf(dynamicFactor * 100));
|
||||
DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]);
|
||||
if (state->updateAxis == gyro.gyroDebugAxis) {
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount && p < 3; p++) {
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
|
||||
}
|
||||
// if (state->updateAxis == 1) {
|
||||
// DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
|
||||
// }
|
||||
DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0]));
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
break;
|
||||
}
|
||||
case STEP_UPDATE_FILTERS:
|
||||
case STEP_UPDATE_FILTERS: // 7us @ F722
|
||||
{
|
||||
// 7us
|
||||
// calculate cutoffFreq and notch Q, update notch filter
|
||||
if (dualNotch) {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
} else {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
for (uint8_t 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);
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
|
||||
state->updateStep++;
|
||||
FALLTHROUGH;
|
||||
}
|
||||
case STEP_HANNING:
|
||||
{
|
||||
// 5us
|
||||
// apply hanning window to gyro samples and store result in fftData[i] to be used in step 1 and 2 and 3
|
||||
const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
|
||||
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
|
||||
if (state->circularBufferIdx > 0) {
|
||||
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -20,40 +20,30 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "arm_math.h"
|
||||
|
||||
#include "common/filter.h"
|
||||
|
||||
#define FFT_WINDOW_SIZE 32
|
||||
#define DYN_NOTCH_COUNT_MAX 5
|
||||
|
||||
typedef struct gyroAnalyseState_s {
|
||||
|
||||
// accumulator for oversampled data => no aliasing and less noise
|
||||
uint8_t sampleCount;
|
||||
uint8_t maxSampleCount;
|
||||
float maxSampleCountRcp;
|
||||
float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
|
||||
|
||||
// downsampled gyro data circular buffer for frequency analysis
|
||||
uint8_t circularBufferIdx;
|
||||
float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
|
||||
// downsampled gyro data for frequency analysis
|
||||
float downsampledGyroData[XYZ_AXIS_COUNT];
|
||||
|
||||
// update state machine step information
|
||||
uint8_t updateTicks;
|
||||
uint8_t updateStep;
|
||||
uint8_t updateAxis;
|
||||
|
||||
arm_rfft_fast_instance_f32 fftInstance;
|
||||
float fftData[FFT_WINDOW_SIZE];
|
||||
float rfftData[FFT_WINDOW_SIZE];
|
||||
|
||||
float centerFreq[XYZ_AXIS_COUNT];
|
||||
float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
|
||||
|
||||
} gyroAnalyseState_t;
|
||||
|
||||
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
|
||||
|
||||
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample);
|
||||
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
|
||||
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
|
||||
void gyroDataAnalyse(gyroAnalyseState_t *state);
|
||||
uint16_t getMaxFFT(void);
|
||||
void resetMaxFFT(void);
|
||||
|
|
|
@ -1771,8 +1771,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
// Added in MSP API 1.42
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
|
||||
sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent
|
||||
sbufWriteU16(dst, 0); // DEPRECATED 1.44: dyn_notch_q
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
|
||||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
|
@ -1799,6 +1799,13 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
#else
|
||||
sbufWriteU8(dst, 0);
|
||||
#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;
|
||||
case MSP_PID_ADVANCED:
|
||||
|
@ -2635,9 +2642,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
if (sbufBytesRemaining(src) >= 8) {
|
||||
// Added in MSP API 1.42
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufReadU8(src); // DEPRECATED: dyn_notch_range
|
||||
gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src);
|
||||
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
|
||||
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_min_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
|
@ -2661,12 +2668,19 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU16(src);
|
||||
#endif
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 1) {
|
||||
if (sbufBytesRemaining(src) >= 4) {
|
||||
// Added in MSP API 1.44
|
||||
#if defined(USE_DYN_LPF)
|
||||
currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#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
|
||||
}
|
||||
|
||||
|
|
|
@ -98,7 +98,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
|
|||
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
|
||||
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
|
||||
|
||||
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
|
||||
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
|
||||
|
||||
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
|
||||
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
|
||||
|
@ -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_width_percent = 8;
|
||||
gyroConfig->dyn_notch_q = 120;
|
||||
gyroConfig->dyn_notch_count = 1;
|
||||
gyroConfig->dyn_notch_bandwidth_hz = 45;
|
||||
gyroConfig->dyn_notch_min_hz = 150;
|
||||
gyroConfig->gyro_filter_debug_axis = FD_ROLL;
|
||||
gyroConfig->dyn_lpf_curve_expo = 5;
|
||||
|
@ -484,7 +484,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
|
|||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
gyroDataAnalyse(&gyro.gyroAnalyseState, gyro.notchFilterDyn, gyro.notchFilterDyn2);
|
||||
gyroDataAnalyse(&gyro.gyroAnalyseState);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -107,12 +107,11 @@ typedef struct gyro_s {
|
|||
filterApplyFnPtr notchFilter2ApplyFn;
|
||||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||
|
||||
filterApplyFnPtr notchFilterDynApplyFn;
|
||||
filterApplyFnPtr notchFilterDynApplyFn2;
|
||||
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
|
||||
biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
filterApplyFnPtr notchFilterDynApplyFn;
|
||||
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
|
||||
uint8_t notchFilterDynCount;
|
||||
|
||||
gyroAnalyseState_t gyroAnalyseState;
|
||||
#endif
|
||||
|
||||
|
@ -196,8 +195,8 @@ typedef struct gyroConfig_s {
|
|||
uint16_t dyn_lpf_gyro_max_hz;
|
||||
|
||||
uint16_t dyn_notch_max_hz;
|
||||
uint8_t dyn_notch_width_percent;
|
||||
uint16_t dyn_notch_q;
|
||||
uint8_t dyn_notch_count;
|
||||
uint16_t dyn_notch_bandwidth_hz;
|
||||
uint16_t dyn_notch_min_hz;
|
||||
|
||||
uint8_t gyro_filter_debug_axis;
|
||||
|
|
|
@ -49,16 +49,6 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
|||
// DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
|
||||
GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
if (axis == gyro.gyroDebugAxis) {
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RPM_FILTER
|
||||
gyroADCf = rpmFilterGyro(axis, gyroADCf);
|
||||
#endif
|
||||
|
@ -76,14 +66,21 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
|
|||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
if (isDynamicFilterActive()) {
|
||||
if (axis == gyro.gyroDebugAxis) {
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
|
||||
}
|
||||
|
||||
gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf);
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
gyroADCf = gyro.notchFilterDynApplyFn((filter_t*)&gyro.notchFilterDyn[axis][p], gyroADCf);
|
||||
}
|
||||
|
||||
if (axis == gyro.gyroDebugAxis) {
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
|
||||
GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
|
||||
}
|
||||
gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf);
|
||||
gyroADCf = gyro.notchFilterDynApplyFn((filter_t *)&gyro.notchFilterDyn[axis], gyroADCf);
|
||||
gyroADCf = gyro.notchFilterDynApplyFn2((filter_t *)&gyro.notchFilterDyn2[axis], gyroADCf);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -133,17 +133,16 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
|
|||
static void gyroInitFilterDynamicNotch()
|
||||
{
|
||||
gyro.notchFilterDynApplyFn = nullFilterApply;
|
||||
gyro.notchFilterDynApplyFn2 = nullFilterApply;
|
||||
|
||||
if (isDynamicFilterActive()) {
|
||||
gyro.notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
|
||||
if(gyroConfig()->dyn_notch_width_percent != 0) {
|
||||
gyro.notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
|
||||
}
|
||||
gyro.notchFilterDynCount = gyroConfig()->dyn_notch_count;
|
||||
|
||||
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(&gyro.notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
biquadFilterInit(&gyro.notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
|
||||
biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue