dynamic notch on pidsum

gyro->pidum is not a linear change. dynamic notch on pidsum might better account for dterm and other dynamics going on.

Author: Quick-Flash

src/main/build/debug.c

@@ -99,5 +99,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",
     "RX_EXPRESSLRS_PHASELOCK",
-    "RX_STATE_TIME"
+    "RX_STATE_TIME",
+    "PIDSUM",
 };

src/main/build/debug.h

@@ -98,6 +98,7 @@ typedef enum {
     DEBUG_RX_EXPRESSLRS_SPI,
     DEBUG_RX_EXPRESSLRS_PHASELOCK,
     DEBUG_RX_STATE_TIME,
+    DEBUG_PIDSUM,
     DEBUG_COUNT
 } debugType_e;
 

src/main/flight/pid.c

@@ -49,6 +49,9 @@
 #include "flight/mixer.h"
 #include "flight/rpm_filter.h"
 #include "flight/feedforward.h"
+#ifdef USE_DYN_NOTCH_FILTER
+#include "flight/dyn_notch_filter.h"
+#endif
 
 #include "io/gps.h"
 
@@ -1125,7 +1128,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         if (feedforwardGain > 0) {
             // halve feedforward in Level mode since stick sensitivity is weaker by about half
             feedforwardGain *= FLIGHT_MODE(ANGLE_MODE) ? 0.5f : 1.0f;
-            // transition now calculated in feedforward.c when new RC data arrives 
+            // transition now calculated in feedforward.c when new RC data arrives
             float feedForward = feedforwardGain * pidSetpointDelta * pidRuntime.pidFrequency;
 
 #ifdef USE_FEEDFORWARD
@@ -1193,7 +1196,31 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         {
             pidData[axis].Sum = pidSum;
         }
+
+#ifdef USE_DYN_NOTCH_FILTER
+        if (isDynNotchActive()) {
+            // if (axis == gyro.gyroDebugAxis) {
+            //     GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
+            //     GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
+            //     GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
+            // }
+
+            dynNotchPush(axis, pidData[axis].Sum);
+            pidData[axis].Sum = dynNotchFilter(axis, pidData[axis].Sum);
+
+            // if (axis == gyro.gyroDebugAxis) {
+            //     GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
+            //     GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
+            // }
+        }
+#endif
+    DEBUG_SET(DEBUG_PIDSUM, axis, lrintf(pidData[axis].Sum));
+    }
+#ifdef USE_DYN_NOTCH_FILTER
+    if (isDynNotchActive()) {
+        dynNotchUpdate();
     }
+#endif
 
     // Disable PID control if at zero throttle or if gyro overflow detected
     // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight

src/main/flight/pid_init.c

@@ -39,6 +39,9 @@
 #include "flight/feedforward.h"
 #include "flight/pid.h"
 #include "flight/rpm_filter.h"
+#ifdef USE_DYN_NOTCH_FILTER
+#include "flight/dyn_notch_filter.h"
+#endif
 
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
@@ -238,6 +241,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 
     pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
     pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
+    #ifdef USE_DYN_NOTCH_FILTER
+        dynNotchInit(dynNotchConfig(), targetPidLooptime);
+    #endif
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -440,4 +446,3 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
         memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
     }
 }
-

src/main/sensors/gyro.c

@@ -45,9 +45,6 @@
 #include "config/config.h"
 #include "fc/runtime_config.h"
 
-#ifdef USE_DYN_NOTCH_FILTER
-#include "flight/dyn_notch_filter.h"
-#endif
 #include "flight/rpm_filter.h"
 
 #include "io/beeper.h"
@@ -110,7 +107,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyroMovementCalibrationThreshold = 48;
     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
     gyroConfig->gyro_lpf1_type = FILTER_PT1;
-    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;  
+    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
         // NOTE: dynamic lpf is enabled by default so this setting is actually
         // overridden and the static lowpass 1 is disabled. We can't set this
         // value to 0 otherwise Configurator versions 10.4 and earlier will also
@@ -472,12 +469,6 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
         filterGyroDebug();
     }
 
-#ifdef USE_DYN_NOTCH_FILTER
-    if (isDynNotchActive()) {
-        dynNotchUpdate();
-    }
-#endif
-
     if (gyro.useDualGyroDebugging) {
         switch (gyro.gyroToUse) {
         case GYRO_CONFIG_USE_GYRO_1:

src/main/sensors/gyro_filter_impl.c

@@ -64,24 +64,6 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         // DEBUG_GYRO_SAMPLE(3) Record the post-static notch and lowpass filter value for the selected debug axis
         GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 3, lrintf(gyroADCf));
 
-#ifdef USE_DYN_NOTCH_FILTER
-        if (isDynNotchActive()) {
-            if (axis == gyro.gyroDebugAxis) {
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
-                GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf));
-            }
-
-            dynNotchPush(axis, gyroADCf);
-            gyroADCf = dynNotchFilter(axis, gyroADCf);
-
-            if (axis == gyro.gyroDebugAxis) {
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
-                GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf));
-            }
-        }
-#endif
-
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
 

src/main/sensors/gyro_init.c

@@ -63,10 +63,6 @@
 
 #include "fc/runtime_config.h"
 
-#ifdef USE_DYN_NOTCH_FILTER
-#include "flight/dyn_notch_filter.h"
-#endif
-
 #include "pg/gyrodev.h"
 
 #include "sensors/gyro.h"
@@ -263,9 +259,6 @@ void gyroInitFilters(void)
 #ifdef USE_DYN_LPF
     dynLpfFilterInit();
 #endif
-#ifdef USE_DYN_NOTCH_FILTER
-    dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
-#endif
 }
 
 #if defined(USE_GYRO_SLEW_LIMITER)