From e339ce3e3a629913163f5568c0ab78d6890e00c1 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 24 Feb 2021 16:13:08 -0700
Subject: [PATCH 01/19] update the Matrix filter, turn it into a dynamic notch
filter
This filter is far more precise than the previous filter was. I've currently set this up with 36 bins (so accuracy is (max-min)/36 which for defaults is, 12.5hz, the old filter had a bin size of 16 which would have given an accuracy of 28.125hz, so this is over double as accurate.) This can also track up to 5 peaks and place 5 notch filters. It is also nicer on the cpu than the older version was, so all around really good. Using this filter + abg i was easily able to raise my gyro lpf filters to 300hz with dterm filters at around 150hz. I'm really excited to see what you all think about this filter :), P.S. all credit goes to @KarateBrot for coding this into betaflight, I may want to update this later to have dynamic widths and dynamic on the number of notches it uses, but so far, hey its awesome. Probably going to stay like this for 0.4.0. Only the defaults need testing right now.
---
make/source.mk | 9 +-
src/main/blackbox/blackbox.c | 3 +-
src/main/cms/cms_menu_imu.c | 44 +--
src/main/common/filter.c | 6 +-
src/main/common/maths.c | 47 +++
src/main/common/maths.h | 5 +
src/main/common/sdft.c | 155 +++++++++
src/main/common/sdft.h | 54 ++++
src/main/interface/msp.c | 4 +-
src/main/interface/settings.c | 3 +-
src/main/sensors/gyro.c | 28 +-
src/main/sensors/gyro.h | 3 +-
src/main/sensors/gyro_filter_impl.h | 21 +-
src/main/sensors/gyroanalyse.c | 473 +++++++++++++++-------------
src/main/sensors/gyroanalyse.h | 28 +-
15 files changed, 564 insertions(+), 319 deletions(-)
create mode 100644 src/main/common/sdft.c
create mode 100644 src/main/common/sdft.h
diff --git a/make/source.mk b/make/source.mk
index 88ee3ece83..ad1a5d0f7a 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -190,6 +190,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_fake.c \
drivers/accgyro/accgyro_mpu.c \
@@ -335,14 +336,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)
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 1628a52d37..c167699875 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1229,7 +1229,8 @@ static bool blackboxWriteSysinfo(void) {
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
#if defined(USE_GYRO_DATA_ANALYSE)
- BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_q", "%d", gyroConfig()->dyn_notch_q_factor);
+ BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_width", "%d", gyroConfig()->dyn_notch_width);
+ BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_count", "%d", gyroConfig()->dyn_notch_count);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
#endif
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 172bbcaf9d..3aa0bf79d6 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -483,13 +483,10 @@ static uint16_t gyroConfig_gyro_lowpass_hz_yaw;
static uint16_t gyroConfig_gyro_lowpass2_hz_roll;
static uint16_t gyroConfig_gyro_lowpass2_hz_pitch;
static uint16_t gyroConfig_gyro_lowpass2_hz_yaw;
-static uint16_t gyroConfig_gyro_soft_notch_hz_1;
-static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
-static uint16_t gyroConfig_gyro_soft_notch_hz_2;
-static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
-static uint16_t gyroConfig_gyro_matrix_q;
-static uint16_t gyroConfig_gyro_matrix_min_hz;
-static uint16_t gyroConfig_gyro_matrix_max_hz;
+static uint16_t gyroConfig_gyro_notch_width;
+static uint8_t gyroConfig_gyro_notch_count;
+static uint16_t gyroConfig_gyro_notch_min_hz;
+static uint16_t gyroConfig_gyro_notch_max_hz;
static uint16_t gyroConfig_gyro_abg_alpha;
static uint16_t gyroConfig_gyro_abg_boost;
static uint8_t gyroConfig_gyro_abg_half_life;
@@ -508,13 +505,10 @@ static long cmsx_menuGyro_onEnter(void) {
gyroConfig_gyro_lowpass2_hz_roll = gyroConfig()->gyro_lowpass2_hz[ROLL];
gyroConfig_gyro_lowpass2_hz_pitch = gyroConfig()->gyro_lowpass2_hz[PITCH];
gyroConfig_gyro_lowpass2_hz_yaw = gyroConfig()->gyro_lowpass2_hz[YAW];
- gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
- gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
- gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
- gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
- gyroConfig_gyro_matrix_q = gyroConfig()->dyn_notch_q_factor;
- gyroConfig_gyro_matrix_min_hz = gyroConfig()->dyn_notch_min_hz;
- gyroConfig_gyro_matrix_max_hz = gyroConfig()->dyn_notch_max_hz;
+ gyroConfig_gyro_notch_width = gyroConfig()->dyn_notch_width;
+ gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count;
+ gyroConfig_gyro_notch_min_hz = gyroConfig()->dyn_notch_min_hz;
+ gyroConfig_gyro_notch_max_hz = gyroConfig()->dyn_notch_max_hz;
gyroConfig_gyro_abg_alpha = gyroConfig()->gyro_ABG_alpha;
gyroConfig_gyro_abg_boost = gyroConfig()->gyro_ABG_boost;
gyroConfig_gyro_abg_half_life = gyroConfig()->gyro_ABG_half_life;
@@ -537,13 +531,10 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) {
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = gyroConfig_gyro_lowpass2_hz_roll;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = gyroConfig_gyro_lowpass2_hz_pitch;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = gyroConfig_gyro_lowpass2_hz_yaw;
- gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
- gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
- gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
- gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
- gyroConfigMutable()->dyn_notch_q_factor = gyroConfig_gyro_matrix_q;
- gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_matrix_min_hz;
- gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_matrix_max_hz;
+ gyroConfigMutable()->dyn_notch_width = gyroConfig_gyro_notch_width;
+ gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count;
+ gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_notch_min_hz;
+ gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_notch_max_hz;
gyroConfigMutable()->gyro_ABG_alpha = gyroConfig_gyro_abg_alpha;
gyroConfigMutable()->gyro_ABG_boost = gyroConfig_gyro_abg_boost;
gyroConfigMutable()->gyro_ABG_half_life = gyroConfig_gyro_abg_half_life;
@@ -568,9 +559,10 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
{ "GYRO LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_pitch, 0, 16000, 1 }, 0 },
{ "GYRO LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_yaw, 0, 16000, 1 }, 0 },
#endif
- { "MATRIX Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_matrix_q, 0, 1000, 1 }, 0 },
- { "MATRIX MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_matrix_min_hz, 30, 1000, 1 }, 0 },
- { "MATRIX MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_matrix_max_hz, 200, 1000, 1 }, 0 },
+ { "DYN NOTCH WIDTH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_width, 0, 1000, 1 }, 0 },
+ { "DYN NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_notch_count, 1, 5, 1 }, 0 },
+ { "DYN NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_min_hz, 30, 1000, 1 }, 0 },
+ { "DYN NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_max_hz, 200, 1000, 1 }, 0 },
#ifndef USE_GYRO_IMUF9001
{ "IMUF W", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w, 0, 512, 1 }, 0 },
{ "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 100, 16000, 100 }, 0 },
@@ -579,10 +571,6 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
{ "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 5 }, 0 },
#endif
- { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
- { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
- { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
- { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "GYRO ABG ALPHA", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_abg_alpha, 0, 1000, 1 }, 0 },
{ "GYRO ABG BOOST", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_abg_boost, 0, 2000, 1 }, 0 },
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 1cf011d441..6154d54e50 100755
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -31,8 +31,6 @@
#include "fc/fc_rc.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) {
// PT1 Low Pass filter
float pt1FilterGain(uint16_t f_cut, float dT) {
- const float RC = 0.5f / (M_PI_FLOAT * f_cut);
+ const float RC = 0.5f / (M_PIf * f_cut);
return dT / (RC + dT);
}
@@ -100,7 +98,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);
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index f72af82423..f0fb1a2b51 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -95,6 +95,53 @@ float acos_approx(float x) {
else
return result;
}
+
+// https://dsp.stackexchange.com/a/17276
+// Original implementation by Robert Bristow-Johnson (rbj@audioimagination.com)
+float sqrt_approx(float x)
+{
+ #define sqrtPowSeriesCoef1 0.49959804148061f
+ #define sqrtPowSeriesCoef2 -0.12047308243453f
+ #define sqrtPowSeriesCoef3 0.04585425015501f
+ #define sqrtPowSeriesCoef4 -0.01076564682800f
+
+ if (x <= 5.877471754e-39f) return 0.0f;
+
+ float acc;
+ float xPower;
+ int32_t intPart;
+
+ union {
+ float f;
+ int32_t i;
+ } xBits;
+
+ xBits.f = x;
+
+ intPart = ((xBits.i) >> 23); // get biased exponent
+ intPart -= 127; // unbias it
+
+ x = (float)(xBits.i & 0x007FFFFF); // mask off exponent leaving 0x800000 * (mantissa - 1)
+ x *= 1.192092895507812e-07f; // divide by 0x800000
+
+ acc = sqrtPowSeriesCoef1 * x + 1.0f;
+ xPower = x * x;
+ acc += sqrtPowSeriesCoef2 * xPower;
+ xPower *= x;
+ acc += sqrtPowSeriesCoef3 * xPower;
+ xPower *= x;
+ acc += sqrtPowSeriesCoef4 * xPower;
+
+ if (intPart & 0x1) {
+ acc *= M_SQRT2f; // an odd input exponent means an extra sqrt(2) in the output
+ }
+
+ xBits.i = intPart >> 1; // divide exponent by 2, lose LSB
+ xBits.i += 127; // rebias exponent
+ xBits.i <<= 23; // move biased exponent into exponent bits
+
+ return acc * xBits.f;
+}
#endif
int gcd(int num, int denom) {
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 2bace86d18..a89946a1cf 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -36,6 +36,9 @@
// Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f
#define M_PI_HALFf 3.14159265358979323846f/2
+#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)
@@ -134,6 +137,7 @@ float acos_approx(float x);
float exp_approx(float val);
float log_approx(float val);
float pow_approx(float a, float b);
+float sqrt_approx(float x);
#else
#define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(x)
@@ -143,6 +147,7 @@ float pow_approx(float a, float b);
#define exp_approx(x) expf(x)
#define log_approx(x) logf(x)
#define pow_approx(a, b) powf(b, a)
+#define sqrt_approx(x) sqrtf(x)
#endif
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c
new file mode 100644
index 0000000000..91caf9ae1b
--- /dev/null
+++ b/src/main/common/sdft.c
@@ -0,0 +1,155 @@
+/*
+ * 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 .
+ */
+
+#include
+
+#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_RAM_ZERO_INIT float rPowerN; // SDFT_R to the power of SDFT_SAMPLE_SIZE
+static FAST_RAM_ZERO_INIT bool isInitialized;
+static FAST_RAM_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 fast square root approximation 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] = sqrt_approx(data[i]);
+ }
+}
diff --git a/src/main/common/sdft.h b/src/main/common/sdft.h
new file mode 100644
index 0000000000..aad349bf8a
--- /dev/null
+++ b/src/main/common/sdft.h
@@ -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 .
+ */
+
+ // Implementation of a Sliding Discrete Fourier Transform (SDFT).
+ // Complexity for calculating frequency spectrum with N bins is O(N).
+
+#pragma once
+
+#include
+#include
+#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);
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index d4325ccb16..5775ed90ce 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1192,7 +1192,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
sbufWriteU8(dst, currentPidProfile->dFilter[ROLL].Wc);
sbufWriteU8(dst, currentPidProfile->dFilter[PITCH].Wc);
sbufWriteU8(dst, currentPidProfile->dFilter[YAW].Wc);
- sbufWriteU16(dst, gyroConfig()->dyn_notch_q_factor);
+ sbufWriteU16(dst, gyroConfig()->dyn_notch_width);
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
break;
/*#ifndef USE_GYRO_IMUF9001
@@ -1768,7 +1768,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
currentPidProfile->dFilter[ROLL].Wc = sbufReadU8(src);
currentPidProfile->dFilter[PITCH].Wc = sbufReadU8(src);
currentPidProfile->dFilter[YAW].Wc = sbufReadU8(src);
- gyroConfigMutable()->dyn_notch_q_factor = sbufReadU16(src);
+ gyroConfigMutable()->dyn_notch_width = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 16bb437315..c28cfd676e 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -543,7 +543,8 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
#if defined(USE_GYRO_DATA_ANALYSE)
- { "dynamic_gyro_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q_factor) },
+ { "dynamic_gyro_notch_width", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width) },
+ { "dynamic_gyro_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
{ "dynamic_gyro_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
{ "dynamic_gyro_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 600, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
#endif
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 1cb63d36f8..398460dc89 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -169,7 +169,7 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
- biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
+ biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][5];
// overflow and recovery
timeUs_t overflowTimeUs;
@@ -181,7 +181,6 @@ typedef struct gyroSensor_s {
#ifdef USE_GYRO_DATA_ANALYSE
gyroAnalyseState_t gyroAnalyseState;
- float dynNotchQ;
#endif
} gyroSensor_t;
@@ -244,7 +243,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
- .dyn_notch_q_factor = 300,
+ .dyn_notch_width = 30,
+ .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
.imuf_mode = GTBCM_GYRO_ACC_FILTER_F,
@@ -295,7 +295,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
- .dyn_notch_q_factor = 250,
+ .dyn_notch_width = 35,
+ .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
.gyro_ABG_alpha = 0,
@@ -520,9 +521,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) {
}
static bool gyroInitSensor(gyroSensor_t *gyroSensor) {
-#ifdef USE_GYRO_DATA_ANALYSE
- gyroSensor->dynNotchQ = gyroConfig()->dyn_notch_q_factor / 100.0f;
-#endif
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160)
@@ -795,8 +793,8 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) {
if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- for (int axis2 = 0; axis2 < XYZ_AXIS_COUNT; axis2++) {
- biquadFilterInit(&gyroSensor->notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
+ for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
+ biquadFilterInit(&gyroSensor->notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, .1, FILTER_NOTCH);
}
}
}
@@ -1065,15 +1063,6 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
-static FAST_CODE void dynamicGyroNotchFiltersUpdate(gyroSensor_t* gyroSensor) {
- if (gyroSensor->gyroAnalyseState.filterUpdateExecute) {
- const uint8_t axis = gyroSensor->gyroAnalyseState.filterUpdateAxis;
- const uint16_t frequency = gyroSensor->gyroAnalyseState.filterUpdateFrequency;
- biquadFilterUpdate(&gyroSensor->notchFilterDyn[0][axis], frequency, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
- biquadFilterUpdate(&gyroSensor->notchFilterDyn[1][axis], frequency, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
- biquadFilterUpdate(&gyroSensor->notchFilterDyn[2][axis], frequency, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
- }
-}
static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs_t currentTimeUs) {
#ifndef USE_DMA_SPI_DEVICE
@@ -1136,8 +1125,7 @@ static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs
#endif
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
- gyroDataAnalyse(&gyroSensor->gyroAnalyseState);
- dynamicGyroNotchFiltersUpdate(gyroSensor);
+ gyroDataAnalyse(&gyroSensor->gyroAnalyseState, &gyroSensor->notchFilterDyn);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 4f753b59ad..4d5f604390 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -113,7 +113,8 @@ typedef struct gyroConfig_s {
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
- uint16_t dyn_notch_q_factor;
+ uint16_t dyn_notch_width;
+ uint8_t dyn_notch_count;
uint16_t dyn_notch_min_hz;
uint16_t dyn_notch_max_hz;
#if defined(USE_GYRO_IMUF9001)
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index cc8b94899b..20068a538c 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/src/main/sensors/gyro_filter_impl.h
@@ -37,18 +37,15 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) {
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
- if (axis == X) {
- GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
- GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
- }
- }
-#endif
-#ifdef USE_GYRO_DATA_ANALYSE
- if (isDynamicFilterActive()) {
- gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
- gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis][0], gyroADCf);
- gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis][1], gyroADCf);
- gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis][2], gyroADCf);
+ if (axis == X) {
+ GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
+ GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
+ }
+
+ gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t*)&gyroSensor->notchFilterDyn[axis][p], gyroADCf);
+ }
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
}
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index 040eaa3780..eb92dafb8d 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -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
#include
#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,304 +51,326 @@
#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 100 (common/sdft.h)
+// We get 50 frequency bins from 100 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 49 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 100 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
+// 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
+
+// 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 32 samples of the FFT input buffer with clean new data takes 24ms
+// Hence to completely replace all 100 samples of the SDFT input buffer with clean new data takes 75ms
-// 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.
+// 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.
+// 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
+// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad.
-// 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/100, ie 13.3Hz per bin at 1333Hz
+// Usable bandwidth is half this, ie 666Hz if sdftSampleRateHz is 1333Hz, i.e. bin 1 is 13.3Hz, bin 2 is 26.7Hz 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_SMOOTH_HZ 4
+#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_RAM_ZERO_INIT fftSamplingRateHz;
-static float FAST_RAM_ZERO_INIT fftResolution;
-static uint8_t FAST_RAM_ZERO_INIT fftStartBin;
+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_RAM_ZERO_INIT sdft[XYZ_AXIS_COUNT];
+static peak_t FAST_RAM_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX];
+static float FAST_RAM_ZERO_INIT sdftData[SDFT_BIN_COUNT];
+static uint16_t FAST_RAM_ZERO_INIT sdftSampleRateHz;
+static float FAST_RAM_ZERO_INIT sdftResolutionHz;
+static uint8_t FAST_RAM_ZERO_INIT sdftStartBin;
+static uint8_t FAST_RAM_ZERO_INIT sdftEndBin;
+static float FAST_RAM_ZERO_INIT sdftMeanSq;
+static uint16_t FAST_RAM_ZERO_INIT dynNotchBandwidthHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
static float FAST_RAM_ZERO_INIT smoothFactor;
-static uint8_t FAST_RAM_ZERO_INIT samples;
-// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
-static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
+static uint8_t FAST_RAM_ZERO_INIT numSamples;
-void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
-#ifdef USE_DUAL_GYRO
+void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
+{
+#ifdef USE_MULTI_GYRO
static bool gyroAnalyseInitialized;
if (gyroAnalyseInitialized) {
return;
}
gyroAnalyseInitialized = true;
#endif
+
+ dynNotchBandwidthHz = gyroConfig()->dyn_notch_width;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
- const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
- samples = MAX(1, gyroLoopRateHz / (2 * dynNotchMaxHz)); //600hz, 8k looptime, 13.333
- 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
- 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)));
+
+ // 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
+
+ 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)
+
+ 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
+
+ for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
}
}
-void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) {
+void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
+{
// 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 < gyroConfig()->dyn_notch_count; 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)gyroConfig()->dyn_notch_count + 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);
+static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5]);
-/*
- * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
- */
-void NOINLINE gyroDataAnalyse(gyroAnalyseState_t *state) {
- state->filterUpdateExecute = false; //This will be changed to true only if new data is present
+// Downsample and analyse gyro data
+FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5])
+{
// 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);
+ gyroDataAnalyseUpdate(state, notchFilterDyn);
--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 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);
+// Find frequency peaks and update filters
+static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5])
+{
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: {
- 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);
+
+ case STEP_WINDOW: // 6us @ F722
+ {
+ 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, 0, micros() - startTime);
+
break;
}
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
- break;
- }
- case STEP_BITREVERSAL: {
- // 6us
- arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
- state->updateStep++;
- FALLTHROUGH;
- }
- 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);
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
- break;
- }
- case STEP_ARM_CMPLX_MAG_F32: {
- // 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
+ case STEP_DETECT_PEAKS: // 6us @ F722
+ {
+ // Get memory ready for new peak data on current axis
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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++) {
+ // 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 < gyroConfig()->dyn_notch_count; p++) {
+ if (sdftData[bin] > peaks[p].value) {
+ for (uint8_t k = gyroConfig()->dyn_notch_count - 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 = gyroConfig()->dyn_notch_count - 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;
}
- 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;
+ case STEP_CALC_FREQUENCIES: // 4us @ F722
+ {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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;
+
+ // 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 (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 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
+ // 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]);
+ }
}
}
- 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;
+
+ if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
}
}
+
+ if (state->updateAxis == X) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count && p < 3; p++) {
+ DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
+ }
+ }
+
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+
+ break;
}
- dataMin = fminf(dataMin, dataMinHi);
- // 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;
- // 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 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;
- }
- // get centerFreq in Hz from weighted bins
- float centerFreq;
- 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];
- }
- centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
- // 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]);
- if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
- dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
- }
- 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));
- }
-// if (state->updateAxis == 1) {
-// DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
-// }
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
- break;
- }
- case STEP_UPDATE_FILTERS: {
- // 7us
- state->filterUpdateExecute = true;
- state->filterUpdateAxis = state->updateAxis;
- state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
- 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);
+ case STEP_UPDATE_FILTERS: // 7us @ F722
+ {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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(¬chFilterDyn[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;
}
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
- }
}
+
state->updateStep = (state->updateStep + 1) % STEP_COUNT;
}
diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h
index 17904ff95b..14d8125ab9 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -20,42 +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][DYN_NOTCH_COUNT_MAX];
- float centerFreq[XYZ_AXIS_COUNT];
- bool filterUpdateExecute;
- uint8_t filterUpdateAxis;
- float filterUpdateFrequency;
} gyroAnalyseState_t;
-STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
-
-void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
-void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
-void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse);
+void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
+void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
+void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5]);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
From 2710be7d7c90ec2daee14e823cf4303dc565b3c9 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 24 Feb 2021 18:23:25 -0700
Subject: [PATCH 02/19] fix emugravity
---
src/main/cms/cms_menu_imu.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 3aa0bf79d6..dd0d647bad 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -210,7 +210,7 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = {
{ "I RELAX THRESH YAW", OME_UINT8, NULL, &(OSD_UINT8_t){ &itermRelaxThresholdYaw, 0, 100, 1 }, 0 },
{ "I WINDUP", OME_UINT8, NULL, &(OSD_UINT8_t){ &itermWindup, 0, 100, 1 }, 0 },
- { "EMU GRAVITY", OME_UINT16, NULL, &(OSD_UINT8_t){ &emuGravityGain, 0, 255, 1}, 0 },
+ { "EMU GRAVITY", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuGravityGain, 0, 255, 1}, 0 },
{ "LINEAR THRUST LOW", OME_UINT8, NULL, &(OSD_UINT8_t) { &linear_thrust_low_output, 0, 100, 1}, 0 },
{ "LINEAR THRUST HIGH", OME_UINT8, NULL, &(OSD_UINT8_t) { &linear_thrust_high_output, 0, 100, 1}, 0 },
From 7d33c3d1df3aa32eb56a1a031e7a5a4b00d5bdc6 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 24 Feb 2021 19:58:06 -0700
Subject: [PATCH 03/19] fix potential bug
---
src/main/sensors/gyro.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 398460dc89..ec5c0a7587 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -1125,7 +1125,7 @@ static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs
#endif
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
- gyroDataAnalyse(&gyroSensor->gyroAnalyseState, &gyroSensor->notchFilterDyn);
+ gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
From 486351cc697e35f0d2edd30f8e28855dae939f58 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Fri, 26 Feb 2021 10:20:07 -0700
Subject: [PATCH 04/19] the approximation is slower and less accurate
---
src/main/common/maths.c | 47 -----------------------------------------
src/main/common/maths.h | 2 --
src/main/common/sdft.c | 3 ++-
3 files changed, 2 insertions(+), 50 deletions(-)
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index f0fb1a2b51..f72af82423 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -95,53 +95,6 @@ float acos_approx(float x) {
else
return result;
}
-
-// https://dsp.stackexchange.com/a/17276
-// Original implementation by Robert Bristow-Johnson (rbj@audioimagination.com)
-float sqrt_approx(float x)
-{
- #define sqrtPowSeriesCoef1 0.49959804148061f
- #define sqrtPowSeriesCoef2 -0.12047308243453f
- #define sqrtPowSeriesCoef3 0.04585425015501f
- #define sqrtPowSeriesCoef4 -0.01076564682800f
-
- if (x <= 5.877471754e-39f) return 0.0f;
-
- float acc;
- float xPower;
- int32_t intPart;
-
- union {
- float f;
- int32_t i;
- } xBits;
-
- xBits.f = x;
-
- intPart = ((xBits.i) >> 23); // get biased exponent
- intPart -= 127; // unbias it
-
- x = (float)(xBits.i & 0x007FFFFF); // mask off exponent leaving 0x800000 * (mantissa - 1)
- x *= 1.192092895507812e-07f; // divide by 0x800000
-
- acc = sqrtPowSeriesCoef1 * x + 1.0f;
- xPower = x * x;
- acc += sqrtPowSeriesCoef2 * xPower;
- xPower *= x;
- acc += sqrtPowSeriesCoef3 * xPower;
- xPower *= x;
- acc += sqrtPowSeriesCoef4 * xPower;
-
- if (intPart & 0x1) {
- acc *= M_SQRT2f; // an odd input exponent means an extra sqrt(2) in the output
- }
-
- xBits.i = intPart >> 1; // divide exponent by 2, lose LSB
- xBits.i += 127; // rebias exponent
- xBits.i <<= 23; // move biased exponent into exponent bits
-
- return acc * xBits.f;
-}
#endif
int gcd(int num, int denom) {
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index a89946a1cf..95d6098149 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -137,7 +137,6 @@ float acos_approx(float x);
float exp_approx(float val);
float log_approx(float val);
float pow_approx(float a, float b);
-float sqrt_approx(float x);
#else
#define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(x)
@@ -147,7 +146,6 @@ float sqrt_approx(float x);
#define exp_approx(x) expf(x)
#define log_approx(x) logf(x)
#define pow_approx(a, b) powf(b, a)
-#define sqrt_approx(x) sqrtf(x)
#endif
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c
index 91caf9ae1b..5603bfce39 100644
--- a/src/main/common/sdft.c
+++ b/src/main/common/sdft.c
@@ -19,6 +19,7 @@
*/
#include
+#include
#include "platform.h"
@@ -150,6 +151,6 @@ FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
{
for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) {
- data[i] = sqrt_approx(data[i]);
+ data[i] = sqrtf(data[i]);
}
}
From 58bbf6827d1836f51e823dd4455ec1805ba7bea5 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Tue, 1 Jun 2021 08:20:44 -0600
Subject: [PATCH 05/19] fixes
---
src/main/blackbox/blackbox.c | 2 +-
src/main/cms/cms_menu_imu.c | 10 +++++-----
src/main/common/sdft.c | 2 +-
src/main/interface/msp.c | 4 ++--
src/main/interface/settings.c | 2 +-
src/main/sensors/gyro.c | 9 ++++-----
src/main/sensors/gyro.h | 2 +-
src/main/sensors/gyro_filter_impl.h | 2 +-
src/main/sensors/gyroanalyse.c | 17 +++++++----------
src/main/sensors/gyroanalyse.h | 3 ++-
10 files changed, 25 insertions(+), 28 deletions(-)
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 98f4de2172..5d73baba8f 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1234,7 +1234,7 @@ static bool blackboxWriteSysinfo(void) {
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
#if defined(USE_GYRO_DATA_ANALYSE)
- BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_width", "%d", gyroConfig()->dyn_notch_width);
+ BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_width", "%d", gyroConfig()->dyn_notch_q);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_count", "%d", gyroConfig()->dyn_notch_count);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index b458429f4e..b521212f6a 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -487,7 +487,7 @@ static uint16_t gyroConfig_gyro_lowpass_hz_yaw;
static uint16_t gyroConfig_gyro_lowpass2_hz_roll;
static uint16_t gyroConfig_gyro_lowpass2_hz_pitch;
static uint16_t gyroConfig_gyro_lowpass2_hz_yaw;
-static uint16_t gyroConfig_gyro_notch_width;
+static uint16_t gyroConfig_gyro_q;
static uint8_t gyroConfig_gyro_notch_count;
static uint16_t gyroConfig_gyro_notch_min_hz;
static uint16_t gyroConfig_gyro_notch_max_hz;
@@ -514,7 +514,7 @@ static long cmsx_menuGyro_onEnter(void) {
gyroConfig_gyro_lowpass2_hz_roll = gyroConfig()->gyro_lowpass2_hz[ROLL];
gyroConfig_gyro_lowpass2_hz_pitch = gyroConfig()->gyro_lowpass2_hz[PITCH];
gyroConfig_gyro_lowpass2_hz_yaw = gyroConfig()->gyro_lowpass2_hz[YAW];
- gyroConfig_gyro_notch_width = gyroConfig()->dyn_notch_width;
+ gyroConfig_gyro_q = gyroConfig()->dyn_notch_q;
gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count;
gyroConfig_gyro_notch_min_hz = gyroConfig()->dyn_notch_min_hz;
gyroConfig_gyro_notch_max_hz = gyroConfig()->dyn_notch_max_hz;
@@ -545,7 +545,7 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) {
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = gyroConfig_gyro_lowpass2_hz_roll;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = gyroConfig_gyro_lowpass2_hz_pitch;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = gyroConfig_gyro_lowpass2_hz_yaw;
- gyroConfigMutable()->dyn_notch_width = gyroConfig_gyro_notch_width;
+ gyroConfigMutable()->dyn_notch_q = gyroConfig_gyro_q;
gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count;
gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_notch_min_hz;
gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_notch_max_hz;
@@ -578,8 +578,8 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
{ "GYRO LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_pitch, 0, 16000, 1 }, 0 },
{ "GYRO LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_yaw, 0, 16000, 1 }, 0 },
#endif
- { "DYN NOTCH WIDTH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_width, 0, 1000, 1 }, 0 },
- { "DYN NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_notch_count, 1, 5, 1 }, 0 },
+ { "DYN NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_q, 0, 1000, 1 }, 0 },
+ { "DYN NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_notch_count, 1, 5, 1 }, 0 },
{ "DYN NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_min_hz, 30, 1000, 1 }, 0 },
{ "DYN NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_max_hz, 200, 1000, 1 }, 0 },
#ifndef USE_GYRO_IMUF9001
diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c
index 5603bfce39..d3eb1f9c11 100644
--- a/src/main/common/sdft.c
+++ b/src/main/common/sdft.c
@@ -38,7 +38,7 @@ 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);
+ 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++) {
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index b5fd4ff088..d79aefe2a4 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1192,7 +1192,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
sbufWriteU8(dst, currentPidProfile->dFilter[ROLL].Wc);
sbufWriteU8(dst, currentPidProfile->dFilter[PITCH].Wc);
sbufWriteU8(dst, currentPidProfile->dFilter[YAW].Wc);
- sbufWriteU16(dst, gyroConfig()->dyn_notch_width);
+ sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
break;
/*#ifndef USE_GYRO_IMUF9001
@@ -1768,7 +1768,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
currentPidProfile->dFilter[ROLL].Wc = sbufReadU8(src);
currentPidProfile->dFilter[PITCH].Wc = sbufReadU8(src);
currentPidProfile->dFilter[YAW].Wc = sbufReadU8(src);
- gyroConfigMutable()->dyn_notch_width = sbufReadU16(src);
+ gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index db4f18147f..229f8e0076 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -543,7 +543,7 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
#if defined(USE_GYRO_DATA_ANALYSE)
- { "dynamic_gyro_notch_width", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width) },
+ { "dynamic_gyro_notch_width", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
{ "dynamic_gyro_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
{ "dynamic_gyro_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
{ "dynamic_gyro_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 400, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 7ba53f26c5..ce844db983 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -169,7 +169,6 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
- biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][5];
// overflow and recovery
timeUs_t overflowTimeUs;
@@ -247,7 +246,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
- .dyn_notch_width = 30,
+ .dyn_notch_q = 400,
.dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
@@ -302,7 +301,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
- .dyn_notch_width = 35,
+ .dyn_notch_q = 350,
.dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
@@ -804,7 +803,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
- biquadFilterInit(&gyroSensor->notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, .1, FILTER_NOTCH);
+ biquadFilterInit(&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, .1, FILTER_NOTCH);
}
}
}
@@ -1175,7 +1174,7 @@ static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs
#endif
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
- gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
+ gyroDataAnalyse(&gyroSensor->gyroAnalyseState);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 0fb4ffb455..78f74f32db 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -132,7 +132,7 @@ typedef struct gyroConfig_s {
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
- uint16_t dyn_notch_width;
+ uint16_t dyn_notch_q;
uint8_t dyn_notch_count;
uint16_t dyn_notch_min_hz;
uint16_t dyn_notch_max_hz;
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index d35cc9160a..c5927aab95 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/src/main/sensors/gyro_filter_impl.h
@@ -45,7 +45,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) {
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
- gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t*)&gyroSensor->notchFilterDyn[axis][p], gyroADCf);
+ gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][p], gyroADCf);
}
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index eb92dafb8d..cec10d92c3 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -111,7 +111,7 @@ static float FAST_RAM_ZERO_INIT sdftResolutionHz;
static uint8_t FAST_RAM_ZERO_INIT sdftStartBin;
static uint8_t FAST_RAM_ZERO_INIT sdftEndBin;
static float FAST_RAM_ZERO_INIT sdftMeanSq;
-static uint16_t FAST_RAM_ZERO_INIT dynNotchBandwidthHz;
+static uint16_t FAST_RAM_ZERO_INIT dynNotchQ;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
@@ -128,7 +128,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true;
#endif
- dynNotchBandwidthHz = gyroConfig()->dyn_notch_width;
+ dynNotchQ = gyroConfig()->dyn_notch_q;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
@@ -175,10 +175,10 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const fl
state->oversampledGyroAccumulator[axis] += sample;
}
-static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5]);
+static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
// Downsample and analyse gyro data
-FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5])
+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
@@ -211,13 +211,13 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t notchFi
// Find frequency peaks and update filters
if (state->updateTicks > 0) {
- gyroDataAnalyseUpdate(state, notchFilterDyn);
+ gyroDataAnalyseUpdate(state);
--state->updateTicks;
}
}
// Find frequency peaks and update filters
-static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5])
+static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
{
uint32_t startTime = 0;
if (debugMode == (DEBUG_FFT_TIME)) {
@@ -358,10 +358,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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(¬chFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynamicQ, FILTER_NOTCH);
+ biquadFilterUpdate(&state->notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
}
}
diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h
index 14d8125ab9..5c9d1b8774 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -39,11 +39,12 @@ typedef struct gyroAnalyseState_s {
uint8_t updateAxis;
float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
+ biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][5];
} gyroAnalyseState_t;
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
-void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t notchFilterDyn[3][5]);
+void gyroDataAnalyse(gyroAnalyseState_t *state);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
From eaf9f3830fae213d52cce67415d8c822d8dc43c7 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Tue, 15 Jun 2021 11:45:37 -0600
Subject: [PATCH 06/19] update q correctly
---
src/main/sensors/gyro.c | 2 +-
src/main/sensors/gyroanalyse.c | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 2e850fda61..bfaf978952 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -805,7 +805,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
- biquadFilterInit(&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, .1, FILTER_NOTCH);
+ biquadFilterInit(&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
}
}
}
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index cec10d92c3..ee9fd20a57 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -128,7 +128,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true;
#endif
- dynNotchQ = gyroConfig()->dyn_notch_q;
+ dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
From 746d03b75913b99efa50ef0a943bca9f36d5040d Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Tue, 15 Jun 2021 12:24:10 -0600
Subject: [PATCH 07/19] EmuBoost2
Adding EmuBoost2 adds a really nice way to boost stick feel
---
src/main/blackbox/blackbox.c | 8 +++-
src/main/build/debug.c | 3 +-
src/main/build/debug.h | 1 +
src/main/cms/cms_menu_imu.c | 18 ++++++++
src/main/common/filter.c | 10 ++--
src/main/common/filter.h | 2 +
src/main/flight/pid.c | 87 +++++++++++++++++++++++++++--------
src/main/flight/pid.h | 5 ++
src/main/interface/settings.c | 5 ++
9 files changed, 116 insertions(+), 23 deletions(-)
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index df6ad0ec9d..58b41039cc 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1208,6 +1208,11 @@ static bool blackboxWriteSysinfo(void) {
BLACKBOX_PRINT_HEADER_LINE("mixer_impl", "%d", currentPidProfile->mixer_impl);
BLACKBOX_PRINT_HEADER_LINE("mixer_laziness", "%d", currentPidProfile->mixer_laziness);
BLACKBOX_PRINT_HEADER_LINE("mixer_yaw_throttle_comp", "%d", currentPidProfile->mixer_yaw_throttle_comp);
+ BLACKBOX_PRINT_HEADER_LINE("emuboost2", "%d", currentPidProfile->emuBoost2);
+ BLACKBOX_PRINT_HEADER_LINE("emuboost2_filter", "%d", currentPidProfile->emuBoost2_filter);
+ BLACKBOX_PRINT_HEADER_LINE("emuboost2_cutoff", "%d", currentPidProfile->emuBoost2_cutoff);
+ BLACKBOX_PRINT_HEADER_LINE("emuboost2_expo", "%d", currentPidProfile->emuBoost2_expo);
+
// End of EmuFlight controller parameters
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
@@ -1235,10 +1240,11 @@ static bool blackboxWriteSysinfo(void) {
BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_alpha", "%d", gyroConfig()->gyro_ABG_alpha);
BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_boost", "%d", gyroConfig()->gyro_ABG_boost);
BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_half_life", "%d", gyroConfig()->gyro_ABG_half_life);
- BLACKBOX_PRINT_HEADER_LINE("smith_predict_enabled", "%d", gyroConfig()->smithPredictorEnabled);
+ BLACKBOX_PRINT_HEADER_LINE("smith_predict_enabled", "%d", gyroConfig()->smithPredictorEnabled);
BLACKBOX_PRINT_HEADER_LINE("smith_predict_str", "%d", gyroConfig()->smithPredictorStrength);
BLACKBOX_PRINT_HEADER_LINE("smith_predict_delay", "%d", gyroConfig()->smithPredictorDelay);
BLACKBOX_PRINT_HEADER_LINE("smith_predict_filt_hz", "%d", gyroConfig()->smithPredictorFilterHz);
+
#if defined(USE_ACC)
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 84d93a28a6..37419d5826 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -77,5 +77,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"IMU",
"KALMAN",
"ANGLE",
- "HORIZON"
+ "HORIZON",
+ "EMUBOOST"
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 8c4a280c11..805d7f7e1d 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -96,6 +96,7 @@ typedef enum {
DEBUG_KALMAN,
DEBUG_ANGLE,
DEBUG_HORIZON,
+ DEBUG_EMUBOOST,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index dc99c7a13b..1cef7cc0bc 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -83,6 +83,11 @@ static uint8_t linear_throttle;
static mixerImplType_e mixer_impl;
static uint8_t mixer_laziness;
static uint8_t mixer_yaw_throttle_comp;
+static uint8_t emuBoost2;
+static uint8_t emuBoost2_filter;
+static uint8_t emuBoost2_cutoff;
+static uint8_t emuBoost2_expo;
+
static uint8_t tmpRateProfileIndex;
static uint8_t rateProfileIndex;
static char rateProfileIndexString[] = " p-r";
@@ -158,6 +163,10 @@ static long cmsx_PidAdvancedRead(void) {
mixer_impl = pidProfile->mixer_impl;
mixer_laziness = pidProfile->mixer_laziness;
mixer_yaw_throttle_comp = pidProfile->mixer_yaw_throttle_comp;
+ emuBoost2 = pidProfile->emuBoost2;
+ emuBoost2_filter = pidProfile->emuBoost2_filter;
+ emuBoost2_cutoff = pidProfile->emuBoost2_cutoff;
+ emuBoost2_expo = pidProfile->emuBoost2_expo;
return 0;
}
@@ -191,6 +200,10 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self) {
pidProfile->mixer_impl = mixer_impl;
pidProfile->mixer_laziness = mixer_laziness;
pidProfile->mixer_yaw_throttle_comp = mixer_yaw_throttle_comp;
+ pidProfile->emuBoost2 = emuBoost2;
+ pidProfile->emuBoost2_filter = emuBoost2_filter;
+ pidProfile->emuBoost2_cutoff = emuBoost2_cutoff;
+ pidProfile->emuBoost2_expo = emuBoost2_expo;
pidInitConfig(currentPidProfile);
return 0;
}
@@ -225,6 +238,11 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = {
{ "MIXER LAZINESS", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &mixer_laziness, 1, cms_offOnLabels }, 0 },
{ "MIXER YAW THR COMP", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &mixer_yaw_throttle_comp, 1, cms_offOnLabels }, 0 },
+ { "EMUBOOST 2.0", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2, 0, 250, 1}, 0 },
+ { "EMUBOOST 2.0 FILT", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_filter, 1, 250, 1}, 0 },
+ { "EMUBOOST 2.0 CUT", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_cutoff, 10, 250, 1}, 0 },
+ { "EMUBOOST 2.0 EXPO", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_expo, 10, 100, 1}, 0 },
+
{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0},
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 0379ce09e6..1dbb891a2e 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -223,14 +223,14 @@ FAST_CODE float alphaBetaGammaApply(alphaBetaGammaFilter_t *filter, float input)
// update (estimated) velocity
filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
filter->ak += filter->dT * filter->jk;
-
+
// what is our residual error (measured - estimated)
rk = input - filter->xk;
-
+
// artificially boost the error to increase the response of the filter
rk += pt1FilterApply(&filter->boostFilter, (fabsf(rk) * rk * filter->boost));
filter->rk = rk; // for logging
-
+
// update our estimates given the residual error.
filter->xk += filter->a * rk;
filter->vk += filter->b / filter->dT * rk;
@@ -275,3 +275,7 @@ FAST_CODE float ptnFilterApply(ptnFilter_t *filter, float input) {
return filter->state[filter->order];
} // ptnFilterApply
+
+FAST_CODE float crossFeed(float crossFeed, float preFiltered, float postFiltered) {
+ return (preFiltered * crossFeed) + (postFiltered * (1.0f - crossFeed));
+}
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index ea81c5d284..b551ab95da 100755
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -96,3 +96,5 @@ float alphaBetaGammaApply(alphaBetaGammaFilter_t *filter, float input);
void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut, float dT);
void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float ScaleF, float dt);
float ptnFilterApply(ptnFilter_t *filter, float input);
+
+float crossFeed(float crossFeed, float preFiltered, float postFiltered);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 4812480721..b1da8aed7f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -192,6 +192,10 @@ void resetPidProfile(pidProfile_t *pidProfile) {
.dterm_ABG_half_life = 50,
.emuGravityGain = 50,
.angle_filter = 100,
+ .emuBoost2 = 150,
+ .emuBoost2_filter = 20,
+ .emuBoost2_cutoff = 10,
+ .emuBoost2_expo = 25,
);
}
@@ -238,6 +242,8 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffYaw;
static FAST_RAM_ZERO_INIT pt1Filter_t emuGravityThrottleLpf;
static FAST_RAM_ZERO_INIT pt1Filter_t axisLockLpf[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT pt1Filter_t emuboost2_0Filter[XYZ_AXIS_COUNT];
+
static FAST_RAM_ZERO_INIT float iDecay;
void pidInitFilters(const pidProfile_t *pidProfile) {
@@ -312,6 +318,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&axisLockLpf[i], pt1FilterGain(pidProfile->axis_lock_hz, dT));
+ pt1FilterInit(&emuboost2_0Filter[i], pt1FilterGain(pidProfile->emuBoost2_filter, dT));
#if defined(USE_ITERM_RELAX)
if (i != FD_YAW) {
pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
@@ -333,7 +340,6 @@ static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float directFF[3];
static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float feathered_pids;
-static FAST_RAM_ZERO_INIT float smart_dterm_smoothing[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float setPointPTransition[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float setPointITransition[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float setPointDTransition[XYZ_AXIS_COUNT];
@@ -473,7 +479,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
float angle = pidProfile->levelAngleLimit * getRcDeflection(axis) * scaledRcDeflection;
if (pidProfile->angleExpo > 0) {
const float expof = pidProfile->angleExpo / 100.0f;
- angle = pidProfile->levelAngleLimit * (getRcDeflection(axis) * power3(getRcDeflectionAbs(axis)) * expof + getRcDeflection(axis) * (1 - expof));
+ angle = pidProfile->levelAngleLimit * (getRcDeflection(axis) * power3(getRcDeflectionAbs(axis)) * expof + getRcDeflection(axis) * (1 - expof)) * scaledRcDeflection;
}
#ifdef USE_GPS_RESCUE
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
@@ -636,17 +642,13 @@ static FAST_RAM_ZERO_INIT float stickMovement[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float lastRcDeflectionAbs[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float previousdDelta[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float kdRingBuffer[XYZ_AXIS_COUNT][KD_RING_BUFFER_SIZE];
-static FAST_RAM_ZERO_INIT float kdRingBufferSum[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT uint8_t kdRingBufferPoint[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float previousDtermErrorRate[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float accumulativeError[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float axisLock[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float inverseScaledError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) {
- float axisLock[XYZ_AXIS_COUNT];
- for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
- axisLock[axis] = pt1FilterApply(&axisLockLpf[axis], stickMovement[axis]) * axisLockMultiplier;
- }
scaledAxisPid[ROLL] = constrainf(1 - axisLock[PITCH] - axisLock[YAW] + axisLock[ROLL], 0.0f, 1.0f);
scaledAxisPid[PITCH] = constrainf(1 - axisLock[ROLL] - axisLock[YAW] + axisLock[PITCH], 0.0f, 1.0f);
@@ -661,6 +663,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+ axisLock[axis] = pt1FilterApply(&axisLockLpf[axis], stickMovement[axis]) * axisLockMultiplier;
+
// emugravity, the different hopefully better version of antiGravity no effect on yaw
float errorAccelerator = 1.0f;
if (pidProfile->emuGravityGain != 0 && axis != FD_YAW) {
@@ -734,9 +738,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
if (axis != FD_YAW) {
- itermRelaxFactor = MAX(1 - setpointHpf / pidProfile->iterm_relax_threshold, 0.0f);
+ itermRelaxFactor = MAX(1.0f - setpointHpf / pidProfile->iterm_relax_threshold, 0.0f);
} else {
- itermRelaxFactor = MAX(1 - setpointHpf / pidProfile->iterm_relax_threshold_yaw, 0.0f);
+ itermRelaxFactor = MAX(1.0f - setpointHpf / pidProfile->iterm_relax_threshold_yaw, 0.0f);
}
if (SIGN(iterm) == SIGN(itermErrorRate)) {
itermErrorRate *= itermRelaxFactor;
@@ -770,13 +774,20 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
temporaryIterm[axis] = iterm;
}
// -----calculate D component
+ float pureError = 0.0f;
+ //filter Kd properly, no setpoint filtering
+ pureError = errorRate - previousError[axis];
+ previousError[axis] = errorRate;
+
+ float dtermErrorRate = currentPidSetpoint - gyroRate * inverseScaledError[axis]; // r - y
+ float dtermError = dtermErrorRate - previousDtermErrorRate[axis];
+ previousDtermErrorRate[axis] = dtermErrorRate;
+
+ const float pureMeasurement = -(gyroRate - previousMeasurement[axis]);
+ previousMeasurement[axis] = gyroRate;
+
if (pidCoefficient[axis].Kd > 0) {
- //filter Kd properly, no setpoint filtering
- const float pureError = errorRate - previousError[axis];
- const float pureMeasurement = -(gyroRate - previousMeasurement[axis]);
- previousMeasurement[axis] = gyroRate;
- previousError[axis] = errorRate;
- float dDelta = ((feathered_pids * pureMeasurement) + ((1 - feathered_pids) * pureError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
+ float dDelta = ((feathered_pids * pureMeasurement) * inverseScaledError[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
//filter the dterm
dDelta = dtermLowpassApplyFn((filter_t *)&dtermLowpass[axis], dDelta);
dDelta = dtermLowpass2ApplyFn((filter_t *)&dtermLowpass2[axis], dDelta);
@@ -843,6 +854,42 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
pidData[axis].D *= getThrottleDAttenuation();
}
+ // emuboost 2.0
+ if (pidProfile->emuBoost2) {
+ //need to add logging, lol
+ float changeError = pureError * pidFrequency * DTERM_SCALE * 20.0f;
+ changeError = changeError / pidProfile->emuBoost2_cutoff;
+
+ float doSignMatch = 0;
+ changeError = constrainf(changeError, -1.0f, 1.0f);
+ changeError = ABS(changeError) * power3(changeError) * (pidProfile->emuBoost2_expo / 100.0f) + changeError * (1 - pidProfile->emuBoost2_expo / 100.0f);
+
+ float scaledError = constrainf(errorRate / pidProfile->emuBoost2_cutoff, -1.0f, 1.0f);
+ scaledError = ABS(scaledError) * power3(scaledError) * (pidProfile->emuBoost2_expo / 100.0f) + scaledError * (1 - pidProfile->emuBoost2_expo / 100.0f);
+
+ scaledError *= changeError;
+ scaledError = pt1FilterApply(&emuboost2_0Filter[axis], scaledError);
+ if (axis == FD_ROLL || axis == FD_PITCH) {
+ DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
+ }
+
+ if (scaledError > 0.0f) {
+ inverseScaledError[axis] = 1 - scaledError; // this is only 1 when the error and change in setpoint are high
+
+ pidData[axis].P *= ((pidProfile->emuBoost2 / 100.0f) * scaledError) + 1.0f;
+ //pidData[axis].D *= inverseScaledError[axis];
+ doSignMatch = 1000.0f;
+ } else {
+ inverseScaledError[axis] = 1.0f;
+ doSignMatch = 0.0f;
+ }
+ if (axis == FD_ROLL || axis == FD_PITCH) {
+ DEBUG_SET(DEBUG_EMUBOOST, axis + 2, lrintf(doSignMatch));
+ }
+ } else {
+ inverseScaledError[axis] = 1.0f;
+ }
+
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + directFeedForward;
pidData[axis].Sum = pidSum * scaledAxisPid[axis];
}
@@ -855,3 +902,7 @@ bool crashRecoveryModeActive(void) {
float pidGetPreviousSetpoint(int axis) {
return previousPidSetpoint[axis];
}
+
+float getDtermPercentLeft(int axis) {
+ return inverseScaledError[axis];
+}
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 1d00580d63..0af8895e23 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -154,6 +154,10 @@ typedef struct pidProfile_s {
uint16_t dterm_ABG_alpha;
uint16_t dterm_ABG_boost;
uint8_t dterm_ABG_half_life;
+ uint8_t emuBoost2;
+ uint8_t emuBoost2_filter;
+ uint8_t emuBoost2_cutoff;
+ uint8_t emuBoost2_expo;
} pidProfile_t;
#ifndef USE_OSD_SLAVE
@@ -198,3 +202,4 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
bool crashRecoveryModeActive(void);
float pidGetPreviousSetpoint(int axis);
void pidUpdateEmuGravityThrottleFilter(float throttle);
+float getDtermPercentLeft(int axis);
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 79ab8441e9..bbb177f52f 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -847,6 +847,11 @@ const clivalue_t valueTable[] = {
{ "runaway_takeoff_deactivate_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
#endif
// PG_PID_PROFILE
+ { "emuboost2", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2) },
+ { "emuboost2_filter", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_filter) },
+ { "emuboost2_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_cutoff) },
+ { "emuboost2_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_expo) },
+
{ "dterm_abg_alpha", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) },
{ "dterm_abg_boost", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_boost) },
{ "dterm_abg_half_life", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_half_life) },
From a7a742d3466f540950b2d11f14d3177492745ea4 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Sun, 20 Jun 2021 02:43:08 -0600
Subject: [PATCH 08/19] remove crossfeed
---
src/main/common/filter.c | 4 ----
src/main/common/filter.h | 2 --
2 files changed, 6 deletions(-)
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 1dbb891a2e..c47c7baeda 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -275,7 +275,3 @@ FAST_CODE float ptnFilterApply(ptnFilter_t *filter, float input) {
return filter->state[filter->order];
} // ptnFilterApply
-
-FAST_CODE float crossFeed(float crossFeed, float preFiltered, float postFiltered) {
- return (preFiltered * crossFeed) + (postFiltered * (1.0f - crossFeed));
-}
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index b551ab95da..ea81c5d284 100755
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -96,5 +96,3 @@ float alphaBetaGammaApply(alphaBetaGammaFilter_t *filter, float input);
void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut, float dT);
void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float ScaleF, float dt);
float ptnFilterApply(ptnFilter_t *filter, float input);
-
-float crossFeed(float crossFeed, float preFiltered, float postFiltered);
From 3fa078c6dcf146f105159efc9e8695b3466f1944 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Sun, 20 Jun 2021 12:50:13 -0600
Subject: [PATCH 09/19] Update pid.c
---
src/main/flight/pid.c | 1 -
1 file changed, 1 deletion(-)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b1da8aed7f..4ac3180e89 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -643,7 +643,6 @@ static FAST_RAM_ZERO_INIT float lastRcDeflectionAbs[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousDtermErrorRate[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float accumulativeError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float axisLock[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float inverseScaledError[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;
From e21315bdd8d9626b072038cabe41a01c128251bd Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Mon, 21 Jun 2021 23:55:02 -0600
Subject: [PATCH 10/19] dterm dyn notch
---
src/main/common/filter.c | 4 +--
src/main/flight/pid.c | 28 +++++++++++++++++++
src/main/sensors/gyro.c | 2 +-
src/main/sensors/gyro.h | 1 +
src/main/sensors/gyro_filter_impl.h | 2 +-
src/main/sensors/gyroanalyse.c | 42 ++++++++++++++++-------------
src/main/sensors/gyroanalyse.h | 3 ++-
7 files changed, 59 insertions(+), 23 deletions(-)
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 039d309334..fbdc018d69 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -255,13 +255,13 @@ FAST_CODE void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut,
Adj_f_cut = (float)f_cut * ScaleF[filter->order - 1];
- filter->k = dT / ((1.0f / (2.0f * M_PI_FLOAT * Adj_f_cut)) + dT);
+ filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
} // ptnFilterInit
FAST_CODE void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float ScaleF, float dT) {
float Adj_f_cut;
Adj_f_cut = (float)f_cut * ScaleF;
- filter->k = dT / ((1.0f / (2.0f * M_PI_FLOAT * Adj_f_cut)) + dT);
+ filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
}
FAST_CODE float ptnFilterApply(ptnFilter_t *filter, float input) {
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 4ac3180e89..caeb74c87c 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -58,6 +58,10 @@
#include "sensors/acceleration.h"
#include "sensors/battery.h"
+#ifdef USE_GYRO_DATA_ANALYSE
+#include "sensors/gyroanalyse.h"
+#endif
+
const char pidNames[] =
"ROLL;"
"PITCH;"
@@ -232,6 +236,7 @@ static FAST_RAM filterApplyFnPtr angleSetpointFilterApplyFn = nullFilterApply;
static FAST_RAM_ZERO_INIT pt1Filter_t angleSetpointFilter[2];
static FAST_RAM filterApplyFnPtr dtermABGapplyFn = nullFilterApply;
static FAST_RAM_ZERO_INIT alphaBetaGammaFilter_t dtermABG[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT][5];
#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
@@ -252,6 +257,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
dtermLowpassApplyFn = nullFilterApply;
dtermLowpass2ApplyFn = nullFilterApply;
angleSetpointFilterApplyFn = nullFilterApply;
+
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (pidProfile->dFilter[axis].dLpf && pidProfile->dFilter[axis].dLpf <= pidFrequencyNyquist) {
switch (pidProfile->dterm_filter_type) {
@@ -277,6 +283,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
break;
}
}
+
if (pidProfile->dFilter[axis].dLpf2 && pidProfile->dFilter[axis].dLpf2 <= pidFrequencyNyquist) {
switch (pidProfile->dterm_filter_type) {
case FILTER_BIQUAD:
@@ -301,15 +308,24 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
break;
}
}
+
if (pidProfile->angle_filter) {
angleSetpointFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
pt1FilterInit(&angleSetpointFilter[axis], pt1FilterGain(pidProfile->angle_filter, dT));
}
+
if (pidProfile->dterm_ABG_alpha) {
dtermABGapplyFn = (filterApplyFnPtr)alphaBetaGammaApply;
ABGInit(&dtermABG[axis], pidProfile->dterm_ABG_alpha, pidProfile->dterm_ABG_boost, pidProfile->dterm_ABG_half_life, dT);
}
+
+ if (isDynamicFilterActive()) {
+ for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
+ biquadFilterInit(&dtermNotch[axis][axis2], 400, targetPidLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
+ }
+ }
}
+
#if defined(USE_THROTTLE_BOOST)
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
#endif
@@ -645,6 +661,7 @@ static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousDtermErrorRate[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float axisLock[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float inverseScaledError[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5];
static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) {
@@ -788,6 +805,17 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
if (pidCoefficient[axis].Kd > 0) {
float dDelta = ((feathered_pids * pureMeasurement) * inverseScaledError[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
//filter the dterm
+#ifdef USE_GYRO_DATA_ANALYSE
+ if (isDynamicFilterActive()) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) {
+ previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p);
+ biquadFilterUpdate(&dtermNotch[axis][p], previousNotchCenterFreq[axis][p], targetPidLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
+ }
+ dDelta = biquadFilterApplyDF1(&dtermNotch[axis][p], dDelta);
+ }
+ }
+#endif
dDelta = dtermLowpassApplyFn((filter_t *)&dtermLowpass[axis], dDelta);
dDelta = dtermLowpass2ApplyFn((filter_t *)&dtermLowpass2[axis], dDelta);
dDelta = dtermABGapplyFn((filter_t *)&dtermABG[axis], dDelta);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 99f6ffb9e9..c24f75d3b8 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -808,7 +808,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin
}
#ifdef USE_GYRO_DATA_ANALYSE
-static bool isDynamicFilterActive(void) {
+bool isDynamicFilterActive(void) {
return feature(FEATURE_DYNAMIC_FILTER);
}
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index c5f9555893..64570346b9 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -185,3 +185,4 @@ bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
float applySmithPredictor(smithPredictor_t *smithPredictor, float gyroFiltered);
+bool isDynamicFilterActive(void);
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index b4b023e483..0c0d1469e7 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/src/main/sensors/gyro_filter_impl.h
@@ -43,7 +43,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) {
}
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][p], gyroADCf);
}
if (axis == X) {
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index ee9fd20a57..dad6e5859c 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -117,6 +117,7 @@ static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
static float FAST_RAM_ZERO_INIT smoothFactor;
static uint8_t FAST_RAM_ZERO_INIT numSamples;
+static float FAST_RAM_ZERO_INIT centerFreq[3][5];
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
@@ -149,7 +150,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
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
- 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);
}
}
@@ -161,8 +162,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
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 < gyroConfig()->dyn_notch_count; p++) {
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; 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)gyroConfig()->dyn_notch_count + dynNotchMinHz;
}
@@ -170,7 +171,7 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
}
// 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;
}
@@ -186,7 +187,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) {
@@ -204,7 +205,7 @@ 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++) {
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftPushBatch(&sdft[axis], &state->downsampledGyroData[axis], &state->sampleCount);
}
state->sampleCount++;
@@ -234,7 +235,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;
@@ -246,20 +247,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 < gyroConfig()->dyn_notch_count; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; 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 < gyroConfig()->dyn_notch_count; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
if (sdftData[bin] > peaks[p].value) {
- for (uint8_t k = gyroConfig()->dyn_notch_count - 1; k > p; k--) {
+ for (int k = gyroConfig()->dyn_notch_count - 1; k > p; k--) {
peaks[k] = peaks[k - 1];
}
peaks[p].bin = bin;
@@ -272,8 +273,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 = gyroConfig()->dyn_notch_count - 1; p > 0; p--) {
- for (uint8_t k = 0; k < p; k++) {
+ for (int p = gyroConfig()->dyn_notch_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 (peaks[k].bin > peaks[k + 1].bin && peaks[k + 1].bin != 0) {
@@ -290,7 +291,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
}
case STEP_CALC_FREQUENCIES: // 4us @ F722
{
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; 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) {
@@ -301,7 +302,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
float sdftWeightedSum = squaredData * peaks[p].bin;
// accumulate upper shoulder unless it would be sdftEndBin
- uint8_t shoulderBin = peaks[p].bin + 1;
+ int shoulderBin = peaks[p].bin + 1;
if (shoulderBin < sdftEndBin) {
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
@@ -338,13 +339,13 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
}
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
}
}
if (state->updateAxis == X) {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count && p < 3; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count && p < 3; p++) {
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
}
}
@@ -355,10 +356,11 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
}
case STEP_UPDATE_FILTERS: // 7us @ F722
{
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (int p = 0; p < gyroConfig()->dyn_notch_count; 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) {
biquadFilterUpdate(&state->notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
+ centerFreq[state->updateAxis][p] = state->centerFreq[state->updateAxis][p];
}
}
@@ -380,4 +382,8 @@ void resetMaxFFT(void) {
dynNotchMaxFFT = 0;
}
+float getCenterFreq(int axis, int peak) {
+ return centerFreq[axis][peak];
+}
+
#endif // USE_GYRO_DATA_ANALYSE
diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h
index 5c9d1b8774..6b499ca732 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -44,7 +44,8 @@ typedef struct gyroAnalyseState_s {
} gyroAnalyseState_t;
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
-void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
+void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample);
void gyroDataAnalyse(gyroAnalyseState_t *state);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
+float getCenterFreq(int axis, int peak);
From e2cc8c7e334e82cff58805532f2a6340eba82c97 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 24 Jun 2021 09:28:58 -0600
Subject: [PATCH 11/19] dterm dyn notch can be tuned
---
src/main/flight/pid.c | 22 +++++++---------------
src/main/flight/pid.h | 2 ++
src/main/interface/settings.c | 2 ++
3 files changed, 11 insertions(+), 15 deletions(-)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index caeb74c87c..a3e560d405 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -200,6 +200,8 @@ void resetPidProfile(pidProfile_t *pidProfile) {
.emuBoost2_filter = 20,
.emuBoost2_cutoff = 10,
.emuBoost2_expo = 25,
+ .dtermDynNotch = true,
+ .dterm_dyn_notch_q = 350,
);
}
@@ -806,11 +808,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float dDelta = ((feathered_pids * pureMeasurement) * inverseScaledError[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
//filter the dterm
#ifdef USE_GYRO_DATA_ANALYSE
- if (isDynamicFilterActive()) {
+ if (isDynamicFilterActive() && pidProfile->dtermDynNotch) {
for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) {
previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p);
- biquadFilterUpdate(&dtermNotch[axis][p], previousNotchCenterFreq[axis][p], targetPidLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
+ biquadFilterUpdate(&dtermNotch[axis][p], previousNotchCenterFreq[axis][p], targetPidLooptime, pidProfile->dterm_dyn_notch_q / 100.0f, FILTER_NOTCH);
}
dDelta = biquadFilterApplyDF1(&dtermNotch[axis][p], dDelta);
}
@@ -883,11 +885,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// emuboost 2.0
if (pidProfile->emuBoost2) {
- //need to add logging, lol
float changeError = pureError * pidFrequency * DTERM_SCALE * 20.0f;
changeError = changeError / pidProfile->emuBoost2_cutoff;
- float doSignMatch = 0;
changeError = constrainf(changeError, -1.0f, 1.0f);
changeError = ABS(changeError) * power3(changeError) * (pidProfile->emuBoost2_expo / 100.0f) + changeError * (1 - pidProfile->emuBoost2_expo / 100.0f);
@@ -896,22 +896,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
scaledError *= changeError;
scaledError = pt1FilterApply(&emuboost2_0Filter[axis], scaledError);
- if (axis == FD_ROLL || axis == FD_PITCH) {
- DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
- }
+ DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
+ inverseScaledError[axis] = 1.0f - scaledError; // this is only 1 when the error and change in setpoint are high
if (scaledError > 0.0f) {
- inverseScaledError[axis] = 1 - scaledError; // this is only 1 when the error and change in setpoint are high
-
pidData[axis].P *= ((pidProfile->emuBoost2 / 100.0f) * scaledError) + 1.0f;
- //pidData[axis].D *= inverseScaledError[axis];
- doSignMatch = 1000.0f;
+ //pidData[axis].D *= inverseScaledError[axis]; // this gets applied elsewhere to the dterm so that the feathered part of dterm is the only part effected
} else {
inverseScaledError[axis] = 1.0f;
- doSignMatch = 0.0f;
- }
- if (axis == FD_ROLL || axis == FD_PITCH) {
- DEBUG_SET(DEBUG_EMUBOOST, axis + 2, lrintf(doSignMatch));
}
} else {
inverseScaledError[axis] = 1.0f;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 0af8895e23..f25787a712 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -158,6 +158,8 @@ typedef struct pidProfile_s {
uint8_t emuBoost2_filter;
uint8_t emuBoost2_cutoff;
uint8_t emuBoost2_expo;
+ uint8_t dtermDynNotch;
+ uint16_t dterm_dyn_notch_q;
} pidProfile_t;
#ifndef USE_OSD_SLAVE
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index a44df858fb..5e42d2b693 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -852,6 +852,8 @@ const clivalue_t valueTable[] = {
{ "emuboost2_filter", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_filter) },
{ "emuboost2_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_cutoff) },
{ "emuboost2_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_expo) },
+ { "dterm_dyn_notch_enable", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermDynNotch) },
+ { "dterm_dyn_notch_q", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_dyn_notch_q) },
{ "dterm_abg_alpha", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) },
{ "dterm_abg_boost", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_boost) },
From 8b3f9a99aabe6f7c4edb4ae32bca41ec34dd98c9 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 24 Jun 2021 09:31:44 -0600
Subject: [PATCH 12/19] Allow Dterm Boosting
as error is decreasing dterm will be boosted, while error is increasing dterm will be shrunk. The amount dterm is boosted or shrunk depends on how quickly error is increasing/decreasing. The faster it is changing the more we will boost/shrink d. As error transitions from growing to shrinking the dterm will go from being shrunk, back to normal strength, and then to boosted, then back to normal strength. Gain scheduling FTW :)
---
src/main/flight/pid.c | 2 --
1 file changed, 2 deletions(-)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a3e560d405..d1144c46d6 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -902,8 +902,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
if (scaledError > 0.0f) {
pidData[axis].P *= ((pidProfile->emuBoost2 / 100.0f) * scaledError) + 1.0f;
//pidData[axis].D *= inverseScaledError[axis]; // this gets applied elsewhere to the dterm so that the feathered part of dterm is the only part effected
- } else {
- inverseScaledError[axis] = 1.0f;
}
} else {
inverseScaledError[axis] = 1.0f;
From b4d09140087958e2ff076099bcf47b1b7f185d24 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 24 Jun 2021 09:33:57 -0600
Subject: [PATCH 13/19] sharpness osd change
Changing sharpness in the osd to increment by 1 for Andrey
---
src/main/cms/cms_menu_imu.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 13be0fa0b1..cf4beca8ea 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -699,7 +699,7 @@ static OSD_Entry cmsx_menuImufEntries[] = {
{ "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 0, 16000, 100 }, 0 },
{ "PITCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_q, 0, 16000, 100 }, 0 },
{ "YAW Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_q, 0, 16000, 100 }, 0 },
- { "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 5 }, 0 },
+ { "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 1 }, 0 },
{ "ROLL LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_lpf_cutoff_hz, 0, 450, 1 }, 0 },
{ "PITCH LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_lpf_cutoff_hz, 0, 450, 1 }, 0 },
{ "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_lpf_cutoff_hz, 0, 450, 1 }, 0 },
From 00d81bdb87ac846419df23dd330019b11276ad3d Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 11 Aug 2021 23:48:03 -0600
Subject: [PATCH 14/19] Revert "sharpness osd change"
This reverts commit b4d09140087958e2ff076099bcf47b1b7f185d24.
---
src/main/cms/cms_menu_imu.c | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index cf4beca8ea..13be0fa0b1 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -699,7 +699,7 @@ static OSD_Entry cmsx_menuImufEntries[] = {
{ "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 0, 16000, 100 }, 0 },
{ "PITCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_q, 0, 16000, 100 }, 0 },
{ "YAW Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_q, 0, 16000, 100 }, 0 },
- { "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 1 }, 0 },
+ { "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 5 }, 0 },
{ "ROLL LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_lpf_cutoff_hz, 0, 450, 1 }, 0 },
{ "PITCH LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_lpf_cutoff_hz, 0, 450, 1 }, 0 },
{ "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_lpf_cutoff_hz, 0, 450, 1 }, 0 },
From 2f91db1cb2ef0d2d80fcdb0fd6a5868470ca2b8d Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 11 Aug 2021 23:50:00 -0600
Subject: [PATCH 15/19] revert dyn dterm notch code
---
src/main/flight/pid.c | 24 +++++++++++++++++-------
src/main/flight/pid.h | 2 --
src/main/interface/settings.c | 2 --
3 files changed, 17 insertions(+), 11 deletions(-)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index d1144c46d6..caeb74c87c 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -200,8 +200,6 @@ void resetPidProfile(pidProfile_t *pidProfile) {
.emuBoost2_filter = 20,
.emuBoost2_cutoff = 10,
.emuBoost2_expo = 25,
- .dtermDynNotch = true,
- .dterm_dyn_notch_q = 350,
);
}
@@ -808,11 +806,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float dDelta = ((feathered_pids * pureMeasurement) * inverseScaledError[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
//filter the dterm
#ifdef USE_GYRO_DATA_ANALYSE
- if (isDynamicFilterActive() && pidProfile->dtermDynNotch) {
+ if (isDynamicFilterActive()) {
for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) {
previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p);
- biquadFilterUpdate(&dtermNotch[axis][p], previousNotchCenterFreq[axis][p], targetPidLooptime, pidProfile->dterm_dyn_notch_q / 100.0f, FILTER_NOTCH);
+ biquadFilterUpdate(&dtermNotch[axis][p], previousNotchCenterFreq[axis][p], targetPidLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
}
dDelta = biquadFilterApplyDF1(&dtermNotch[axis][p], dDelta);
}
@@ -885,9 +883,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// emuboost 2.0
if (pidProfile->emuBoost2) {
+ //need to add logging, lol
float changeError = pureError * pidFrequency * DTERM_SCALE * 20.0f;
changeError = changeError / pidProfile->emuBoost2_cutoff;
+ float doSignMatch = 0;
changeError = constrainf(changeError, -1.0f, 1.0f);
changeError = ABS(changeError) * power3(changeError) * (pidProfile->emuBoost2_expo / 100.0f) + changeError * (1 - pidProfile->emuBoost2_expo / 100.0f);
@@ -896,12 +896,22 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
scaledError *= changeError;
scaledError = pt1FilterApply(&emuboost2_0Filter[axis], scaledError);
- DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
- inverseScaledError[axis] = 1.0f - scaledError; // this is only 1 when the error and change in setpoint are high
+ if (axis == FD_ROLL || axis == FD_PITCH) {
+ DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
+ }
if (scaledError > 0.0f) {
+ inverseScaledError[axis] = 1 - scaledError; // this is only 1 when the error and change in setpoint are high
+
pidData[axis].P *= ((pidProfile->emuBoost2 / 100.0f) * scaledError) + 1.0f;
- //pidData[axis].D *= inverseScaledError[axis]; // this gets applied elsewhere to the dterm so that the feathered part of dterm is the only part effected
+ //pidData[axis].D *= inverseScaledError[axis];
+ doSignMatch = 1000.0f;
+ } else {
+ inverseScaledError[axis] = 1.0f;
+ doSignMatch = 0.0f;
+ }
+ if (axis == FD_ROLL || axis == FD_PITCH) {
+ DEBUG_SET(DEBUG_EMUBOOST, axis + 2, lrintf(doSignMatch));
}
} else {
inverseScaledError[axis] = 1.0f;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index f25787a712..0af8895e23 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -158,8 +158,6 @@ typedef struct pidProfile_s {
uint8_t emuBoost2_filter;
uint8_t emuBoost2_cutoff;
uint8_t emuBoost2_expo;
- uint8_t dtermDynNotch;
- uint16_t dterm_dyn_notch_q;
} pidProfile_t;
#ifndef USE_OSD_SLAVE
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 5e42d2b693..a44df858fb 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -852,8 +852,6 @@ const clivalue_t valueTable[] = {
{ "emuboost2_filter", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_filter) },
{ "emuboost2_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_cutoff) },
{ "emuboost2_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_expo) },
- { "dterm_dyn_notch_enable", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermDynNotch) },
- { "dterm_dyn_notch_q", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_dyn_notch_q) },
{ "dterm_abg_alpha", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) },
{ "dterm_abg_boost", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_boost) },
From 19b2f19a8a34b67235a57054bb3843f978a2588c Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 11 Aug 2021 23:50:15 -0600
Subject: [PATCH 16/19] Revert "dterm dyn notch"
This reverts commit e21315bdd8d9626b072038cabe41a01c128251bd.
---
src/main/common/filter.c | 4 +--
src/main/flight/pid.c | 28 -------------------
src/main/sensors/gyro.c | 2 +-
src/main/sensors/gyro.h | 1 -
src/main/sensors/gyro_filter_impl.h | 2 +-
src/main/sensors/gyroanalyse.c | 42 +++++++++++++----------------
src/main/sensors/gyroanalyse.h | 3 +--
7 files changed, 23 insertions(+), 59 deletions(-)
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index fbdc018d69..039d309334 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -255,13 +255,13 @@ FAST_CODE void ptnFilterInit(ptnFilter_t *filter, uint8_t order, uint16_t f_cut,
Adj_f_cut = (float)f_cut * ScaleF[filter->order - 1];
- filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
+ filter->k = dT / ((1.0f / (2.0f * M_PI_FLOAT * Adj_f_cut)) + dT);
} // ptnFilterInit
FAST_CODE void ptnFilterUpdate(ptnFilter_t *filter, float f_cut, float ScaleF, float dT) {
float Adj_f_cut;
Adj_f_cut = (float)f_cut * ScaleF;
- filter->k = dT / ((1.0f / (2.0f * M_PIf * Adj_f_cut)) + dT);
+ filter->k = dT / ((1.0f / (2.0f * M_PI_FLOAT * Adj_f_cut)) + dT);
}
FAST_CODE float ptnFilterApply(ptnFilter_t *filter, float input) {
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index caeb74c87c..4ac3180e89 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -58,10 +58,6 @@
#include "sensors/acceleration.h"
#include "sensors/battery.h"
-#ifdef USE_GYRO_DATA_ANALYSE
-#include "sensors/gyroanalyse.h"
-#endif
-
const char pidNames[] =
"ROLL;"
"PITCH;"
@@ -236,7 +232,6 @@ static FAST_RAM filterApplyFnPtr angleSetpointFilterApplyFn = nullFilterApply;
static FAST_RAM_ZERO_INIT pt1Filter_t angleSetpointFilter[2];
static FAST_RAM filterApplyFnPtr dtermABGapplyFn = nullFilterApply;
static FAST_RAM_ZERO_INIT alphaBetaGammaFilter_t dtermABG[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT][5];
#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
@@ -257,7 +252,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
dtermLowpassApplyFn = nullFilterApply;
dtermLowpass2ApplyFn = nullFilterApply;
angleSetpointFilterApplyFn = nullFilterApply;
-
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (pidProfile->dFilter[axis].dLpf && pidProfile->dFilter[axis].dLpf <= pidFrequencyNyquist) {
switch (pidProfile->dterm_filter_type) {
@@ -283,7 +277,6 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
break;
}
}
-
if (pidProfile->dFilter[axis].dLpf2 && pidProfile->dFilter[axis].dLpf2 <= pidFrequencyNyquist) {
switch (pidProfile->dterm_filter_type) {
case FILTER_BIQUAD:
@@ -308,24 +301,15 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
break;
}
}
-
if (pidProfile->angle_filter) {
angleSetpointFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
pt1FilterInit(&angleSetpointFilter[axis], pt1FilterGain(pidProfile->angle_filter, dT));
}
-
if (pidProfile->dterm_ABG_alpha) {
dtermABGapplyFn = (filterApplyFnPtr)alphaBetaGammaApply;
ABGInit(&dtermABG[axis], pidProfile->dterm_ABG_alpha, pidProfile->dterm_ABG_boost, pidProfile->dterm_ABG_half_life, dT);
}
-
- if (isDynamicFilterActive()) {
- for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
- biquadFilterInit(&dtermNotch[axis][axis2], 400, targetPidLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
- }
- }
}
-
#if defined(USE_THROTTLE_BOOST)
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
#endif
@@ -661,7 +645,6 @@ static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousDtermErrorRate[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float axisLock[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float inverseScaledError[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5];
static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) {
@@ -805,17 +788,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
if (pidCoefficient[axis].Kd > 0) {
float dDelta = ((feathered_pids * pureMeasurement) * inverseScaledError[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
//filter the dterm
-#ifdef USE_GYRO_DATA_ANALYSE
- if (isDynamicFilterActive()) {
- for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
- if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) {
- previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p);
- biquadFilterUpdate(&dtermNotch[axis][p], previousNotchCenterFreq[axis][p], targetPidLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
- }
- dDelta = biquadFilterApplyDF1(&dtermNotch[axis][p], dDelta);
- }
- }
-#endif
dDelta = dtermLowpassApplyFn((filter_t *)&dtermLowpass[axis], dDelta);
dDelta = dtermLowpass2ApplyFn((filter_t *)&dtermLowpass2[axis], dDelta);
dDelta = dtermABGapplyFn((filter_t *)&dtermABG[axis], dDelta);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index c24f75d3b8..99f6ffb9e9 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -808,7 +808,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin
}
#ifdef USE_GYRO_DATA_ANALYSE
-bool isDynamicFilterActive(void) {
+static bool isDynamicFilterActive(void) {
return feature(FEATURE_DYNAMIC_FILTER);
}
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 64570346b9..c5f9555893 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -185,4 +185,3 @@ bool gyroYawSpinDetected(void);
uint16_t gyroAbsRateDps(int axis);
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
float applySmithPredictor(smithPredictor_t *smithPredictor, float gyroFiltered);
-bool isDynamicFilterActive(void);
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index 0c0d1469e7..b4b023e483 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/src/main/sensors/gyro_filter_impl.h
@@ -43,7 +43,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) {
}
gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
- for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][p], gyroADCf);
}
if (axis == X) {
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index dad6e5859c..ee9fd20a57 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -117,7 +117,6 @@ static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
static float FAST_RAM_ZERO_INIT smoothFactor;
static uint8_t FAST_RAM_ZERO_INIT numSamples;
-static float FAST_RAM_ZERO_INIT centerFreq[3][5];
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
@@ -150,7 +149,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
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
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
}
}
@@ -162,8 +161,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
state->maxSampleCount = numSamples;
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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)gyroConfig()->dyn_notch_count + dynNotchMinHz;
}
@@ -171,7 +170,7 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
}
// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function
-void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
+void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample)
{
state->oversampledGyroAccumulator[axis] += sample;
}
@@ -187,7 +186,7 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
state->sampleCount = 0;
// calculate mean value of accumulated samples
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ 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) {
@@ -205,7 +204,7 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
// 2us @ F722
// SDFT processing in batches to synchronize with incoming downsampled data
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftPushBatch(&sdft[axis], &state->downsampledGyroData[axis], &state->sampleCount);
}
state->sampleCount++;
@@ -235,7 +234,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
// Calculate mean square over frequency range (= average power of vibrations)
sdftMeanSq = 0.0f;
- for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
+ 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;
@@ -247,20 +246,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 (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
peaks[p].bin = 0;
peaks[p].value = 0.0f;
}
// Search for N biggest peaks in frequency spectrum
- for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) {
+ 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 (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
if (sdftData[bin] > peaks[p].value) {
- for (int k = gyroConfig()->dyn_notch_count - 1; k > p; k--) {
+ for (uint8_t k = gyroConfig()->dyn_notch_count - 1; k > p; k--) {
peaks[k] = peaks[k - 1];
}
peaks[p].bin = bin;
@@ -273,8 +272,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 (int p = gyroConfig()->dyn_notch_count - 1; p > 0; p--) {
- for (int k = 0; k < p; k++) {
+ for (uint8_t p = gyroConfig()->dyn_notch_count - 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) {
@@ -291,7 +290,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
}
case STEP_CALC_FREQUENCIES: // 4us @ F722
{
- for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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) {
@@ -302,7 +301,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
float sdftWeightedSum = squaredData * peaks[p].bin;
// accumulate upper shoulder unless it would be sdftEndBin
- int shoulderBin = peaks[p].bin + 1;
+ uint8_t shoulderBin = peaks[p].bin + 1;
if (shoulderBin < sdftEndBin) {
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
@@ -339,13 +338,13 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
}
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
- for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
}
}
if (state->updateAxis == X) {
- for (int p = 0; p < gyroConfig()->dyn_notch_count && p < 3; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count && p < 3; p++) {
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
}
}
@@ -356,11 +355,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
}
case STEP_UPDATE_FILTERS: // 7us @ F722
{
- for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
+ for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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) {
biquadFilterUpdate(&state->notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
- centerFreq[state->updateAxis][p] = state->centerFreq[state->updateAxis][p];
}
}
@@ -382,8 +380,4 @@ void resetMaxFFT(void) {
dynNotchMaxFFT = 0;
}
-float getCenterFreq(int axis, int peak) {
- return centerFreq[axis][peak];
-}
-
#endif // USE_GYRO_DATA_ANALYSE
diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h
index 6b499ca732..5c9d1b8774 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -44,8 +44,7 @@ typedef struct gyroAnalyseState_s {
} gyroAnalyseState_t;
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
-void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample);
+void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
void gyroDataAnalyse(gyroAnalyseState_t *state);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
-float getCenterFreq(int axis, int peak);
From b5978455b30188ee302d3e8b8a2e24ba1c50848e Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Wed, 11 Aug 2021 23:50:24 -0600
Subject: [PATCH 17/19] Revert "Merge branch
'upgrade-the-dyn-notch-filter-in-emu' into emuboost2"
This reverts commit dcef34eb6ad16a1183d2e2d336a0bbcd279aa848, reversing
changes made to 3fa078c6dcf146f105159efc9e8695b3466f1944.
---
make/source.mk | 9 +-
src/main/blackbox/blackbox.c | 3 +-
src/main/cms/cms_menu_imu.c | 44 ++-
src/main/common/filter.c | 6 +-
src/main/common/maths.h | 3 -
src/main/common/sdft.c | 156 ----------
src/main/common/sdft.h | 54 ----
src/main/interface/msp.c | 4 +-
src/main/interface/settings.c | 3 +-
src/main/sensors/gyro.c | 25 +-
src/main/sensors/gyro.h | 3 +-
src/main/sensors/gyro_filter_impl.h | 21 +-
src/main/sensors/gyroanalyse.c | 466 +++++++++++++---------------
src/main/sensors/gyroanalyse.h | 29 +-
14 files changed, 316 insertions(+), 510 deletions(-)
delete mode 100644 src/main/common/sdft.c
delete mode 100644 src/main/common/sdft.h
diff --git a/make/source.mk b/make/source.mk
index ad1a5d0f7a..88ee3ece83 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -190,7 +190,6 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
common/encoding.c \
common/filter.c \
common/maths.c \
- common/sdft.c \
common/typeconversion.c \
drivers/accgyro/accgyro_fake.c \
drivers/accgyro/accgyro_mpu.c \
@@ -336,6 +335,14 @@ 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)
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 05c2d9772d..58b41039cc 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1233,8 +1233,7 @@ static bool blackboxWriteSysinfo(void) {
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
#if defined(USE_GYRO_DATA_ANALYSE)
- BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_width", "%d", gyroConfig()->dyn_notch_q);
- BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_count", "%d", gyroConfig()->dyn_notch_count);
+ BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_q", "%d", gyroConfig()->dyn_notch_q_factor);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
#endif
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 13be0fa0b1..1cef7cc0bc 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -508,10 +508,13 @@ static uint16_t gyroConfig_gyro_lowpass_hz_yaw;
static uint16_t gyroConfig_gyro_lowpass2_hz_roll;
static uint16_t gyroConfig_gyro_lowpass2_hz_pitch;
static uint16_t gyroConfig_gyro_lowpass2_hz_yaw;
-static uint16_t gyroConfig_gyro_q;
-static uint8_t gyroConfig_gyro_notch_count;
-static uint16_t gyroConfig_gyro_notch_min_hz;
-static uint16_t gyroConfig_gyro_notch_max_hz;
+static uint16_t gyroConfig_gyro_soft_notch_hz_1;
+static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
+static uint16_t gyroConfig_gyro_soft_notch_hz_2;
+static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
+static uint16_t gyroConfig_gyro_matrix_q;
+static uint16_t gyroConfig_gyro_matrix_min_hz;
+static uint16_t gyroConfig_gyro_matrix_max_hz;
static uint16_t gyroConfig_gyro_abg_alpha;
static uint16_t gyroConfig_gyro_abg_boost;
static uint8_t gyroConfig_gyro_abg_half_life;
@@ -537,10 +540,13 @@ static long cmsx_menuGyro_onEnter(void) {
gyroConfig_gyro_lowpass2_hz_roll = gyroConfig()->gyro_lowpass2_hz[ROLL];
gyroConfig_gyro_lowpass2_hz_pitch = gyroConfig()->gyro_lowpass2_hz[PITCH];
gyroConfig_gyro_lowpass2_hz_yaw = gyroConfig()->gyro_lowpass2_hz[YAW];
- gyroConfig_gyro_q = gyroConfig()->dyn_notch_q;
- gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count;
- gyroConfig_gyro_notch_min_hz = gyroConfig()->dyn_notch_min_hz;
- gyroConfig_gyro_notch_max_hz = gyroConfig()->dyn_notch_max_hz;
+ gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
+ gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
+ gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
+ gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
+ gyroConfig_gyro_matrix_q = gyroConfig()->dyn_notch_q_factor;
+ gyroConfig_gyro_matrix_min_hz = gyroConfig()->dyn_notch_min_hz;
+ gyroConfig_gyro_matrix_max_hz = gyroConfig()->dyn_notch_max_hz;
gyroConfig_gyro_abg_alpha = gyroConfig()->gyro_ABG_alpha;
gyroConfig_gyro_abg_boost = gyroConfig()->gyro_ABG_boost;
gyroConfig_gyro_abg_half_life = gyroConfig()->gyro_ABG_half_life;
@@ -570,10 +576,13 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) {
gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = gyroConfig_gyro_lowpass2_hz_roll;
gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = gyroConfig_gyro_lowpass2_hz_pitch;
gyroConfigMutable()->gyro_lowpass2_hz[YAW] = gyroConfig_gyro_lowpass2_hz_yaw;
- gyroConfigMutable()->dyn_notch_q = gyroConfig_gyro_q;
- gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count;
- gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_notch_min_hz;
- gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_notch_max_hz;
+ gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
+ gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
+ gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
+ gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
+ gyroConfigMutable()->dyn_notch_q_factor = gyroConfig_gyro_matrix_q;
+ gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_matrix_min_hz;
+ gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_matrix_max_hz;
gyroConfigMutable()->gyro_ABG_alpha = gyroConfig_gyro_abg_alpha;
gyroConfigMutable()->gyro_ABG_boost = gyroConfig_gyro_abg_boost;
gyroConfigMutable()->gyro_ABG_half_life = gyroConfig_gyro_abg_half_life;
@@ -605,10 +614,9 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
{ "GYRO LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_pitch, 0, 16000, 1 }, 0 },
{ "GYRO LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_yaw, 0, 16000, 1 }, 0 },
#endif
- { "DYN NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_q, 0, 1000, 1 }, 0 },
- { "DYN NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_notch_count, 1, 5, 1 }, 0 },
- { "DYN NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_min_hz, 30, 1000, 1 }, 0 },
- { "DYN NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_max_hz, 200, 1000, 1 }, 0 },
+ { "MATRIX Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_matrix_q, 0, 1000, 1 }, 0 },
+ { "MATRIX MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_matrix_min_hz, 30, 1000, 1 }, 0 },
+ { "MATRIX MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_matrix_max_hz, 200, 1000, 1 }, 0 },
#ifndef USE_GYRO_IMUF9001
{ "IMUF W", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w, 0, 512, 1 }, 0 },
{ "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 100, 16000, 100 }, 0 },
@@ -617,6 +625,10 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = {
{ "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 5 }, 0 },
#endif
+ { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
+ { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
+ { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
+ { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "GYRO ABG ALPHA", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_abg_alpha, 0, 1000, 1 }, 0 },
{ "GYRO ABG BOOST", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_abg_boost, 0, 2000, 1 }, 0 },
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 039d309334..c47c7baeda 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -31,6 +31,8 @@
#include "fc/fc_rc.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
@@ -44,7 +46,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input) {
// PT1 Low Pass filter
float pt1FilterGain(uint16_t f_cut, float dT) {
- const float RC = 0.5f / (M_PIf * f_cut);
+ const float RC = 0.5f / (M_PI_FLOAT * f_cut);
return dT / (RC + dT);
}
@@ -106,7 +108,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) {
// setup variables
- const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
+ const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
const float sn = sin_approx(omega);
const float cs = cos_approx(omega);
const float alpha = sn / (2.0f * Q);
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 8943e79b61..f6beead31d 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -37,9 +37,6 @@
#define M_PIf 3.14159265358979323846f
#define M_PI_HALFf 3.14159265358979323846f/2
#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)
diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c
deleted file mode 100644
index d3eb1f9c11..0000000000
--- a/src/main/common/sdft.c
+++ /dev/null
@@ -1,156 +0,0 @@
-/*
- * 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 .
- */
-
-#include
-#include
-
-#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_RAM_ZERO_INIT float rPowerN; // SDFT_R to the power of SDFT_SAMPLE_SIZE
-static FAST_RAM_ZERO_INIT bool isInitialized;
-static FAST_RAM_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 = 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++) {
- 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 fast square root approximation 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]);
- }
-}
diff --git a/src/main/common/sdft.h b/src/main/common/sdft.h
deleted file mode 100644
index aad349bf8a..0000000000
--- a/src/main/common/sdft.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * 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 .
- */
-
- // Implementation of a Sliding Discrete Fourier Transform (SDFT).
- // Complexity for calculating frequency spectrum with N bins is O(N).
-
-#pragma once
-
-#include
-#include
-#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);
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 4c7674ccd6..477f97de0e 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1192,7 +1192,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
sbufWriteU8(dst, 0); //was WitchCraft
sbufWriteU8(dst, 0); //was WitchCraft
sbufWriteU8(dst, 0); //was WitchCraft
- sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
+ sbufWriteU16(dst, gyroConfig()->dyn_notch_q_factor);
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
break;
/*#ifndef USE_GYRO_IMUF9001
@@ -1768,7 +1768,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
sbufReadU8(src); //was WitchCraft
sbufReadU8(src); //was WitchCraft
sbufReadU8(src); //was WitchCraft
- gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
+ gyroConfigMutable()->dyn_notch_q_factor = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index a44df858fb..bbb177f52f 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -538,8 +538,7 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
#if defined(USE_GYRO_DATA_ANALYSE)
- { "dynamic_gyro_notch_width", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
- { "dynamic_gyro_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
+ { "dynamic_gyro_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q_factor) },
{ "dynamic_gyro_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
{ "dynamic_gyro_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 400, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
#endif
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 99f6ffb9e9..9d7110c820 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -170,6 +170,7 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
+ biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
// overflow and recovery
timeUs_t overflowTimeUs;
@@ -181,6 +182,7 @@ typedef struct gyroSensor_s {
#ifdef USE_GYRO_DATA_ANALYSE
gyroAnalyseState_t gyroAnalyseState;
+ float dynNotchQ;
#endif
#ifdef USE_SMITH_PREDICTOR
@@ -247,8 +249,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
- .dyn_notch_q = 400,
- .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
+ .dyn_notch_q_factor = 400,
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
.imuf_mode = GTBCM_GYRO_ACC_FILTER_F,
@@ -303,8 +304,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
- .dyn_notch_q = 350,
- .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
+ .dyn_notch_q_factor = 350,
.dyn_notch_min_hz = 150,
.dyn_notch_max_hz = 600,
.gyro_ABG_alpha = 0,
@@ -533,6 +533,9 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) {
}
static bool gyroInitSensor(gyroSensor_t *gyroSensor) {
+#ifdef USE_GYRO_DATA_ANALYSE
+ gyroSensor->dynNotchQ = gyroConfig()->dyn_notch_q_factor / 100.0f;
+#endif
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160)
@@ -817,8 +820,8 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) {
if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) {
- biquadFilterInit(&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH);
+ for (int axis2 = 0; axis2 < XYZ_AXIS_COUNT; axis2++) {
+ biquadFilterInit(&gyroSensor->notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
}
}
}
@@ -1128,6 +1131,15 @@ float applySmithPredictor(smithPredictor_t *smithPredictor, float gyroFiltered)
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET
+static FAST_CODE void dynamicGyroNotchFiltersUpdate(gyroSensor_t* gyroSensor) {
+ if (gyroSensor->gyroAnalyseState.filterUpdateExecute) {
+ const uint8_t axis = gyroSensor->gyroAnalyseState.filterUpdateAxis;
+ const uint16_t frequency = gyroSensor->gyroAnalyseState.filterUpdateFrequency;
+ biquadFilterUpdate(&gyroSensor->notchFilterDyn[0][axis], frequency, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
+ biquadFilterUpdate(&gyroSensor->notchFilterDyn[1][axis], frequency, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
+ biquadFilterUpdate(&gyroSensor->notchFilterDyn[2][axis], frequency, gyro.targetLooptime, gyroSensor->dynNotchQ, FILTER_NOTCH);
+ }
+}
static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs_t currentTimeUs) {
#ifndef USE_DMA_SPI_DEVICE
@@ -1191,6 +1203,7 @@ static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState);
+ dynamicGyroNotchFiltersUpdate(gyroSensor);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index c5f9555893..25d76f739b 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -133,8 +133,7 @@ typedef struct gyroConfig_s {
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
- uint16_t dyn_notch_q;
- uint8_t dyn_notch_count;
+ uint16_t dyn_notch_q_factor;
uint16_t dyn_notch_min_hz;
uint16_t dyn_notch_max_hz;
#if defined(USE_GYRO_IMUF9001)
diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h
index b4b023e483..ec9829ff2f 100644
--- a/src/main/sensors/gyro_filter_impl.h
+++ b/src/main/sensors/gyro_filter_impl.h
@@ -37,15 +37,18 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) {
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
- if (axis == X) {
- GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
- GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
- }
-
- gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
- gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][p], gyroADCf);
- }
+ if (axis == X) {
+ GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
+ GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
+ }
+ }
+#endif
+#ifdef USE_GYRO_DATA_ANALYSE
+ if (isDynamicFilterActive()) {
+ gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
+ gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis][0], gyroADCf);
+ gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis][1], gyroADCf);
+ gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis][2], gyroADCf);
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
}
diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c
index ee9fd20a57..040eaa3780 100644
--- a/src/main/sensors/gyroanalyse.c
+++ b/src/main/sensors/gyroanalyse.c
@@ -19,16 +19,10 @@
*/
/* 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
#include
#include "platform.h"
@@ -38,7 +32,6 @@
#include "common/filter.h"
#include "common/maths.h"
-#include "common/sdft.h"
#include "common/time.h"
#include "common/utils.h"
@@ -51,323 +44,304 @@
#include "gyroanalyse.h"
-// SDFT_SAMPLE_SIZE defaults to 100 (common/sdft.h)
-// We get 50 frequency bins from 100 consecutive data values, called SDFT_BIN_COUNT (common/sdft.h)
+// FFT_WINDOW_SIZE defaults to 32 (gyroanalyse.h)
+// We get 16 frequency bins from 32 consecutive data values
// Bin 0 is DC and can't be used.
-// Only bins 1 to 49 are usable.
+// Only bins 1 to 15 are usable.
-// A gyro sample is collected every PID loop
+// A gyro sample is collected every gyro loop
// maxSampleCount recent gyro values are accumulated and averaged
-// to ensure that 100 samples are collected at the right rate for the required SDFT bandwidth
+// to ensure that 32 samples are collected at the right rate for the required FFT bandwidth
-// 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
+// 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
// 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, 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
-
-// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the corresponding SDFT.
+// 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 100 samples of the SDFT input buffer with clean new data takes 75ms
+// Hence to completely replace all 32 samples of the FFT input buffer with clean new data takes 24ms
-// 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.
+// 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.
// At 8k, any one axis gets updated at 8000 / 12 or 666hz or every 1.5ms
-// In this time, 2 points in the SDFT buffer will have changed.
+// 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
// 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.
-
-// Each SDFT output bin has width sdftSampleRateHz/100, ie 13.3Hz per bin at 1333Hz
-// Usable bandwidth is half this, ie 666Hz if sdftSampleRateHz is 1333Hz, i.e. bin 1 is 13.3Hz, bin 2 is 26.7Hz etc
-
-#define DYN_NOTCH_SMOOTH_HZ 4
-#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis
-#define DYN_NOTCH_OSD_MIN_THROTTLE 20
-
-typedef enum {
-
- STEP_WINDOW,
- STEP_DETECT_PEAKS,
- STEP_CALC_FREQUENCIES,
- STEP_UPDATE_FILTERS,
- STEP_COUNT
-
-} step_e;
+// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad.
-typedef struct peak_s {
+// 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
- uint8_t bin;
- float value;
+// 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
-} peak_t;
+#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_OSD_MIN_THROTTLE 20
-static sdft_t FAST_RAM_ZERO_INIT sdft[XYZ_AXIS_COUNT];
-static peak_t FAST_RAM_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX];
-static float FAST_RAM_ZERO_INIT sdftData[SDFT_BIN_COUNT];
-static uint16_t FAST_RAM_ZERO_INIT sdftSampleRateHz;
-static float FAST_RAM_ZERO_INIT sdftResolutionHz;
-static uint8_t FAST_RAM_ZERO_INIT sdftStartBin;
-static uint8_t FAST_RAM_ZERO_INIT sdftEndBin;
-static float FAST_RAM_ZERO_INIT sdftMeanSq;
-static uint16_t FAST_RAM_ZERO_INIT dynNotchQ;
+static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
+static float FAST_RAM_ZERO_INIT fftResolution;
+static uint8_t FAST_RAM_ZERO_INIT fftStartBin;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
static float FAST_RAM_ZERO_INIT smoothFactor;
-static uint8_t FAST_RAM_ZERO_INIT numSamples;
+static uint8_t FAST_RAM_ZERO_INIT samples;
+// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
+static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
-void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
-{
-#ifdef USE_MULTI_GYRO
+void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
+#ifdef USE_DUAL_GYRO
static bool gyroAnalyseInitialized;
if (gyroAnalyseInitialized) {
return;
}
gyroAnalyseInitialized = true;
#endif
-
- 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);
- numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00
-
- 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)
-
- 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
-
- for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
+ const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
+ samples = MAX(1, gyroLoopRateHz / (2 * dynNotchMaxHz)); //600hz, 8k looptime, 13.333
+ 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
+ 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)));
}
}
-void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
-{
+void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) {
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
gyroDataAnalyseInit(targetLooptimeUs);
- state->maxSampleCount = numSamples;
+ state->maxSampleCount = samples;
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
-
- for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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)gyroConfig()->dyn_notch_count + dynNotchMinHz;
- }
+ 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;
}
}
-// 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;
}
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
-// Downsample and analyse gyro data
-FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
-{
+/*
+ * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
+ */
+void NOINLINE gyroDataAnalyse(gyroAnalyseState_t *state) {
+ state->filterUpdateExecute = false; //This will be changed to true only if new data is present
// 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 (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
- state->downsampledGyroData[axis] = sample;
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
+ state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
}
state->oversampledGyroAccumulator[axis] = 0;
}
-
- // We need DYN_NOTCH_CALC_TICKS ticks to update all axes with newly sampled value
+ state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
+ // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
- // 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
+ // 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
state->updateTicks = DYN_NOTCH_CALC_TICKS;
}
-
- // 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
+ // calculate FFT and update filters
if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state);
--state->updateTicks;
}
}
-// Find frequency peaks and update filters
-static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
-{
+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 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_WINDOW: // 6us @ F722
- {
- 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, 0, micros() - startTime);
-
+ case STEP_ARM_CFFT_F32: {
+ switch (FFT_BIN_COUNT) {
+ case 16:
+ // 16us
+ arm_cfft_radix8by2_f32(Sint, state->fftData);
break;
- }
- case STEP_DETECT_PEAKS: // 6us @ F722
- {
- // Get memory ready for new peak data on current axis
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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++) {
- // 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 < gyroConfig()->dyn_notch_count; p++) {
- if (sdftData[bin] > peaks[p].value) {
- for (uint8_t k = gyroConfig()->dyn_notch_count - 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 = gyroConfig()->dyn_notch_count - 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);
-
+ 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;
}
- case STEP_CALC_FREQUENCIES: // 4us @ F722
- {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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;
-
- // 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 (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 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
- // 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]);
- }
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ break;
+ }
+ case STEP_BITREVERSAL: {
+ // 6us
+ arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ state->updateStep++;
+ FALLTHROUGH;
+ }
+ 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);
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ break;
+ }
+ case STEP_ARM_CMPLX_MAG_F32: {
+ // 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(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; p++) {
- dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
+ }
+ 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;
}
}
-
- if (state->updateAxis == X) {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count && p < 3; p++) {
- DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
+ 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;
}
}
-
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
-
- break;
}
- case STEP_UPDATE_FILTERS: // 7us @ F722
- {
- for (uint8_t p = 0; p < gyroConfig()->dyn_notch_count; 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) {
- biquadFilterUpdate(&state->notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
- }
- }
-
- DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
-
- state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
+ dataMin = fminf(dataMin, dataMinHi);
+ // 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;
+ // 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 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;
+ }
+ // get centerFreq in Hz from weighted bins
+ float centerFreq;
+ 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];
+ }
+ centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
+ // 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]);
+ if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
+ dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
+ }
+ 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));
+ }
+// if (state->updateAxis == 1) {
+// DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
+// }
+ DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+ break;
+ }
+ case STEP_UPDATE_FILTERS: {
+ // 7us
+ state->filterUpdateExecute = true;
+ state->filterUpdateAxis = state->updateAxis;
+ state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
+ 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);
+ }
}
-
state->updateStep = (state->updateStep + 1) % STEP_COUNT;
}
diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h
index 5c9d1b8774..17904ff95b 100644
--- a/src/main/sensors/gyroanalyse.h
+++ b/src/main/sensors/gyroanalyse.h
@@ -20,31 +20,42 @@
#pragma once
-#define DYN_NOTCH_COUNT_MAX 5
+#include "arm_math.h"
-typedef struct gyroAnalyseState_s {
+#include "common/filter.h"
+
+#define FFT_WINDOW_SIZE 32
+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 for frequency analysis
- float downsampledGyroData[XYZ_AXIS_COUNT];
+ // downsampled gyro data circular buffer for frequency analysis
+ uint8_t circularBufferIdx;
+ float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
// update state machine step information
uint8_t updateTicks;
uint8_t updateStep;
uint8_t updateAxis;
- float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX];
- biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][5];
+ arm_rfft_fast_instance_f32 fftInstance;
+ float fftData[FFT_WINDOW_SIZE];
+ float rfftData[FFT_WINDOW_SIZE];
+ float centerFreq[XYZ_AXIS_COUNT];
+ bool filterUpdateExecute;
+ uint8_t filterUpdateAxis;
+ float filterUpdateFrequency;
} gyroAnalyseState_t;
-void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
-void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
-void gyroDataAnalyse(gyroAnalyseState_t *state);
+STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
+
+void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime);
+void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
+void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse);
uint16_t getMaxFFT(void);
void resetMaxFFT(void);
From 86d2732e25c1e78a51ad57f19c538f6df9e9ffbc Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 12 Aug 2021 00:45:15 -0600
Subject: [PATCH 18/19] slight renaming and fixing bugs from merging
---
src/main/flight/pid.c | 18 +++++++-----------
1 file changed, 7 insertions(+), 11 deletions(-)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b8354277d4..30fdfd50a6 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -664,7 +664,7 @@ static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5];
static FAST_RAM_ZERO_INIT float previousDtermErrorRate[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float axisLock[XYZ_AXIS_COUNT];
-static FAST_RAM_ZERO_INIT float inverseScaledError[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT float dynamicDtermScaler[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs;
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) {
@@ -798,7 +798,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
pureError = errorRate - previousError[axis];
previousError[axis] = errorRate;
- float dtermErrorRate = currentPidSetpoint - gyroRate * inverseScaledError[axis]; // r - y
+ float dtermErrorRate = currentPidSetpoint - gyroRate * dynamicDtermScaler[axis]; // r - y
float dtermError = dtermErrorRate - previousDtermErrorRate[axis];
previousDtermErrorRate[axis] = dtermErrorRate;
@@ -806,7 +806,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
previousMeasurement[axis] = gyroRate;
if (pidCoefficient[axis].Kd > 0) {
- float dDelta = ((feathered_pids * pureMeasurement) * inverseScaledError[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
+ float dDelta = ((feathered_pids * pureMeasurement) * dynamicDtermScaler[axis] + ((1 - feathered_pids) * dtermError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error
// filter the dterm
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive() && pidProfile->dtermDynNotch) {
@@ -886,7 +886,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// emuboost 2.0
if (pidProfile->emuBoost2) {
- //need to add logging, lol
float changeError = pureError * pidFrequency * DTERM_SCALE * 20.0f;
changeError = changeError / pidProfile->emuBoost2_cutoff;
@@ -897,27 +896,24 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float scaledError = constrainf(errorRate / pidProfile->emuBoost2_cutoff, -1.0f, 1.0f);
scaledError = ABS(scaledError) * power3(scaledError) * (pidProfile->emuBoost2_expo / 100.0f) + scaledError * (1 - pidProfile->emuBoost2_expo / 100.0f);
- scaledError *= changeError;
+ scaledError *= changeError; // this is only 1 when the error and change in setpoint are high
scaledError = pt1FilterApply(&emuboost2_0Filter[axis], scaledError);
if (axis == FD_ROLL || axis == FD_PITCH) {
DEBUG_SET(DEBUG_EMUBOOST, axis, lrintf(scaledError * 1000));
}
+ dynamicDtermScaler[axis] = 1 - scaledError; // this is only 0 when the error and change in setpoint are high
if (scaledError > 0.0f) {
- inverseScaledError[axis] = 1 - scaledError; // this is only 1 when the error and change in setpoint are high
-
pidData[axis].P *= ((pidProfile->emuBoost2 / 100.0f) * scaledError) + 1.0f;
- //pidData[axis].D *= inverseScaledError[axis];
doSignMatch = 1000.0f;
} else {
- inverseScaledError[axis] = 1.0f;
doSignMatch = 0.0f;
}
if (axis == FD_ROLL || axis == FD_PITCH) {
DEBUG_SET(DEBUG_EMUBOOST, axis + 2, lrintf(doSignMatch));
}
} else {
- inverseScaledError[axis] = 1.0f;
+ dynamicDtermScaler[axis] = 1.0f;
}
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + directFeedForward;
@@ -934,5 +930,5 @@ float pidGetPreviousSetpoint(int axis) {
}
float getDtermPercentLeft(int axis) {
- return inverseScaledError[axis];
+ return dynamicDtermScaler[axis];
}
From 91d5c01be00f01edd30f5123364abd4fb0197ca7 Mon Sep 17 00:00:00 2001
From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com>
Date: Fri, 10 Sep 2021 09:26:45 -0600
Subject: [PATCH 19/19] change defaults
---
src/main/cms/cms_menu_imu.c | 2 +-
src/main/flight/pid.c | 6 +++---
src/main/interface/settings.c | 2 +-
3 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 5dea2f891b..9b74e707b8 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -240,7 +240,7 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = {
{ "EMUBOOST 2.0", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2, 0, 250, 1}, 0 },
{ "EMUBOOST 2.0 FILT", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_filter, 1, 250, 1}, 0 },
- { "EMUBOOST 2.0 CUT", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_cutoff, 10, 250, 1}, 0 },
+ { "EMUBOOST 2.0 CUT", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_cutoff, 5, 250, 1}, 0 },
{ "EMUBOOST 2.0 EXPO", OME_UINT8, NULL, &(OSD_UINT8_t){ &emuBoost2_expo, 10, 100, 1}, 0 },
{ "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0},
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 2604023d9d..9918b959fb 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -200,9 +200,9 @@ void resetPidProfile(pidProfile_t *pidProfile) {
.dtermDynNotch = false,
.dterm_dyn_notch_q = 400,
.emuBoost2 = 150,
- .emuBoost2_filter = 20,
- .emuBoost2_cutoff = 10,
- .emuBoost2_expo = 25,
+ .emuBoost2_filter = 50,
+ .emuBoost2_cutoff = 5,
+ .emuBoost2_expo = 35,
);
}
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 32035260b4..773d900385 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -854,7 +854,7 @@ const clivalue_t valueTable[] = {
{ "dterm_dyn_notch_q", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_dyn_notch_q) },
{ "emuboost2", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2) },
{ "emuboost2_filter", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_filter) },
- { "emuboost2_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_cutoff) },
+ { "emuboost2_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 5, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_cutoff) },
{ "emuboost2_expo", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, emuBoost2_expo) },
{ "dterm_abg_alpha", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) },