add dynamicFilter mode. RP or RPY

emuflight · May 3, 2023 · dbf64f5 · dbf64f5
1 parent 1ff18fc
commit dbf64f5
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 2 deletions.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -809,7 +809,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             float dDelta = ((feathered_pids * pureMeasurement) + ((1 - feathered_pids) * pureError)) * 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() && pidProfile->dtermDynNotch && axis <= gyroConfig()->dyn_notch_mode+1) {
                 for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) {
                     if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) {
                         previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p);

diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
@@ -285,6 +285,7 @@ static const char * const lookupTableRcInterpolation[] = {
 static const char * const lookupTableRcInterpolationChannels[] = {
     "RP", "RPY", "RPYT", "T", "RPT",
 };
+
 static const char * const lookupTableFilterType[] = {
     "PT1", "BIQUAD", "PT2", "PT3", "PT4",
 };
@@ -385,6 +386,12 @@ static const char *const lookupTableMixerImplType[] = {
     "LEGACY", "SMOOTH", "2PASS"
 };
 
+#ifdef USE_GYRO_DATA_ANALYSE
+static const char *const lookupTableDynNotchModeType[] = {
+    "RP", "RPY"
+};
+#endif
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -474,6 +481,9 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming),
 #endif
     LOOKUP_TABLE_ENTRY(lookupTableMixerImplType),
+#ifdef USE_GYRO_DATA_ANALYSE
+    LOOKUP_TABLE_ENTRY(lookupTableDynNotchModeType),
+#endif
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -545,6 +555,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_mode",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYN_NOTCH_MODE_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_mode) },
     { "dynamic_gyro_notch_q",       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) },

diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h
@@ -112,6 +112,9 @@ typedef enum {
     TABLE_OSD_LOGO_ON_ARMING,
 #endif
     TABLE_MIXER_IMPL_TYPE,
+#ifdef USE_GYRO_DATA_ANALYSE
+    TABLE_DYN_NOTCH_MODE_TYPE,
+#endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -249,6 +249,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
                   .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
                   .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO,
                   .yaw_spin_threshold = 1950,
+                  .dyn_notch_mode = RPY,
                   .dyn_notch_q = 400,
                   .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
                   .dyn_notch_min_hz = 150,
@@ -304,6 +305,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
                   .gyro_offset_yaw = 0,
                   .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO,
                   .yaw_spin_threshold = 1950,
+                  .dyn_notch_mode = RPY,
                   .dyn_notch_q = 350,
                   .dyn_notch_count = 3, // default of 3 is similar to the matrix filter.
                   .dyn_notch_min_hz = 150,
@@ -817,7 +819,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) {
     gyroSensor->notchFilterDynApplyFn = nullFilterApply;
     if (isDynamicFilterActive()) {
         gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        for (int axis = 0; axis < gyroConfig()->dyn_notch_mode+1; 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);
             }

diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
@@ -111,6 +111,13 @@ typedef struct smithPredictor_s {
 } smithPredictor_t;
 #endif // USE_SMITH_PREDICTOR
 
+#ifdef USE_GYRO_DATA_ANALYSE
+typedef enum {
+    RP = 0,
+    RPY = 1
+} dynamicGyroNotchMode_e;
+#endif
+
 typedef struct gyroConfig_s {
     uint8_t  gyro_align;                       // gyro alignment
     uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@@ -144,6 +151,7 @@ typedef struct gyroConfig_s {
     int16_t  yaw_spin_threshold;
 
     uint16_t gyroCalibrationDuration;  // Gyro calibration duration in 1/100 second
+    uint8_t dyn_notch_mode;
     uint16_t dyn_notch_q;
     uint8_t dyn_notch_count;
     uint16_t dyn_notch_min_hz;