Diff: Filter.cpp

Revision:

5:ab6351c18264

Parent:

4:fc9412c8334e

Child:

6:13ff4bea3c83

--- a/Filter.cpp	Mon Jan 15 01:57:38 2018 +0000
+++ b/Filter.cpp	Tue Jan 16 15:56:33 2018 +0000
@@ -25,27 +25,29 @@
     }
 }
 
-void Filter::setNotchPara(double Omega)
+void Filter::setNotchPara(double Omega, double int_data)
 {
     // 落としたい角周波数[rad/s]をOm_nに入れる
     Om_n = Omega;
     sq_Om = pow(Om_n, 2.0); // Om_nの2乗
     sq_dt = pow(int_time, 2.0); // dtの2乗
-
+    
+    n_preOutput[0] = int_data;
+    n_preOutput[1] = int_data;
+    
+    n_preInput[0] = int_data;
+    n_preInput[1] = int_data;
 }
 
 double Filter::NotchFilter(double input)
 {
-    static double preOutput[2] = {0.0, 0.0};
-    static double preInput[2] = {0.0, 0.0};
-
-    double Output = (2*(1 + Om_n * int_time) * preOutput[0]-preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * preInput[0] + preInput[1]) / (1 + 2 * Om_n * int_time + sq_Om * sq_dt);
+    double Output = (2*(1 + Om_n * int_time) * n_preOutput[0]-n_preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * n_preInput[0] + n_preInput[1]) / (1 + 2 * Om_n * int_time + sq_Om * sq_dt);
 
-    preInput[1] = preInput[0];
-    preInput[0] = input;
+    n_preInput[1] = n_preInput[0];
+    n_preInput[0] = input;
 
-    preOutput[1] = preOutput[0];
-    preOutput[0] = Output;
+    n_preOutput[1] = n_preOutput[0];
+    n_preOutput[0] = Output;
 
     return Output;
 }
\ No newline at end of file