Yoichi Nagashima
/
CQ_nagasm_08_notch_test
add 60Hz/50Hz notch filter
notch.hpp@1:8fa2f521009a, 2014-12-16 (annotated)
- Committer:
- nagasm
- Date:
- Tue Dec 16 23:41:53 2014 +0000
- Revision:
- 1:8fa2f521009a
- Parent:
- 0:5ec74240d7f1
update - notch on/off, etc.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:5ec74240d7f1 | 1 | /* |
nagasm | 0:5ec74240d7f1 | 2 | #include <stdio.h> |
nagasm | 0:5ec74240d7f1 | 3 | #include <stdlib.h> |
nagasm | 0:5ec74240d7f1 | 4 | #include <math.h> |
nagasm | 0:5ec74240d7f1 | 5 | int main(){ |
nagasm | 0:5ec74240d7f1 | 6 | int fs = 10000; |
nagasm | 0:5ec74240d7f1 | 7 | double B0, a1, a2, b1, c0, F0, T; |
nagasm | 0:5ec74240d7f1 | 8 | F0 = 60; // or 50 |
nagasm | 0:5ec74240d7f1 | 9 | B0 = 100.; |
nagasm | 0:5ec74240d7f1 | 10 | T = 1./fs; |
nagasm | 0:5ec74240d7f1 | 11 | a1 = 2. * exp(-M_PI * B0 * T) * cos(2. * M_PI * F0 * T); |
nagasm | 0:5ec74240d7f1 | 12 | a2 = -exp(-2. * M_PI * B0 * T); |
nagasm | 0:5ec74240d7f1 | 13 | b1 = -2. * cos(2. * M_PI * F0 * T); |
nagasm | 0:5ec74240d7f1 | 14 | c0 = (1-a1-a2)/(2+b1); |
nagasm | 0:5ec74240d7f1 | 15 | printf("a1 = %f\n", a1); |
nagasm | 0:5ec74240d7f1 | 16 | printf("a2 = %f\n", a2); |
nagasm | 0:5ec74240d7f1 | 17 | printf("b1 = %f\n", b1); |
nagasm | 0:5ec74240d7f1 | 18 | printf("c0 = %f\n", c0); |
nagasm | 0:5ec74240d7f1 | 19 | return 0; |
nagasm | 0:5ec74240d7f1 | 20 | } |
nagasm | 0:5ec74240d7f1 | 21 | 60Hz |
nagasm | 0:5ec74240d7f1 | 22 | a1 = 1.936768 |
nagasm | 0:5ec74240d7f1 | 23 | a2 = -0.939101 |
nagasm | 0:5ec74240d7f1 | 24 | b1 = -1.998579 |
nagasm | 0:5ec74240d7f1 | 25 | c0 = 1.642174 |
nagasm | 0:5ec74240d7f1 | 26 | 50Hz |
nagasm | 0:5ec74240d7f1 | 27 | a1 = 1.937188 |
nagasm | 0:5ec74240d7f1 | 28 | a2 = -0.939101 |
nagasm | 0:5ec74240d7f1 | 29 | b1 = -1.999013 |
nagasm | 0:5ec74240d7f1 | 30 | c0 = 1.938304 |
nagasm | 0:5ec74240d7f1 | 31 | */ |
nagasm | 0:5ec74240d7f1 | 32 | |
nagasm | 0:5ec74240d7f1 | 33 | const float _60Hz_a1 = 1.936768, _60Hz_a2 = -0.939101, _60Hz_b1 = -1.998579, _60Hz_c0 = 1.642174; |
nagasm | 0:5ec74240d7f1 | 34 | const float _50Hz_a1 = 1.937188, _50Hz_a2 = -0.939101, _50Hz_b1 = -1.999013, _50Hz_c0 = 1.938304; |
nagasm | 0:5ec74240d7f1 | 35 | float a1, a2, b1, c0; |
nagasm | 0:5ec74240d7f1 | 36 | float y1[3], y2[3]; |
nagasm | 1:8fa2f521009a | 37 | int notch_1, notch_2; |
nagasm | 0:5ec74240d7f1 | 38 | |
nagasm | 0:5ec74240d7f1 | 39 | void coef_set(int herz){ |
nagasm | 0:5ec74240d7f1 | 40 | if(herz < 55){ |
nagasm | 0:5ec74240d7f1 | 41 | a1 = _50Hz_a1; |
nagasm | 0:5ec74240d7f1 | 42 | a2 = _50Hz_a2; |
nagasm | 0:5ec74240d7f1 | 43 | b1 = _50Hz_b1; |
nagasm | 0:5ec74240d7f1 | 44 | c0 = _50Hz_c0; |
nagasm | 0:5ec74240d7f1 | 45 | } |
nagasm | 0:5ec74240d7f1 | 46 | else{ |
nagasm | 0:5ec74240d7f1 | 47 | a1 = _60Hz_a1; |
nagasm | 0:5ec74240d7f1 | 48 | a2 = _60Hz_a2; |
nagasm | 0:5ec74240d7f1 | 49 | b1 = _60Hz_b1; |
nagasm | 0:5ec74240d7f1 | 50 | c0 = _60Hz_c0; |
nagasm | 0:5ec74240d7f1 | 51 | } |
nagasm | 0:5ec74240d7f1 | 52 | for (int i=0; i<3; i++) y1[i] = y2[i] = 0; |
nagasm | 0:5ec74240d7f1 | 53 | } |
nagasm | 0:5ec74240d7f1 | 54 | |
nagasm | 0:5ec74240d7f1 | 55 | float notch_filter1(float data){ |
nagasm | 0:5ec74240d7f1 | 56 | y1[0] = data + a1*y1[1] + a2*y1[2]; |
nagasm | 0:5ec74240d7f1 | 57 | float reault = y1[0] + b1*y1[1] + y1[2]; |
nagasm | 0:5ec74240d7f1 | 58 | y1[2] = y1[1]; |
nagasm | 0:5ec74240d7f1 | 59 | y1[1] = y1[0]; |
nagasm | 0:5ec74240d7f1 | 60 | return(reault); |
nagasm | 0:5ec74240d7f1 | 61 | } |
nagasm | 0:5ec74240d7f1 | 62 | |
nagasm | 0:5ec74240d7f1 | 63 | float notch_filter2(float data){ |
nagasm | 0:5ec74240d7f1 | 64 | y2[0] = data + a1*y2[1] + a2*y2[2]; |
nagasm | 0:5ec74240d7f1 | 65 | float reault = y2[0] + b1*y2[1] + y2[2]; |
nagasm | 0:5ec74240d7f1 | 66 | y2[2] = y2[1]; |
nagasm | 0:5ec74240d7f1 | 67 | y2[1] = y2[0]; |
nagasm | 0:5ec74240d7f1 | 68 | return(reault); |
nagasm | 0:5ec74240d7f1 | 69 | } |