Kim Bruil / Mbed 2 deprecated EMG_Input

EMG input library biorobortics 31-10-2016

Dependencies:   HIDScope mbed

Fork of EMG by Tom Tom

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?

UserRevisionLine numberNew 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 }