Kim Bruil
/
EMG_Input
EMG input library biorobortics 31-10-2016
Fork of EMG by
filt.cpp@29:98406a20a42b, 2016-10-31 (annotated)
- Committer:
- kbruil
- Date:
- Mon Oct 31 12:30:20 2016 +0000
- Revision:
- 29:98406a20a42b
- Parent:
- 22:c01f61be07e0
Code project biorobotics 31-10-2016;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kbruil | 22:c01f61be07e0 | 1 | /* |
kbruil | 22:c01f61be07e0 | 2 | * FIR filter class, by Mike Perkins |
kbruil | 22:c01f61be07e0 | 3 | * |
kbruil | 22:c01f61be07e0 | 4 | * a simple C++ class for linear phase FIR filtering |
kbruil | 22:c01f61be07e0 | 5 | * |
kbruil | 22:c01f61be07e0 | 6 | * For background, see the post http://www.cardinalpeak.com/blog?p=1841 |
kbruil | 22:c01f61be07e0 | 7 | * |
kbruil | 22:c01f61be07e0 | 8 | * Copyright (c) 2013, Cardinal Peak, LLC. http://www.cardinalpeak.com |
kbruil | 22:c01f61be07e0 | 9 | * |
kbruil | 22:c01f61be07e0 | 10 | * Redistribution and use in source and binary forms, with or without |
kbruil | 22:c01f61be07e0 | 11 | * modification, are permitted provided that the following conditions |
kbruil | 22:c01f61be07e0 | 12 | * are met: |
kbruil | 22:c01f61be07e0 | 13 | * |
kbruil | 22:c01f61be07e0 | 14 | * 1) Redistributions of source code must retain the above copyright |
kbruil | 22:c01f61be07e0 | 15 | * notice, this list of conditions and the following disclaimer. |
kbruil | 22:c01f61be07e0 | 16 | * |
kbruil | 22:c01f61be07e0 | 17 | * 2) Redistributions in binary form must reproduce the above |
kbruil | 22:c01f61be07e0 | 18 | * copyright notice, this list of conditions and the following |
kbruil | 22:c01f61be07e0 | 19 | * disclaimer in the documentation and/or other materials provided |
kbruil | 22:c01f61be07e0 | 20 | * with the distribution. |
kbruil | 22:c01f61be07e0 | 21 | * |
kbruil | 22:c01f61be07e0 | 22 | * 3) Neither the name of Cardinal Peak nor the names of its |
kbruil | 22:c01f61be07e0 | 23 | * contributors may be used to endorse or promote products derived |
kbruil | 22:c01f61be07e0 | 24 | * from this software without specific prior written permission. |
kbruil | 22:c01f61be07e0 | 25 | * |
kbruil | 22:c01f61be07e0 | 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
kbruil | 22:c01f61be07e0 | 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
kbruil | 22:c01f61be07e0 | 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
kbruil | 22:c01f61be07e0 | 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL |
kbruil | 22:c01f61be07e0 | 30 | * CARDINAL PEAK, LLC BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
kbruil | 22:c01f61be07e0 | 31 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
kbruil | 22:c01f61be07e0 | 32 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF |
kbruil | 22:c01f61be07e0 | 33 | * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
kbruil | 22:c01f61be07e0 | 34 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
kbruil | 22:c01f61be07e0 | 35 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT |
kbruil | 22:c01f61be07e0 | 36 | * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
kbruil | 22:c01f61be07e0 | 37 | * SUCH DAMAGE. |
kbruil | 22:c01f61be07e0 | 38 | */ |
kbruil | 22:c01f61be07e0 | 39 | |
kbruil | 22:c01f61be07e0 | 40 | #include "filt.h" |
kbruil | 22:c01f61be07e0 | 41 | #define ECODE(x) {m_error_flag = x; return;} |
kbruil | 22:c01f61be07e0 | 42 | |
kbruil | 22:c01f61be07e0 | 43 | // Handles LPF and HPF case |
kbruil | 22:c01f61be07e0 | 44 | Filter::Filter(filterType filt_t, int num_taps, double Fs, double Fx) |
kbruil | 22:c01f61be07e0 | 45 | { |
kbruil | 22:c01f61be07e0 | 46 | m_error_flag = 0; |
kbruil | 22:c01f61be07e0 | 47 | m_filt_t = filt_t; |
kbruil | 22:c01f61be07e0 | 48 | m_num_taps = num_taps; |
kbruil | 22:c01f61be07e0 | 49 | m_Fs = Fs; |
kbruil | 22:c01f61be07e0 | 50 | m_Fx = Fx; |
kbruil | 22:c01f61be07e0 | 51 | m_lambda = M_PI * Fx / (Fs/2); |
kbruil | 22:c01f61be07e0 | 52 | |
kbruil | 22:c01f61be07e0 | 53 | if( Fs <= 0 ) ECODE(-1); |
kbruil | 22:c01f61be07e0 | 54 | if( Fx <= 0 || Fx >= Fs/2 ) ECODE(-2); |
kbruil | 22:c01f61be07e0 | 55 | if( m_num_taps <= 0 || m_num_taps > MAX_NUM_FILTER_TAPS ) ECODE(-3); |
kbruil | 22:c01f61be07e0 | 56 | |
kbruil | 22:c01f61be07e0 | 57 | m_taps = m_sr = NULL; |
kbruil | 22:c01f61be07e0 | 58 | m_taps = (double*)malloc( m_num_taps * sizeof(double) ); |
kbruil | 22:c01f61be07e0 | 59 | m_sr = (double*)malloc( m_num_taps * sizeof(double) ); |
kbruil | 22:c01f61be07e0 | 60 | if( m_taps == NULL || m_sr == NULL ) ECODE(-4); |
kbruil | 22:c01f61be07e0 | 61 | |
kbruil | 22:c01f61be07e0 | 62 | init(); |
kbruil | 22:c01f61be07e0 | 63 | |
kbruil | 22:c01f61be07e0 | 64 | if( m_filt_t == LPF ) designLPF(); |
kbruil | 22:c01f61be07e0 | 65 | else if( m_filt_t == HPF ) designHPF(); |
kbruil | 22:c01f61be07e0 | 66 | else ECODE(-5); |
kbruil | 22:c01f61be07e0 | 67 | |
kbruil | 22:c01f61be07e0 | 68 | return; |
kbruil | 22:c01f61be07e0 | 69 | } |
kbruil | 22:c01f61be07e0 | 70 | |
kbruil | 22:c01f61be07e0 | 71 | // Handles BPF case |
kbruil | 22:c01f61be07e0 | 72 | Filter::Filter(filterType filt_t, int num_taps, double Fs, double Fl, |
kbruil | 22:c01f61be07e0 | 73 | double Fu) |
kbruil | 22:c01f61be07e0 | 74 | { |
kbruil | 22:c01f61be07e0 | 75 | m_error_flag = 0; |
kbruil | 22:c01f61be07e0 | 76 | m_filt_t = filt_t; |
kbruil | 22:c01f61be07e0 | 77 | m_num_taps = num_taps; |
kbruil | 22:c01f61be07e0 | 78 | m_Fs = Fs; |
kbruil | 22:c01f61be07e0 | 79 | m_Fx = Fl; |
kbruil | 22:c01f61be07e0 | 80 | m_Fu = Fu; |
kbruil | 22:c01f61be07e0 | 81 | m_lambda = M_PI * Fl / (Fs/2); |
kbruil | 22:c01f61be07e0 | 82 | m_phi = M_PI * Fu / (Fs/2); |
kbruil | 22:c01f61be07e0 | 83 | |
kbruil | 22:c01f61be07e0 | 84 | if( Fs <= 0 ) ECODE(-10); |
kbruil | 22:c01f61be07e0 | 85 | if( Fl >= Fu ) ECODE(-11); |
kbruil | 22:c01f61be07e0 | 86 | if( Fl <= 0 || Fl >= Fs/2 ) ECODE(-12); |
kbruil | 22:c01f61be07e0 | 87 | if( Fu <= 0 || Fu >= Fs/2 ) ECODE(-13); |
kbruil | 22:c01f61be07e0 | 88 | if( m_num_taps <= 0 || m_num_taps > MAX_NUM_FILTER_TAPS ) ECODE(-14); |
kbruil | 22:c01f61be07e0 | 89 | |
kbruil | 22:c01f61be07e0 | 90 | m_taps = m_sr = NULL; |
kbruil | 22:c01f61be07e0 | 91 | m_taps = (double*)malloc( m_num_taps * sizeof(double) ); |
kbruil | 22:c01f61be07e0 | 92 | m_sr = (double*)malloc( m_num_taps * sizeof(double) ); |
kbruil | 22:c01f61be07e0 | 93 | if( m_taps == NULL || m_sr == NULL ) ECODE(-15); |
kbruil | 22:c01f61be07e0 | 94 | |
kbruil | 22:c01f61be07e0 | 95 | init(); |
kbruil | 22:c01f61be07e0 | 96 | |
kbruil | 22:c01f61be07e0 | 97 | if( m_filt_t == BPF ) designBPF(); |
kbruil | 22:c01f61be07e0 | 98 | else ECODE(-16); |
kbruil | 22:c01f61be07e0 | 99 | |
kbruil | 22:c01f61be07e0 | 100 | return; |
kbruil | 22:c01f61be07e0 | 101 | } |
kbruil | 22:c01f61be07e0 | 102 | |
kbruil | 22:c01f61be07e0 | 103 | Filter::~Filter() |
kbruil | 22:c01f61be07e0 | 104 | { |
kbruil | 22:c01f61be07e0 | 105 | if( m_taps != NULL ) free( m_taps ); |
kbruil | 22:c01f61be07e0 | 106 | if( m_sr != NULL ) free( m_sr ); |
kbruil | 22:c01f61be07e0 | 107 | } |
kbruil | 22:c01f61be07e0 | 108 | |
kbruil | 22:c01f61be07e0 | 109 | void |
kbruil | 22:c01f61be07e0 | 110 | Filter::designLPF() |
kbruil | 22:c01f61be07e0 | 111 | { |
kbruil | 22:c01f61be07e0 | 112 | int n; |
kbruil | 22:c01f61be07e0 | 113 | double mm; |
kbruil | 22:c01f61be07e0 | 114 | |
kbruil | 22:c01f61be07e0 | 115 | for(n = 0; n < m_num_taps; n++){ |
kbruil | 22:c01f61be07e0 | 116 | mm = n - (m_num_taps - 1.0) / 2.0; |
kbruil | 22:c01f61be07e0 | 117 | if( mm == 0.0 ) m_taps[n] = m_lambda / M_PI; |
kbruil | 22:c01f61be07e0 | 118 | else m_taps[n] = sin( mm * m_lambda ) / (mm * M_PI); |
kbruil | 22:c01f61be07e0 | 119 | } |
kbruil | 22:c01f61be07e0 | 120 | |
kbruil | 22:c01f61be07e0 | 121 | return; |
kbruil | 22:c01f61be07e0 | 122 | } |
kbruil | 22:c01f61be07e0 | 123 | |
kbruil | 22:c01f61be07e0 | 124 | void |
kbruil | 22:c01f61be07e0 | 125 | Filter::designHPF() |
kbruil | 22:c01f61be07e0 | 126 | { |
kbruil | 22:c01f61be07e0 | 127 | int n; |
kbruil | 22:c01f61be07e0 | 128 | double mm; |
kbruil | 22:c01f61be07e0 | 129 | |
kbruil | 22:c01f61be07e0 | 130 | for(n = 0; n < m_num_taps; n++){ |
kbruil | 22:c01f61be07e0 | 131 | mm = n - (m_num_taps - 1.0) / 2.0; |
kbruil | 22:c01f61be07e0 | 132 | if( mm == 0.0 ) m_taps[n] = 1.0 - m_lambda / M_PI; |
kbruil | 22:c01f61be07e0 | 133 | else m_taps[n] = -sin( mm * m_lambda ) / (mm * M_PI); |
kbruil | 22:c01f61be07e0 | 134 | } |
kbruil | 22:c01f61be07e0 | 135 | |
kbruil | 22:c01f61be07e0 | 136 | return; |
kbruil | 22:c01f61be07e0 | 137 | } |
kbruil | 22:c01f61be07e0 | 138 | |
kbruil | 22:c01f61be07e0 | 139 | void |
kbruil | 22:c01f61be07e0 | 140 | Filter::designBPF() |
kbruil | 22:c01f61be07e0 | 141 | { |
kbruil | 22:c01f61be07e0 | 142 | int n; |
kbruil | 22:c01f61be07e0 | 143 | double mm; |
kbruil | 22:c01f61be07e0 | 144 | |
kbruil | 22:c01f61be07e0 | 145 | for(n = 0; n < m_num_taps; n++){ |
kbruil | 22:c01f61be07e0 | 146 | mm = n - (m_num_taps - 1.0) / 2.0; |
kbruil | 22:c01f61be07e0 | 147 | if( mm == 0.0 ) m_taps[n] = (m_phi - m_lambda) / M_PI; |
kbruil | 22:c01f61be07e0 | 148 | else m_taps[n] = ( sin( mm * m_phi ) - |
kbruil | 22:c01f61be07e0 | 149 | sin( mm * m_lambda ) ) / (mm * M_PI); |
kbruil | 22:c01f61be07e0 | 150 | } |
kbruil | 22:c01f61be07e0 | 151 | |
kbruil | 22:c01f61be07e0 | 152 | return; |
kbruil | 22:c01f61be07e0 | 153 | } |
kbruil | 22:c01f61be07e0 | 154 | |
kbruil | 22:c01f61be07e0 | 155 | void |
kbruil | 22:c01f61be07e0 | 156 | Filter::get_taps( double *taps ) |
kbruil | 22:c01f61be07e0 | 157 | { |
kbruil | 22:c01f61be07e0 | 158 | int i; |
kbruil | 22:c01f61be07e0 | 159 | |
kbruil | 22:c01f61be07e0 | 160 | if( m_error_flag != 0 ) return; |
kbruil | 22:c01f61be07e0 | 161 | |
kbruil | 22:c01f61be07e0 | 162 | for(i = 0; i < m_num_taps; i++) taps[i] = m_taps[i]; |
kbruil | 22:c01f61be07e0 | 163 | |
kbruil | 22:c01f61be07e0 | 164 | return; |
kbruil | 22:c01f61be07e0 | 165 | } |
kbruil | 22:c01f61be07e0 | 166 | |
kbruil | 22:c01f61be07e0 | 167 | int |
kbruil | 22:c01f61be07e0 | 168 | Filter::write_taps_to_file( char *filename ) |
kbruil | 22:c01f61be07e0 | 169 | { |
kbruil | 22:c01f61be07e0 | 170 | FILE *fd; |
kbruil | 22:c01f61be07e0 | 171 | |
kbruil | 22:c01f61be07e0 | 172 | if( m_error_flag != 0 ) return -1; |
kbruil | 22:c01f61be07e0 | 173 | |
kbruil | 22:c01f61be07e0 | 174 | int i; |
kbruil | 22:c01f61be07e0 | 175 | fd = fopen(filename, "w"); |
kbruil | 22:c01f61be07e0 | 176 | if( fd == NULL ) return -1; |
kbruil | 22:c01f61be07e0 | 177 | |
kbruil | 22:c01f61be07e0 | 178 | fprintf(fd, "%d\n", m_num_taps); |
kbruil | 22:c01f61be07e0 | 179 | for(i = 0; i < m_num_taps; i++){ |
kbruil | 22:c01f61be07e0 | 180 | fprintf(fd, "%15.6f\n", m_taps[i]); |
kbruil | 22:c01f61be07e0 | 181 | } |
kbruil | 22:c01f61be07e0 | 182 | fclose(fd); |
kbruil | 22:c01f61be07e0 | 183 | |
kbruil | 22:c01f61be07e0 | 184 | return 0; |
kbruil | 22:c01f61be07e0 | 185 | } |
kbruil | 22:c01f61be07e0 | 186 | |
kbruil | 22:c01f61be07e0 | 187 | // Output the magnitude of the frequency response in dB |
kbruil | 22:c01f61be07e0 | 188 | #define NP 1000 |
kbruil | 22:c01f61be07e0 | 189 | int |
kbruil | 22:c01f61be07e0 | 190 | Filter::write_freqres_to_file( char *filename ) |
kbruil | 22:c01f61be07e0 | 191 | { |
kbruil | 22:c01f61be07e0 | 192 | FILE *fd; |
kbruil | 22:c01f61be07e0 | 193 | int i, k; |
kbruil | 22:c01f61be07e0 | 194 | double w, dw; |
kbruil | 22:c01f61be07e0 | 195 | double y_r[NP], y_i[NP], y_mag[NP]; |
kbruil | 22:c01f61be07e0 | 196 | double mag_max = -1; |
kbruil | 22:c01f61be07e0 | 197 | double tmp_d; |
kbruil | 22:c01f61be07e0 | 198 | |
kbruil | 22:c01f61be07e0 | 199 | if( m_error_flag != 0 ) return -1; |
kbruil | 22:c01f61be07e0 | 200 | |
kbruil | 22:c01f61be07e0 | 201 | dw = M_PI / (NP - 1.0); |
kbruil | 22:c01f61be07e0 | 202 | for(i = 0; i < NP; i++){ |
kbruil | 22:c01f61be07e0 | 203 | w = i*dw; |
kbruil | 22:c01f61be07e0 | 204 | y_r[i] = y_i[i] = 0; |
kbruil | 22:c01f61be07e0 | 205 | for(k = 0; k < m_num_taps; k++){ |
kbruil | 22:c01f61be07e0 | 206 | y_r[i] += m_taps[k] * cos(k * w); |
kbruil | 22:c01f61be07e0 | 207 | y_i[i] -= m_taps[k] * sin(k * w); |
kbruil | 22:c01f61be07e0 | 208 | } |
kbruil | 22:c01f61be07e0 | 209 | } |
kbruil | 22:c01f61be07e0 | 210 | |
kbruil | 22:c01f61be07e0 | 211 | for(i = 0; i < NP; i++){ |
kbruil | 22:c01f61be07e0 | 212 | y_mag[i] = sqrt( y_r[i]*y_r[i] + y_i[i]*y_i[i] ); |
kbruil | 22:c01f61be07e0 | 213 | if( y_mag[i] > mag_max ) mag_max = y_mag[i]; |
kbruil | 22:c01f61be07e0 | 214 | } |
kbruil | 22:c01f61be07e0 | 215 | |
kbruil | 22:c01f61be07e0 | 216 | if( mag_max <= 0.0 ) return -2; |
kbruil | 22:c01f61be07e0 | 217 | |
kbruil | 22:c01f61be07e0 | 218 | fd = fopen(filename, "w"); |
kbruil | 22:c01f61be07e0 | 219 | if( fd == NULL ) return -3; |
kbruil | 22:c01f61be07e0 | 220 | |
kbruil | 22:c01f61be07e0 | 221 | for(i = 0; i < NP; i++){ |
kbruil | 22:c01f61be07e0 | 222 | w = i*dw; |
kbruil | 22:c01f61be07e0 | 223 | if( y_mag[i] == 0 ) tmp_d = -100; |
kbruil | 22:c01f61be07e0 | 224 | else{ |
kbruil | 22:c01f61be07e0 | 225 | tmp_d = 20 * log10( y_mag[i] / mag_max ); |
kbruil | 22:c01f61be07e0 | 226 | if( tmp_d < -100 ) tmp_d = -100; |
kbruil | 22:c01f61be07e0 | 227 | } |
kbruil | 22:c01f61be07e0 | 228 | fprintf(fd, "%10.6e %10.6e\n", w * (m_Fs/2)/M_PI, tmp_d); |
kbruil | 22:c01f61be07e0 | 229 | } |
kbruil | 22:c01f61be07e0 | 230 | |
kbruil | 22:c01f61be07e0 | 231 | fclose(fd); |
kbruil | 22:c01f61be07e0 | 232 | return 0; |
kbruil | 22:c01f61be07e0 | 233 | } |
kbruil | 22:c01f61be07e0 | 234 | |
kbruil | 22:c01f61be07e0 | 235 | void |
kbruil | 22:c01f61be07e0 | 236 | Filter::init() |
kbruil | 22:c01f61be07e0 | 237 | { |
kbruil | 22:c01f61be07e0 | 238 | int i; |
kbruil | 22:c01f61be07e0 | 239 | |
kbruil | 22:c01f61be07e0 | 240 | if( m_error_flag != 0 ) return; |
kbruil | 22:c01f61be07e0 | 241 | |
kbruil | 22:c01f61be07e0 | 242 | for(i = 0; i < m_num_taps; i++) m_sr[i] = 0; |
kbruil | 22:c01f61be07e0 | 243 | |
kbruil | 22:c01f61be07e0 | 244 | return; |
kbruil | 22:c01f61be07e0 | 245 | } |
kbruil | 22:c01f61be07e0 | 246 | |
kbruil | 22:c01f61be07e0 | 247 | double |
kbruil | 22:c01f61be07e0 | 248 | Filter::do_sample(double data_sample) |
kbruil | 22:c01f61be07e0 | 249 | { |
kbruil | 22:c01f61be07e0 | 250 | int i; |
kbruil | 22:c01f61be07e0 | 251 | double result; |
kbruil | 22:c01f61be07e0 | 252 | |
kbruil | 22:c01f61be07e0 | 253 | if( m_error_flag != 0 ) return(0); |
kbruil | 22:c01f61be07e0 | 254 | |
kbruil | 22:c01f61be07e0 | 255 | for(i = m_num_taps - 1; i >= 1; i--){ |
kbruil | 22:c01f61be07e0 | 256 | m_sr[i] = m_sr[i-1]; |
kbruil | 22:c01f61be07e0 | 257 | } |
kbruil | 22:c01f61be07e0 | 258 | m_sr[0] = data_sample; |
kbruil | 22:c01f61be07e0 | 259 | |
kbruil | 22:c01f61be07e0 | 260 | result = 0; |
kbruil | 22:c01f61be07e0 | 261 | for(i = 0; i < m_num_taps; i++) result += m_sr[i] * m_taps[i]; |
kbruil | 22:c01f61be07e0 | 262 | |
kbruil | 22:c01f61be07e0 | 263 | return result; |
kbruil | 22:c01f61be07e0 | 264 | } |