first order filter

rtlopez · Feb 26, 2024 · 61424e5 · 61424e5
1 parent d84a286
commit 61424e5
Show file tree

Hide file tree

Showing 3 changed files with 65 additions and 4 deletions.
diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h
@@ -356,7 +356,7 @@ class Cli
                                                   PSTR("DSHOT_RPM_TELEMETRY"), PSTR("RPM_FILTER"), PSTR("D_MIN"), PSTR("AC_CORRECTION"), PSTR("AC_ERROR"), PSTR("DUAL_GYRO_SCALED"), PSTR("DSHOT_RPM_ERRORS"), 
                                                   PSTR("CRSF_LINK_STATISTICS_UPLINK"), PSTR("CRSF_LINK_STATISTICS_PWR"), PSTR("CRSF_LINK_STATISTICS_DOWN"), PSTR("BARO"), PSTR("GPS_RESCUE_THROTTLE_PID"), 
                                                   PSTR("DYN_IDLE"), PSTR("FF_LIMIT"), PSTR("FF_INTERPOLATED"), PSTR("BLACKBOX_OUTPUT"), PSTR("GYRO_SAMPLE"), PSTR("RX_TIMING"), NULL };
-      static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL };
+      static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FO"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL };
       static const char* alignChoices[]      = { PSTR("DEFAULT"), PSTR("CW0"), PSTR("CW90"), PSTR("CW180"), PSTR("CW270"), PSTR("CW0_FLIP"), PSTR("CW90_FLIP"), PSTR("CW180_FLIP"), PSTR("CW270_FLIP"), PSTR("CUSTOM"), NULL };
       static const char* mixerTypeChoices[]  = { PSTR("NONE"), PSTR("TRI"), PSTR("QUADP"), PSTR("QUADX"), PSTR("BI"),
                                                  PSTR("GIMBAL"), PSTR("Y6"), PSTR("HEX6"), PSTR("FWING"), PSTR("Y4"),

diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h
@@ -34,6 +34,7 @@ enum FilterType {
   FILTER_NOTCH,
   FILTER_NOTCH_DF1,
   FILTER_BPF,
+  FILTER_FO,
   FILTER_FIR2,
   FILTER_MEDIAN3,
   FILTER_NONE,
@@ -218,6 +219,56 @@ class FilterStateBiquad {
     float x1, x2, y1, y2;
 };
 
+class FilterStateFirstOrder {
+  public:
+    void reset()
+    {
+      x1 = y1 = 0;
+    }
+
+    void init(float rate, float freq)
+    {
+      freq = Math::clamp(freq, 0.0f, rate * 0.48f);
+
+      const float W = std::tan(Math::pi() * freq / rate);
+
+      a1 = (W - 1) / (W + 1);
+      b1 = b0 = W / (W + 1);
+    }
+
+    void reconfigure(const FilterStateFirstOrder& from)
+    {
+      b0 = from.b0;
+      b1 = from.b1;
+      a1 = from.a1;
+    }
+
+    float update(float n)
+    {
+      // DF2
+      const float result = b0 * n + x1;
+      x1 = b1 * n - a1 * result;
+      return result;
+    }
+
+    float updateDF1(float n)
+    {
+      /* compute result */
+      const float result = b0 * n + b1 * x1 - a1 * y1;
+
+      /* shift input to x1 */
+      x1 = n;
+
+      /* shift result to y1 */
+      y1 = result;
+
+      return result;
+    }
+
+    float b0, b1, a1;
+    float x1, y1;
+};
+
 class FilterStateMedian {
   public:
     void reset()
@@ -227,7 +278,6 @@ class FilterStateMedian {
 
     void init()
     {
-
     }
 
     void reconfigure(const FilterStateMedian& from)
@@ -345,6 +395,8 @@ class Filter
           return _state.pt2.update(v);
         case FILTER_PT3:
           return _state.pt3.update(v);
+        case FILTER_FO:
+          return _state.fo.update(v);
         case FILTER_NONE:
         default:
           return v;
@@ -374,6 +426,8 @@ class Filter
           return _state.pt2.reset();
         case FILTER_PT3:
           return _state.pt3.reset();
+        case FILTER_FO:
+          return _state.fo.reset();
         case FILTER_NONE:
         default:
           ;
@@ -441,6 +495,9 @@ class Filter
         case FILTER_PT3:
           _state.pt3.init(_rate, _conf.freq);
           break;
+        case FILTER_FO:
+          _state.fo.init(_rate, _conf.freq);
+          break;
         case FILTER_NONE:
         default:
           ;
@@ -475,6 +532,9 @@ class Filter
         case FILTER_PT3:
           _state.pt3.reconfigure(filter._state.pt3);
           break;
+        case FILTER_FO:
+          _state.fo.reconfigure(filter._state.fo);
+          break;
         case FILTER_NONE:
         default:
           ;
@@ -511,6 +571,7 @@ class Filter
       FilterStateMedian median;
       FilterStatePt2 pt2;
       FilterStatePt3 pt3;
+      FilterStateFirstOrder fo;
     } _state;
     float _input_weight;
     float _output_weight;

diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h
@@ -759,13 +759,13 @@ class ModelConfig
       rpmFilterHarmonics = 3;
       rpmFilterMinFreq = 100;
       rpmFilterQ = 500;
-      rpmFilterFade = 50;
+      rpmFilterFade = 30;
       rpmFilterWeights[0] = 100;
       rpmFilterWeights[1] = 100;
       rpmFilterWeights[2] = 100;
       rpmFilterFreqLpf = 150;
 
-      gyroFilter3 = FilterConfig(FILTER_PT1, 150);
+      gyroFilter3 = FilterConfig(FILTER_FO, 150);
       gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off
       gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off
       dtermNotchFilter = FilterConfig(FILTER_NOTCH, 0, 0); // off