projectR講習2018 / Filter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of Filter by Yuki Ueno

Filter.cpp@1:637f9a61b133, 2018-01-11 (annotated)

Committer:
uenono
Date:
Thu Jan 11 06:53:03 2018 +0000
Revision:
1:637f9a61b133
Parent:
0:17706252d1ed
Child:
2:a842c1a33e4f
INT_TIME?????????privulate?????

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 0:17706252d1ed 6 }
uenono 0:17706252d1ed 7
uenono 0:17706252d1ed 8 void Filter::setLowPassPara(double T)
uenono 0:17706252d1ed 9 {
uenono 0:17706252d1ed 10 T_LPF = T;
uenono 0:17706252d1ed 11 }
uenono 0:17706252d1ed 12
uenono 0:17706252d1ed 13 double Filter::LowPassFilter(double input)
uenono 0:17706252d1ed 14 {
uenono 0:17706252d1ed 15 static double preOutput = 0.0;
uenono 0:17706252d1ed 16
uenono 0:17706252d1ed 17 double Output = (INT_TIME * input + T_LPF * preOutput)/(T_LPF + INT_TIME);
uenono 0:17706252d1ed 18
uenono 0:17706252d1ed 19 preOutput = Output;
uenono 0:17706252d1ed 20
uenono 0:17706252d1ed 21 return Output;
uenono 0:17706252d1ed 22 }
uenono 0:17706252d1ed 23
uenono 0:17706252d1ed 24 void Filter::setNotchPara(double Omega)
uenono 0:17706252d1ed 25 {
uenono 0:17706252d1ed 26 // 落としたい角周波数[rad/s]をOm_nに入れる
uenono 0:17706252d1ed 27 Om_n = Omega;
uenono 0:17706252d1ed 28 sq_Om = pow(Om_n, 2.0); // Om_nの2乗
uenono 0:17706252d1ed 29 sq_dt = pow(INT_TIME, 2.0); // dtの2乗
uenono 0:17706252d1ed 30
uenono 0:17706252d1ed 31 }
uenono 0:17706252d1ed 32
uenono 0:17706252d1ed 33 double Filter::NotchFilter(double input)
uenono 0:17706252d1ed 34 {
uenono 0:17706252d1ed 35 static double preOutput[2] = {0.0, 0.0};
uenono 0:17706252d1ed 36 static double preInput[2] = {0.0, 0.0};
uenono 0:17706252d1ed 37
uenono 0:17706252d1ed 38 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 0:17706252d1ed 39
uenono 0:17706252d1ed 40 preInput[1] = preInput[0];
uenono 0:17706252d1ed 41 preInput[0] = input;
uenono 0:17706252d1ed 42
uenono 0:17706252d1ed 43 preOutput[1] = preOutput[0];
uenono 0:17706252d1ed 44 preOutput[0] = Output;
uenono 0:17706252d1ed 45
uenono 0:17706252d1ed 46 return Output;
uenono 0:17706252d1ed 47 }