Fix HPF bug and cross-coupling scale (#158)

rotorflight · Sep 11, 2024 · 6229abf · 6229abf
1 parent 36b58c7
commit 6229abf
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 6 deletions.
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
@@ -405,6 +405,7 @@ FAST_CODE float ewma3FilterApply(ewma3Filter_t *filter, float input)
  *   Fc = Cutoff frequency
  *   Fs = Sampling frequency
  *
+ *   Wc = 2⋅π⋅Fc
  *
  *                Wc          Wc
  *  H(s) = s ⋅ ―――――――― = ――――――――――――
@@ -418,7 +419,7 @@ FAST_CODE float ewma3FilterApply(ewma3Filter_t *filter, float input)
  *          a₀ + a₁⋅z⁻¹
  *
  * Where
- *      b₀ = K / (1 + K)
+ *      b₀ = 2*Fs * K / (1 + K)
  *      b₁ = -b₀
  *      a₀ = 1
  *      a₁ = (1 - K) / (1 + K)
@@ -438,12 +439,12 @@ void difFilterInit(difFilter_t *filter, float cutoff, float sampleRate)
 
 void difFilterUpdate(difFilter_t *filter, float cutoff, float sampleRate)
 {
-    cutoff = limitCutoff(cutoff, sampleRate);
+    cutoff = limitCutoff(cutoff, sampleRate / 2);
 
     const float W = tan_approx(M_PIf * cutoff / sampleRate);
 
     filter->a = (W - 1) / (W + 1);
-    filter->b = W / (W + 1);
+    filter->b = 2 * sampleRate * W / (W + 1);
 }
 
 FAST_CODE float difFilterApply(difFilter_t *filter, float input)
@@ -648,13 +649,13 @@ FAST_CODE void firstOrderLPFUpdate(order1Filter_t *filter, float cutoff, float s
 
 FAST_CODE void firstOrderHPFUpdate(order1Filter_t *filter, float cutoff, float sampleRate)
 {
-    cutoff = limitCutoff(cutoff, sampleRate);
+    cutoff = limitCutoff(cutoff, sampleRate / 2);
 
     const float W = tan_approx(M_PIf * cutoff / sampleRate);
 
     filter->a1 = (W - 1) / (W + 1);
     filter->b0 = 1 / (W + 1);
-    filter->b1 = -filter->b1;
+    filter->b1 = -filter->b0;
 }
 
 FAST_CODE float firstOrderFilterApply(order1Filter_t *filter, float input)

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -51,7 +51,7 @@
 #define YAW_F_TERM_SCALE            0.000025f
 #define YAW_B_TERM_SCALE            1.0e-6f
 
-#define CROSS_COUPLING_SCALE        100.0e-6f
+#define CROSS_COUPLING_SCALE        10.0e-6f
 
 typedef struct {
     float P;