add dynamicFilter mode. RP or RPY. fix as 0,1 not 1,2 :(

emuflight · Mar 30, 2023 · 7e848ae · 7e848ae
1 parent d56b242
commit 7e848ae
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 4 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 && axis <= gyroConfig()->dyn_notch_mode) {
+            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/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -819,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 < gyroConfig()->dyn_notch_mode; 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
@@ -113,8 +113,8 @@ typedef struct smithPredictor_s {
 
 #ifdef USE_GYRO_DATA_ANALYSE
 typedef enum {
-    RP = 1,
-    RPY = 2
+    RP = 0,
+    RPY = 1
 } dynamicGyroNotchMode_e;
 #endif