daiki hiraoka / Mbed 2 deprecated EMG

筋電センサプログラム

Dependencies:   mbed-rtos mbed

筋電

FilterTest.cpp@13:94ce8bd0c0e1, 2018-01-04 (annotated)

Committer:
dhpt
Date:
Thu Jan 04 06:30:00 2018 +0000
Revision:
13:94ce8bd0c0e1
Parent:
3:5d91bfe4a79d 
???????????????; ????read?3.3????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dhpt 3:5d91bfe4a79d 1 /**
dhpt 3:5d91bfe4a79d 2 * @file FilterTest.cpp
dhpt 3:5d91bfe4a79d 3 * @brief Calculate filters
dhpt 3:5d91bfe4a79d 4 * HPF: 1st order / LPF: 2nd order / Notch: 2nd order
dhpt 3:5d91bfe4a79d 5 * @date 2015.02.22
dhpt 3:5d91bfe4a79d 6 * @version 1.0.1
dhpt 3:5d91bfe4a79d 7 */
dhpt 3:5d91bfe4a79d 8 #include "FilterTest.h"
dhpt 3:5d91bfe4a79d 9
dhpt 3:5d91bfe4a79d 10 /** Constructor
dhpt 3:5d91bfe4a79d 11 */
dhpt 3:5d91bfe4a79d 12 FilterTest::FilterTest ()
dhpt 3:5d91bfe4a79d 13 {
dhpt 3:5d91bfe4a79d 14 set_hpf_coef(INIT_HB, INIT_HA);
dhpt 3:5d91bfe4a79d 15 set_lpf_coef(INIT_LB, INIT_LA1, INIT_LA2);
dhpt 3:5d91bfe4a79d 16 set_brf_coef(INIT_NB, INIT_NA1, INIT_NA2);
dhpt 3:5d91bfe4a79d 17 reset_hpf_buf();
dhpt 3:5d91bfe4a79d 18 reset_lpf_buf();
dhpt 3:5d91bfe4a79d 19 reset_brf_buf();
dhpt 3:5d91bfe4a79d 20 }
dhpt 3:5d91bfe4a79d 21
dhpt 3:5d91bfe4a79d 22 /** Calculate filter
dhpt 3:5d91bfe4a79d 23 * @param val Input value
dhpt 3:5d91bfe4a79d 24 * @param hpf_on High pass filter enable
dhpt 3:5d91bfe4a79d 25 * @param lpf_on Low pass filter enable
dhpt 3:5d91bfe4a79d 26 * @param brf_on Notch filter enable
dhpt 3:5d91bfe4a79d 27 * @return Output value
dhpt 3:5d91bfe4a79d 28 */
dhpt 3:5d91bfe4a79d 29 double FilterTest::calc(double val, int hpf_on, int lpf_on, int brf_on)
dhpt 3:5d91bfe4a79d 30 {
dhpt 3:5d91bfe4a79d 31 double retVal = val;
dhpt 3:5d91bfe4a79d 32 /* High pass filter */
dhpt 3:5d91bfe4a79d 33 if(hpf_on) {
dhpt 3:5d91bfe4a79d 34 retVal = hpf(retVal);
dhpt 3:5d91bfe4a79d 35 } else {
dhpt 3:5d91bfe4a79d 36 reset_hpf_buf();
dhpt 3:5d91bfe4a79d 37 }
dhpt 3:5d91bfe4a79d 38 /* Low pass filter */
dhpt 3:5d91bfe4a79d 39 if(lpf_on) {
dhpt 3:5d91bfe4a79d 40 retVal = lpf(retVal);
dhpt 3:5d91bfe4a79d 41 } else {
dhpt 3:5d91bfe4a79d 42 reset_lpf_buf();
dhpt 3:5d91bfe4a79d 43 }
dhpt 3:5d91bfe4a79d 44 /* Notch (Band reject) filter */
dhpt 3:5d91bfe4a79d 45 if(brf_on) {
dhpt 3:5d91bfe4a79d 46 retVal = brf(retVal);
dhpt 3:5d91bfe4a79d 47 } else {
dhpt 3:5d91bfe4a79d 48 reset_brf_buf();
dhpt 3:5d91bfe4a79d 49 }
dhpt 3:5d91bfe4a79d 50 return retVal;
dhpt 3:5d91bfe4a79d 51 }
dhpt 3:5d91bfe4a79d 52
dhpt 3:5d91bfe4a79d 53 /** Reset delay buffers for high pass filter
dhpt 3:5d91bfe4a79d 54 */
dhpt 3:5d91bfe4a79d 55 void FilterTest::reset_hpf_buf()
dhpt 3:5d91bfe4a79d 56 {
dhpt 3:5d91bfe4a79d 57 _hw = 0.0;
dhpt 3:5d91bfe4a79d 58 }
dhpt 3:5d91bfe4a79d 59
dhpt 3:5d91bfe4a79d 60 /** Reset delay buffers for low pass filter
dhpt 3:5d91bfe4a79d 61 */
dhpt 3:5d91bfe4a79d 62 void FilterTest::reset_lpf_buf()
dhpt 3:5d91bfe4a79d 63 {
dhpt 3:5d91bfe4a79d 64 _lw1 = 0.0;
dhpt 3:5d91bfe4a79d 65 _lw2 = 0.0;
dhpt 3:5d91bfe4a79d 66 }
dhpt 3:5d91bfe4a79d 67
dhpt 3:5d91bfe4a79d 68 /** Reset delay buffers for notch filter
dhpt 3:5d91bfe4a79d 69 */
dhpt 3:5d91bfe4a79d 70 void FilterTest::reset_brf_buf()
dhpt 3:5d91bfe4a79d 71 {
dhpt 3:5d91bfe4a79d 72 _nw1 = 0.0;
dhpt 3:5d91bfe4a79d 73 _nw2 = 0.0;
dhpt 3:5d91bfe4a79d 74 }
dhpt 3:5d91bfe4a79d 75
dhpt 3:5d91bfe4a79d 76 /** Set coefficient for HPF
dhpt 3:5d91bfe4a79d 77 * @param hb Numerator cofficient
dhpt 3:5d91bfe4a79d 78 * @param ha Denominator cofficient
dhpt 3:5d91bfe4a79d 79 * @retval true OK
dhpt 3:5d91bfe4a79d 80 * @retval false NG
dhpt 3:5d91bfe4a79d 81 */
dhpt 3:5d91bfe4a79d 82 bool FilterTest:: set_hpf_coef(double hb, double ha)
dhpt 3:5d91bfe4a79d 83 {
dhpt 3:5d91bfe4a79d 84 if(hb > 1.0) {
dhpt 3:5d91bfe4a79d 85 return false;
dhpt 3:5d91bfe4a79d 86 } else if((ha > 1.0)||(ha < -1.0)) {
dhpt 3:5d91bfe4a79d 87 return false;
dhpt 3:5d91bfe4a79d 88 } else {
dhpt 3:5d91bfe4a79d 89 _hb = hb;
dhpt 3:5d91bfe4a79d 90 _ha = ha;
dhpt 3:5d91bfe4a79d 91 reset_hpf_buf();
dhpt 3:5d91bfe4a79d 92 return true;
dhpt 3:5d91bfe4a79d 93 }
dhpt 3:5d91bfe4a79d 94 }
dhpt 3:5d91bfe4a79d 95
dhpt 3:5d91bfe4a79d 96 /** Set coefficient for LPF
dhpt 3:5d91bfe4a79d 97 * @param lb Numerator cofficient
dhpt 3:5d91bfe4a79d 98 * @param la1 Denominator cofficient 1
dhpt 3:5d91bfe4a79d 99 * @param la2 Denominator cofficient 2
dhpt 3:5d91bfe4a79d 100 * @retval true OK
dhpt 3:5d91bfe4a79d 101 * @retval false NG
dhpt 3:5d91bfe4a79d 102 */
dhpt 3:5d91bfe4a79d 103 bool FilterTest:: set_lpf_coef(double lb, double la1, double la2)
dhpt 3:5d91bfe4a79d 104 {
dhpt 3:5d91bfe4a79d 105 if(lb > 1.0) {
dhpt 3:5d91bfe4a79d 106 return false;
dhpt 3:5d91bfe4a79d 107 } else if((la1 > 2.0)||(la1 < -2.0)) {
dhpt 3:5d91bfe4a79d 108 return false;
dhpt 3:5d91bfe4a79d 109 } else if(la2 > 1.0) {
dhpt 3:5d91bfe4a79d 110 return false;
dhpt 3:5d91bfe4a79d 111 } else {
dhpt 3:5d91bfe4a79d 112 _lb = lb;
dhpt 3:5d91bfe4a79d 113 _la1 = la1;
dhpt 3:5d91bfe4a79d 114 _la2 = la2;
dhpt 3:5d91bfe4a79d 115 reset_lpf_buf();
dhpt 3:5d91bfe4a79d 116 return true;
dhpt 3:5d91bfe4a79d 117 }
dhpt 3:5d91bfe4a79d 118 }
dhpt 3:5d91bfe4a79d 119
dhpt 3:5d91bfe4a79d 120 /** Set coefficient for BRF
dhpt 3:5d91bfe4a79d 121 * @param nb Numerator cofficient
dhpt 3:5d91bfe4a79d 122 * @param na1 Denominator cofficient 1
dhpt 3:5d91bfe4a79d 123 * @param na2 Denominator cofficient 2
dhpt 3:5d91bfe4a79d 124 * @retval true OK
dhpt 3:5d91bfe4a79d 125 * @retval false NG
dhpt 3:5d91bfe4a79d 126 */
dhpt 3:5d91bfe4a79d 127 bool FilterTest:: set_brf_coef(double nb, double na1, double na2)
dhpt 3:5d91bfe4a79d 128 {
dhpt 3:5d91bfe4a79d 129 if(nb > 1.0) {
dhpt 3:5d91bfe4a79d 130 return false;
dhpt 3:5d91bfe4a79d 131 } else if((na1 > 2.0)||(na1 < -2.0)) {
dhpt 3:5d91bfe4a79d 132 return false;
dhpt 3:5d91bfe4a79d 133 } else if(na2 > 1.0) {
dhpt 3:5d91bfe4a79d 134 return false;
dhpt 3:5d91bfe4a79d 135 } else {
dhpt 3:5d91bfe4a79d 136 _nb = nb;
dhpt 3:5d91bfe4a79d 137 _na1 = na1;
dhpt 3:5d91bfe4a79d 138 _na2 = na2;
dhpt 3:5d91bfe4a79d 139 reset_brf_buf();
dhpt 3:5d91bfe4a79d 140 return true;
dhpt 3:5d91bfe4a79d 141 }
dhpt 3:5d91bfe4a79d 142 }
dhpt 3:5d91bfe4a79d 143
dhpt 3:5d91bfe4a79d 144 /** High pass filter (1st order)
dhpt 3:5d91bfe4a79d 145 * @param x Input value
dhpt 3:5d91bfe4a79d 146 * @return Output value
dhpt 3:5d91bfe4a79d 147 *
dhpt 3:5d91bfe4a79d 148 * hb v +
dhpt 3:5d91bfe4a79d 149 * x ---I>---+---O-------+--- y
dhpt 3:5d91bfe4a79d 150 * | hw|- |
dhpt 3:5d91bfe4a79d 151 * | [z] |
dhpt 3:5d91bfe4a79d 152 * | | ha |
dhpt 3:5d91bfe4a79d 153 * +---O---<I--+
dhpt 3:5d91bfe4a79d 154 */
dhpt 3:5d91bfe4a79d 155 double FilterTest::hpf(double x)
dhpt 3:5d91bfe4a79d 156 {
dhpt 3:5d91bfe4a79d 157 double v, y;
dhpt 3:5d91bfe4a79d 158
dhpt 3:5d91bfe4a79d 159 v = _hb * x;
dhpt 3:5d91bfe4a79d 160 y = v - _hw;
dhpt 3:5d91bfe4a79d 161 _hw = v + _ha * y;
dhpt 3:5d91bfe4a79d 162 return y;
dhpt 3:5d91bfe4a79d 163 }
dhpt 3:5d91bfe4a79d 164
dhpt 3:5d91bfe4a79d 165 /** Low pass filter (2nd order)
dhpt 3:5d91bfe4a79d 166 * @param x Input value
dhpt 3:5d91bfe4a79d 167 * @return Output value
dhpt 3:5d91bfe4a79d 168 *
dhpt 3:5d91bfe4a79d 169 * lb v
dhpt 3:5d91bfe4a79d 170 * x --I>--+-----O-------+--- y
dhpt 3:5d91bfe4a79d 171 * | lw1| |
dhpt 3:5d91bfe4a79d 172 * | [z] |
dhpt 3:5d91bfe4a79d 173 * | 2 | -la1 |
dhpt 3:5d91bfe4a79d 174 * +-I>--O---<I--+
dhpt 3:5d91bfe4a79d 175 * | lw2| |
dhpt 3:5d91bfe4a79d 176 * | [z] |
dhpt 3:5d91bfe4a79d 177 * | | -la2 |
dhpt 3:5d91bfe4a79d 178 * +-----O---<I--+
dhpt 3:5d91bfe4a79d 179 */
dhpt 3:5d91bfe4a79d 180 double FilterTest::lpf(double x)
dhpt 3:5d91bfe4a79d 181 {
dhpt 3:5d91bfe4a79d 182 double v, y;
dhpt 3:5d91bfe4a79d 183
dhpt 3:5d91bfe4a79d 184 v = _lb * x;
dhpt 3:5d91bfe4a79d 185 y = v + _lw1;
dhpt 3:5d91bfe4a79d 186 _lw1 = 2 * v - _la1 * y + _lw2;
dhpt 3:5d91bfe4a79d 187 _lw2 = v - _la2 * y;
dhpt 3:5d91bfe4a79d 188 return y;
dhpt 3:5d91bfe4a79d 189 }
dhpt 3:5d91bfe4a79d 190
dhpt 3:5d91bfe4a79d 191 /** Notch filter (Band reject filter) (2nd order)
dhpt 3:5d91bfe4a79d 192 * @param x Input value
dhpt 3:5d91bfe4a79d 193 * @return Output value
dhpt 3:5d91bfe4a79d 194 *
dhpt 3:5d91bfe4a79d 195 * nb v
dhpt 3:5d91bfe4a79d 196 * x -+-I>--+----O-------+--- y
dhpt 3:5d91bfe4a79d 197 * | | nw1| |
dhpt 3:5d91bfe4a79d 198 * | | [z] |
dhpt 3:5d91bfe4a79d 199 * +| na1 | | |
dhpt 3:5d91bfe4a79d 200 * O-I>-------O |
dhpt 3:5d91bfe4a79d 201 * -| | nw2| |
dhpt 3:5d91bfe4a79d 202 * | | [z] |
dhpt 3:5d91bfe4a79d 203 * | | | -na2 |
dhpt 3:5d91bfe4a79d 204 * | +----O---<I--+
dhpt 3:5d91bfe4a79d 205 * | |
dhpt 3:5d91bfe4a79d 206 * +------------------+
dhpt 3:5d91bfe4a79d 207 */
dhpt 3:5d91bfe4a79d 208 double FilterTest::brf(double x)
dhpt 3:5d91bfe4a79d 209 {
dhpt 3:5d91bfe4a79d 210 double v, y;
dhpt 3:5d91bfe4a79d 211
dhpt 3:5d91bfe4a79d 212 v = _nb * x;
dhpt 3:5d91bfe4a79d 213 y = v + _nw1;
dhpt 3:5d91bfe4a79d 214 _nw1 = _na1 * ( x - y ) + _nw2;
dhpt 3:5d91bfe4a79d 215 _nw2 = v - _na2 * y;
dhpt 3:5d91bfe4a79d 216 return y;
dhpt 3:5d91bfe4a79d 217 }