smoothing fix level mode

betaflight · Sep 1, 2021 · 4ef0292 · 4ef0292
1 parent 86aa5cc
commit 4ef0292
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 3 deletions.
diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c
@@ -5018,7 +5018,7 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
             cliPrintLine("(auto)");
         }
         cliPrintf("# Active throttle cutoff: %dhz ", rcSmoothingData->throttleCutoffFrequency);
-        if (rcSmoothingData->ffCutoffSetting) {
+        if (rcSmoothingData->throttleCutoffSetting) {
             cliPrintLine("(manual)");
         } else {
             cliPrintLine("(auto)");

diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
@@ -64,6 +64,7 @@ float rcCommandDelta[XYZ_AXIS_COUNT];
 #endif
 static float rawSetpoint[XYZ_AXIS_COUNT];
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
+static float rcDeflectionSmoothed[3];
 static float throttlePIDAttenuation;
 static bool reverseMotors = false;
 static applyRatesFn *applyRates;
@@ -106,14 +107,21 @@ bool getShouldUpdateFeedforward()
 }
 
 float getSetpointRate(int axis)
-// only used in pid.c to provide setpointRate for the crash recovery function
 {
+#ifdef USE_RC_SMOOTHING_FILTER
     return setpointRate[axis];
+#else
+    return rawSetpoint[axis];
+#endif
 }
 
 float getRcDeflection(int axis)
 {
+#ifdef USE_RC_SMOOTHING_FILTER
+    return rcDeflectionSmoothed[axis];
+#else
     return rcDeflection[axis];
+#endif
 }
 
 float getRcDeflectionAbs(int axis)
@@ -336,7 +344,6 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
         smoothingData->throttleCutoffFrequency = calcAutoSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactorThrottle);
     }
 
-
     // initialize or update the Setpoint filter
     if ((smoothingData->setpointCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
@@ -354,6 +361,15 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
                 }
             }
         }
+
+        // initialize or update the Level filter
+        for (int i = FD_ROLL; i < FD_YAW; i++) {
+            if (!smoothingData->filterInitialized) {
+                pt3FilterInit(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+            } else {
+                pt3FilterUpdateCutoff(&smoothingData->filterDeflection[i], pt3FilterGain(smoothingData->setpointCutoffFrequency, dT));
+            }
+        }
     }
 
     // update or initialize the FF filter
@@ -537,6 +553,17 @@ static FAST_CODE void processRcSmoothingFilter(void)
             *dst = rxDataToSmooth[i];
         }
     }
+
+    // for ANGLE and HORIZON, smooth rcDeflection on pitch and roll to avoid setpoint steps
+    bool smoothingNeeded = (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && rcSmoothingData.filterInitialized;
+    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+        if (smoothingNeeded && axis < FD_YAW) {
+            rcDeflectionSmoothed[axis] = pt3FilterApply(&rcSmoothingData.filterDeflection[axis], rcDeflection[axis]);
+        } else {
+            rcDeflectionSmoothed[axis] = rcDeflection[axis];
+        }
+    }
+
 }
 #endif // USE_RC_SMOOTHING_FILTER
 

diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
@@ -95,6 +95,7 @@ typedef struct rcSmoothingFilterTraining_s {
 typedef struct rcSmoothingFilter_s {
     bool filterInitialized;
     pt3Filter_t filter[4];
+    pt3Filter_t filterDeflection[2];
     uint8_t setpointCutoffSetting;
     uint8_t throttleCutoffSetting;
     uint16_t setpointCutoffFrequency;