projectR講習2018 / Filter

1次遅れと2次遅れ，ノッチフィルターを実装

Fork of Filter by Yuki Ueno

Filter.cpp@4:fc9412c8334e, 2018-01-15 (annotated)

Committer:
uenono
Date:
Mon Jan 15 01:57:38 2018 +0000
Revision:
4:fc9412c8334e
Parent:
3:987d5d78f863
Child:
5:ab6351c18264
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uenono 0:17706252d1ed 1 #include "Filter.h"
uenono 0:17706252d1ed 2
uenono 1:637f9a61b133 3 Filter::Filter(double xint_time)
uenono 0:17706252d1ed 4 {
uenono 1:637f9a61b133 5 int_time = xint_time;
uenono 3:987d5d78f863 6 set_t = false;
uenono 0:17706252d1ed 7 }
uenono 0:17706252d1ed 8
uenono 4:fc9412c8334e 9 void Filter::setLowPassPara(double T, double init_data)
uenono 0:17706252d1ed 10 {
uenono 0:17706252d1ed 11 T_LPF = T;
uenono 4:fc9412c8334e 12 preOutput = init_data;
uenono 3:987d5d78f863 13 set_t = true;
uenono 0:17706252d1ed 14 }
uenono 0:17706252d1ed 15
uenono 0:17706252d1ed 16 double Filter::LowPassFilter(double input)
uenono 0:17706252d1ed 17 {
uenono 3:987d5d78f863 18 //static double preOutput = input;
uenono 3:987d5d78f863 19 if(!set_t) {
uenono 3:987d5d78f863 20 return input;
uenono 3:987d5d78f863 21 } else {
uenono 3:987d5d78f863 22 double Output = (int_time * input + T_LPF * preOutput)/(T_LPF + int_time);
uenono 3:987d5d78f863 23 preOutput = Output;
uenono 3:987d5d78f863 24 return Output;
uenono 3:987d5d78f863 25 }
uenono 0:17706252d1ed 26 }
uenono 0:17706252d1ed 27
uenono 0:17706252d1ed 28 void Filter::setNotchPara(double Omega)
uenono 0:17706252d1ed 29 {
uenono 0:17706252d1ed 30 // 落としたい角周波数[rad/s]をOm_nに入れる
uenono 0:17706252d1ed 31 Om_n = Omega;
uenono 0:17706252d1ed 32 sq_Om = pow(Om_n, 2.0); // Om_nの2乗
uenono 2:a842c1a33e4f 33 sq_dt = pow(int_time, 2.0); // dtの2乗
uenono 0:17706252d1ed 34
uenono 0:17706252d1ed 35 }
uenono 0:17706252d1ed 36
uenono 0:17706252d1ed 37 double Filter::NotchFilter(double input)
uenono 0:17706252d1ed 38 {
uenono 0:17706252d1ed 39 static double preOutput[2] = {0.0, 0.0};
uenono 0:17706252d1ed 40 static double preInput[2] = {0.0, 0.0};
uenono 3:987d5d78f863 41
uenono 2:a842c1a33e4f 42 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);
uenono 3:987d5d78f863 43
uenono 0:17706252d1ed 44 preInput[1] = preInput[0];
uenono 0:17706252d1ed 45 preInput[0] = input;
uenono 3:987d5d78f863 46
uenono 0:17706252d1ed 47 preOutput[1] = preOutput[0];
uenono 0:17706252d1ed 48 preOutput[0] = Output;
uenono 3:987d5d78f863 49
uenono 0:17706252d1ed 50 return Output;
uenono 0:17706252d1ed 51 }