From 02d01ed1daa031f2acddeb28b7452e158fa5d91d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 14 Jul 2022 15:48:56 +0200 Subject: [PATCH 1/5] Proof of concept --- src/main/CMakeLists.txt | 2 + src/main/flight/dynamic_gyro_notch.h | 2 - .../flight/secondary_dynamic_gyro_notch.c | 83 +++++++++++++++++++ .../flight/secondary_dynamic_gyro_notch.h | 43 ++++++++++ src/main/sensors/gyro.c | 21 +++++ src/main/sensors/gyro.h | 1 + 6 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 src/main/flight/secondary_dynamic_gyro_notch.c create mode 100644 src/main/flight/secondary_dynamic_gyro_notch.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index aadbaf2006..595ac4cbb1 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -331,6 +331,8 @@ main_sources(COMMON_SRC flight/rpm_filter.h flight/dynamic_gyro_notch.c flight/dynamic_gyro_notch.h + flight/secondary_dynamic_gyro_notch.c + flight/secondary_dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h flight/secondary_imu.c diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index 40203a7bc5..b8adbfa0ea 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -37,8 +37,6 @@ typedef struct dynamicGyroNotchState_s { uint16_t frequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; float dynNotchQ; - float dynNotch1Ctr; - float dynNotch2Ctr; uint32_t looptime; uint8_t enabled; diff --git a/src/main/flight/secondary_dynamic_gyro_notch.c b/src/main/flight/secondary_dynamic_gyro_notch.c new file mode 100644 index 0000000000..cf6360c46b --- /dev/null +++ b/src/main/flight/secondary_dynamic_gyro_notch.c @@ -0,0 +1,83 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it 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. + * + * This file is distributed in the hope that it 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#ifdef USE_DYNAMIC_FILTERS + +FILE_COMPILE_FOR_SPEED + +#include +#include "secondary_dynamic_gyro_notch.h" +#include "fc/config.h" +#include "build/debug.h" +#include "sensors/gyro.h" + +#define SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 150 + +void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state) { + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + state->filtersApplyFn[axis] = nullFilterApply; + } + + state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; + state->enabled = gyroConfig()->dynamicGyroNotchEnabled; + state->looptime = getLooptime(); + + if (state->enabled) { + /* + * Step 1 - init all filters even if they will not be used further down the road + */ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + //Any initial notch Q is valid sice it will be updated immediately after + biquadFilterInit(&state->filters[axis], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[axis] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + + } +} + +void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) { + + if (state->enabled) { + + /* + * The secondary dynamic notch uses only the first detected frequency + * The rest of peaks are ignored + */ + state->frequency[axis] = frequency[0]; + + // Filter update happens only if peak was detected + if (frequency[0] > 0.0f) { + biquadFilterUpdate(&state->filters[axis], state->frequency[axis], state->looptime, state->dynNotchQ, FILTER_NOTCH); + } + } +} + +float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input) { + return state->filtersApplyFn[axis]((filter_t *)&state->filters[axis], input); +} + +#endif \ No newline at end of file diff --git a/src/main/flight/secondary_dynamic_gyro_notch.h b/src/main/flight/secondary_dynamic_gyro_notch.h new file mode 100644 index 0000000000..ac2e4975e5 --- /dev/null +++ b/src/main/flight/secondary_dynamic_gyro_notch.h @@ -0,0 +1,43 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it 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. + * + * This file is distributed in the hope that it 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 program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include +#include "common/axis.h" +#include "common/filter.h" + +typedef struct secondaryDynamicGyroNotchState_s { + uint16_t frequency[XYZ_AXIS_COUNT]; + float dynNotchQ; + uint32_t looptime; + uint8_t enabled; + + biquadFilter_t filters[XYZ_AXIS_COUNT]; + filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT]; +} secondaryDynamicGyroNotchState_t; + +void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state); +void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]); +float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input); \ No newline at end of file diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b13def362f..34c3eaef39 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -92,6 +92,7 @@ STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; +EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState; #endif @@ -298,6 +299,9 @@ bool gyroInit(void) #ifdef USE_DYNAMIC_FILTERS // Dynamic notch running at PID frequency dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); + + secondaryDynamicGyroNotchFiltersInit(&secondaryDynamicGyroNotchState); + gyroDataAnalyseStateInit( &gyroAnalyseState, gyroConfig()->dynamicGyroNotchMinHz, @@ -449,6 +453,16 @@ void FAST_CODE NOINLINE gyroFilter() gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf); } + + /** + * Secondary dynamic notch filter. + * In some cases, noise amplitude is high enough not to be filtered by the primary filter. + * This happens on the first frequency with the biggest aplitude + */ + DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); + secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); + DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); + #endif #ifdef USE_GYRO_KALMAN @@ -470,6 +484,13 @@ void FAST_CODE NOINLINE gyroFilter() gyroAnalyseState.filterUpdateAxis, gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] ); + + secondaryDynamicGyroNotchFiltersUpdate( + &secondaryDynamicGyroNotchState, + gyroAnalyseState.filterUpdateAxis, + gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] + ); + } } #endif diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b98b141ee3..d2c310f1a9 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -24,6 +24,7 @@ #include "config/parameter_group.h" #include "drivers/sensor.h" #include "flight/dynamic_gyro_notch.h" +#include "flight/secondary_dynamic_gyro_notch.h" typedef enum { GYRO_NONE = 0, From 43bdb0f0572e0b18d9efe72021931ba8af7c65bf Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 15 Jul 2022 08:49:34 +0200 Subject: [PATCH 2/5] Fix secondary gyro --- 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 34c3eaef39..dd093fc4ae 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -460,7 +460,7 @@ void FAST_CODE NOINLINE gyroFilter() * This happens on the first frequency with the biggest aplitude */ DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); - secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); + gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); #endif From 8deb1054ba8edf6ee909f13149863b9668c96e87 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 23 Jul 2022 12:08:37 +0200 Subject: [PATCH 3/5] Make 3D matrix filter configurable --- src/main/fc/settings.yaml | 16 ++++++ src/main/flight/dynamic_gyro_notch.h | 2 +- .../flight/secondary_dynamic_gyro_notch.c | 51 +++++++++++++++---- src/main/sensors/gyro.c | 4 +- src/main/sensors/gyro.h | 13 +++++ 5 files changed, 73 insertions(+), 13 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 07a093bb4d..b9b078b81f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -187,6 +187,9 @@ tables: - name: rth_trackback_mode values: ["OFF", "ON", "FS"] enum: rthTrackbackMode_e + - name: dynamic_gyro_notch_mode + values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"] + enum: dynamicGyroNotchMode_e constants: RPYL_PID_MIN: 0 @@ -285,6 +288,19 @@ groups: condition: USE_DYNAMIC_FILTERS min: 30 max: 250 + - name: dynamic_gyro_notch_mode + description: "Gyro dynamic notch type" + default_value: "2D" + table: dynamic_gyro_notch_mode + field: dynamicGyroNotchMode + condition: USE_DYNAMIC_FILTERS + - name: dynamic_gyro_notch_3d_q + description: "Q factor for 3D dynamic notches" + default_value: 200 + field: dynamicGyroNotch3dQ + condition: USE_DYNAMIC_FILTERS + min: 1 + max: 1000 - name: gyro_to_use condition: USE_DUAL_GYRO min: 0 diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index b8adbfa0ea..b3337da921 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -31,7 +31,7 @@ #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 /* - * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters + * Number of peaks to detect with Dynamic Notch Filter aka Matrix Filter. This is equal to the number of dynamic notch filters */ #define DYN_NOTCH_PEAK_COUNT 3 typedef struct dynamicGyroNotchState_s { diff --git a/src/main/flight/secondary_dynamic_gyro_notch.c b/src/main/flight/secondary_dynamic_gyro_notch.c index cf6360c46b..b5070a9f22 100644 --- a/src/main/flight/secondary_dynamic_gyro_notch.c +++ b/src/main/flight/secondary_dynamic_gyro_notch.c @@ -42,21 +42,50 @@ void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *stat state->filtersApplyFn[axis] = nullFilterApply; } - state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; - state->enabled = gyroConfig()->dynamicGyroNotchEnabled; + state->dynNotchQ = gyroConfig()->dynamicGyroNotch3dQ / 100.0f; + state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D; state->looptime = getLooptime(); - if (state->enabled) { - /* - * Step 1 - init all filters even if they will not be used further down the road + if ( + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D + ) { + /* + * Enable ROLL filter */ - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - //Any initial notch Q is valid sice it will be updated immediately after - biquadFilterInit(&state->filters[axis], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - state->filtersApplyFn[axis] = (filterApplyFnPtr)biquadFilterApplyDF1; - } - + biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1; } + + if ( + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D + ) { + /* + * Enable PITCH filter + */ + biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + + if ( + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY || + gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D + ) { + /* + * Enable YAW filter + */ + biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + + } void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 34c3eaef39..a229571fd6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -117,6 +117,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT, .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT, .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT, + .dynamicGyroNotchMode = SETTING_DYNAMIC_GYRO_NOTCH_MODE_DEFAULT, + .dynamicGyroNotch3dQ = SETTING_DYNAMIC_GYRO_NOTCH_3D_Q_DEFAULT, #endif #ifdef USE_GYRO_KALMAN .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d2c310f1a9..f6a3f71b8f 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,6 +40,17 @@ typedef enum { GYRO_FAKE } gyroSensor_e; +typedef enum { + DYNAMIC_NOTCH_MODE_2D = 0, + DYNAMIC_NOTCH_MODE_R, + DYNAMIC_NOTCH_MODE_P, + DYNAMIC_NOTCH_MODE_Y, + DYNAMIC_NOTCH_MODE_RP, + DYNAMIC_NOTCH_MODE_RY, + DYNAMIC_NOTCH_MODE_PY, + DYNAMIC_NOTCH_MODE_3D +} dynamicGyroNotchMode_e; + typedef struct gyro_s { bool initialized; uint32_t targetLooptime; @@ -69,6 +80,8 @@ typedef struct gyroConfig_s { uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; + uint8_t dynamicGyroNotchMode; + uint16_t dynamicGyroNotch3dQ; #endif #ifdef USE_GYRO_KALMAN uint16_t kalman_q; From 408123ba80a05edc88474303f2c0901d69b5ea69 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 23 Jul 2022 12:17:29 +0200 Subject: [PATCH 4/5] Docs update --- docs/Settings.md | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index af0ea39fd7..ac854bd979 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -752,6 +752,16 @@ Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, --- +### dynamic_gyro_notch_3d_q + +Q factor for 3D dynamic notches + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 1 | 1000 | + +--- + ### dynamic_gyro_notch_enabled Enable/disable dynamic gyro notch also known as Matrix Filter @@ -772,6 +782,16 @@ Minimum frequency for dynamic notches. Default value of `150` works best with 5" --- +### dynamic_gyro_notch_mode + +Gyro dynamic notch type + +| Default | Min | Max | +| --- | --- | --- | +| 2D | | | + +--- + ### dynamic_gyro_notch_q Q factor for dynamic notches From e37d5088587c07296a473acd4c7dc1360f52f86a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 24 Jul 2022 11:01:01 +0200 Subject: [PATCH 5/5] Remove not needed debug --- src/main/sensors/gyro.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 827a5b4ebe..bb81200b20 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -461,9 +461,7 @@ void FAST_CODE NOINLINE gyroFilter() * In some cases, noise amplitude is high enough not to be filtered by the primary filter. * This happens on the first frequency with the biggest aplitude */ - DEBUG_SET(DEBUG_ALWAYS, axis, gyroADCf); gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf); - DEBUG_SET(DEBUG_ALWAYS, axis + 3, gyroADCf); #endif