├── FIR_filter.c ├── IIR_filter.c ├── Makefile ├── README.md ├── ana.m ├── ana7.m ├── beamer.h ├── beamer7.h ├── beamformer.c ├── channel7.c ├── filter.dat ├── main.c ├── noise.c ├── signal.c └── tags /FIR_filter.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | File Name: FIR_filter.c 3 | Author: Fang Yuan 4 | Mail: yfang@nju.edu.cn 5 | Created Time: Thu 07 Jul 2016 10:18:25 PM CST 6 | ************************************************************************/ 7 | 8 | #include "beamer.h" 9 | 10 | void Blackman_window(float f1, float f2, float *filter) 11 | { 12 | float window, hd; 13 | float omega1, omega2; 14 | int i; 15 | 16 | omega1 = 2 * M_PI * f1 / FS; 17 | omega2 = 2 * M_PI * f2 / FS; 18 | 19 | for(i = 1; i <= (TAPS - 1) / 2; i++) 20 | { 21 | window = 0.42 + 0.5 * cos(2 * M_PI * i / (TAPS - 1)) 22 | + 0.08 * cos(4 * M_PI * i / (TAPS - 1)); 23 | hd = (sin(omega2 * i) - sin(omega1 * i)) / M_PI / i; 24 | filter[(TAPS - 1)/2 + i] = hd * window; 25 | filter[(TAPS - 1)/2 - i] = filter[(TAPS - 1)/2 + i]; 26 | } 27 | hd = (omega2 - omega1) / M_PI; 28 | filter[(TAPS - 1)/2] = hd; 29 | } 30 | 31 | // End of Filter.c 32 | -------------------------------------------------------------------------------- /IIR_filter.c: -------------------------------------------------------------------------------- 1 | #include "beamer7.h" 2 | 3 | /* 4 | Function: Calculate HPF/LPF coefficients, max. 6th order 5 | Import: type,Freq,type, freq in High Pass Filter/Low Pass Filter 6 | type: 7 | 0: direct 8 | 1: Butterworth, 2nd order(-12dB/oct) 9 | 2: Butterworth, 3rd order(-18dB/oct) 10 | 3: Butterworth, 4th order(-24dB/oct) 11 | 4: Butterworth, 5th order(-30dB/oct) 12 | 5: Butterworth, 6th order(-36dB/oct) 13 | 6: Bessel, 2nd order 14 | 7: Bessel, 3nd order 15 | 8: Bessel, 4th order 16 | 9: Bessel, 5th order 17 | 10:Bessel, 6th order 18 | 11:Linkwitz-Riley, 2nd order 19 | 12:Linkwitz-Riley, 4th order 20 | 21 | */ 22 | #define IIR_SECOND_ORDER(param) \ 23 | if(1 == low_pass) rk = wo * wc; \ 24 | else rk = wo / wc; \ 25 | temp = 1.0 / (rk * (rk + pk) + 1.0); \ 26 | param[0] = rk * rk * temp; /*b2*/\ 27 | param[1] = 2.0 * low_pass * param[0]; /*b1*/\ 28 | param[2] = param[0]; /*b0*/\ 29 | param[3] = (rk * (rk - pk) + 1.0) * temp; /*a2*/\ 30 | param[4] = 2.0 * low_pass * (rk * rk - 1) * temp;/*a1*/ 31 | 32 | #define IIR_FIRST_ORDER(param) \ 33 | if(1 == low_pass) rk = wo * wc; \ 34 | else rk = wo / wc; \ 35 | temp = 1.0 / (rk + 1.0); \ 36 | param[2] = rk * temp; /*b0*/\ 37 | param[1] = low_pass * param[2]; /*b1*/\ 38 | param[4] = low_pass * (rk - 1.0) * temp;/*a1*/ 39 | 40 | float coeff[2][5]; 41 | 42 | void filter_design(int low_pass, int freq, int type) 43 | { 44 | float coeff_temp1[5]; //temp for exchanging.. 45 | float coeff_temp2[5]; //temp for exchanging.. 46 | // low_pass = 1 for low pass filter, low_pass = -1 for high pass filter 47 | float wc, wo, rk, temp, pk; 48 | 49 | //Direct refresh coeff_temp[], a0 = 1 50 | coeff_temp1[0] = 0;//b2 51 | coeff_temp1[1] = 0;//b1 52 | coeff_temp1[2] = 1;//b0 53 | coeff_temp1[3] = 0;//a2 54 | coeff_temp1[4] = 0;//a1 55 | 56 | coeff_temp2[0] = 0;//b2 57 | coeff_temp2[1] = 0;//b1 58 | coeff_temp2[2] = 1;//b0 59 | coeff_temp2[3] = 0;//a2 60 | coeff_temp2[4] = 0;//a1 61 | 62 | wc = tan(M_PI * freq / FS); 63 | switch (type) 64 | { 65 | case 1: //Butterworth, 2nd order(-12dB/oct) 66 | wo = 1.0; 67 | pk = 1.414213562373095; // 2 * sin(PI/4); 68 | IIR_SECOND_ORDER(coeff_temp1); 69 | break; 70 | 71 | case 2: //Butterworth, 3rd order(-18dB/oct) 72 | wo = 1.0; 73 | pk = 1.0; // 2 * sin(PI/6); 74 | IIR_SECOND_ORDER(coeff_temp1); 75 | 76 | IIR_FIRST_ORDER(coeff_temp2); 77 | break; 78 | 79 | case 3: //Butterworth, 4th order(-24dB/oct) 80 | wo = 1.0; 81 | pk = 0.76536686473018; // 2 * sin(PI/8); 82 | IIR_SECOND_ORDER(coeff_temp1); 83 | 84 | pk = 1.84775906502257351; // 2 * sin(3*PI/8); 85 | IIR_SECOND_ORDER(coeff_temp2); 86 | break; 87 | 88 | case 6: //Bessel, 2nd order(-12dB/oct) 89 | wo = 1.732050808; 90 | pk = 3.0 / wo; 91 | IIR_SECOND_ORDER(coeff_temp1); 92 | break; 93 | 94 | case 7: //Bessel, 3rd order(-18dB/oct) 95 | wo = 2.541541401; 96 | pk = 3.6778146454 / wo; 97 | IIR_SECOND_ORDER(coeff_temp1); 98 | 99 | wo = 2.3221853546; 100 | IIR_FIRST_ORDER(coeff_temp2); 101 | break; 102 | 103 | case 8: //Bessel, 4th order(-24dB/oct) 104 | wo = 3.389365793; 105 | pk = 4.2075787944 / wo; 106 | IIR_SECOND_ORDER(coeff_temp1); 107 | 108 | wo = 3.023264939; 109 | pk = 5.7924212056 / wo; 110 | IIR_SECOND_ORDER(coeff_temp2); 111 | break; 112 | 113 | case 11://Linkwitz-Riley, 2nd order(-12dB/oct) 114 | wo = 1.0; 115 | pk = 2.0; 116 | IIR_SECOND_ORDER(coeff_temp1); 117 | break; 118 | 119 | case 12://Linkwitz-Riley, 4th order(-24dB/oct) 120 | wo = 1; 121 | pk = 1.414213562373095; //2*sin(PI/4) 122 | IIR_SECOND_ORDER(coeff_temp1); 123 | 124 | IIR_SECOND_ORDER(coeff_temp2); 125 | break; 126 | 127 | default: 128 | break; 129 | } 130 | 131 | /* 132 | printf("b0, b1, b2 -- a0, a1, a2\n"); 133 | printf("p1=[%4.8f %4.8f %4.8f]; q1=[1.0 %4.8f %4.8f];\n", 134 | coeff_temp1[2], coeff_temp1[1], coeff_temp1[0], 135 | coeff_temp1[4], coeff_temp1[3]); 136 | printf("p2=[%4.8f %4.8f %4.8f]; q2=[1.0 %4.8f %4.8f];\n", 137 | coeff_temp2[2], coeff_temp2[1], coeff_temp2[0], 138 | coeff_temp2[4], coeff_temp2[3]); 139 | */ 140 | coeff[0][0] = coeff_temp1[0]; 141 | coeff[0][1] = coeff_temp1[1]; 142 | coeff[0][2] = coeff_temp1[2]; 143 | coeff[0][3] = coeff_temp1[3]; 144 | coeff[0][4] = coeff_temp1[4]; 145 | 146 | coeff[1][0] = coeff_temp2[0]; 147 | coeff[1][1] = coeff_temp2[1]; 148 | coeff[1][2] = coeff_temp2[2]; 149 | coeff[1][3] = coeff_temp2[3]; 150 | coeff[1][4] = coeff_temp2[4]; 151 | } 152 | /* 153 | y(n) = coeff2*x(n)+coeff1*x(n-1)+coeff0*x(n-2)-coeff4*y(n-1)-coeff3*y(n-2) 154 | 155 | 156 | p1=[0.95363986 -1.90727973 0.95363986]; q1=[1.0 -1.90064657 0.91391283]; 157 | p2=[0.89892012 -1.79784024 0.89892012]; q2=[1.0 -1.79158771 0.80409276]; 158 | 159 | z=poly(0, 'z'); 160 | r = [z^2 z 1]'; 161 | hz =( p1*r )/(q1*r)*(p2*r)/(q2*r); 162 | 163 | [hzm,fr]=frmag(hz,256); 164 | */ 165 | 166 | /*****************************************************************************/ 167 | /* DESCRIPTION */ 168 | /* Infinite Impulse Response (IIR) filters fourth order type I and type II */ 169 | /* Takes 3 numerator coefficients and 3 denominator coefficients. */ 170 | /* */ 171 | /*---------------------------------------------------------------------------*/ 172 | /* */ 173 | /* An second infinite impulse response (IIR) filter can be represented by */ 174 | /* the following equation: */ 175 | /* */ 176 | /* b0 + b1.z^-1 + b2.z^-2 */ 177 | /* H(z) = ---------------------- */ 178 | /* a0 + a1.z^-1 + a2.z^-2 */ 179 | /* */ 180 | /* where H(z) is the transfer function. a0 is always 1.000 */ 181 | /* */ 182 | /* To implement a fourth order filter, two of these stages are cascaded. */ 183 | /* */ 184 | /*****************************************************************************/ 185 | 186 | /* Numerator coefficients */ 187 | #define B0 2 188 | #define B1 1 189 | #define B2 0 190 | 191 | /* Denominator coefficients */ 192 | #define A0 0 193 | #define A1 4 194 | #define A2 3 195 | 196 | /*****************************************************************************/ 197 | /* IIR_direct_form_I() */ 198 | /*---------------------------------------------------------------------------*/ 199 | /* */ 200 | /* Forth order direct form I IIR filter implemented by cascading 2 second */ 201 | /* order filters. */ 202 | /* */ 203 | /* This implementation uses two buffers, one for x[n] and the other for y[n] */ 204 | /* */ 205 | /*****************************************************************************/ 206 | 207 | short IIR_direct_form_I(short input, struct FilterData *buf) 208 | { 209 | float temp, y0; 210 | /* x(n), x(n-1), x(n-2), y(n), y(n-1), y(n-2). Must be static */ 211 | unsigned int stages; 212 | 213 | temp = (float) input; /* Copy input to temp */ 214 | 215 | for (stages = 0; stages < 2; stages++) { 216 | y0 = coeff[stages][B0] * temp; /* B0 * x(n) */ 217 | y0 += coeff[stages][B1] * buf->x1[stages]; /* B1 * x(n-1) */ 218 | y0 += coeff[stages][B2] * buf->x2[stages]; /* B2 * x(n-2) */ 219 | 220 | buf->x2[stages] = buf->x1[stages]; /* x(n-2) = x(n-1) */ 221 | buf->x1[stages] = temp; /* x(n-1) = x(n) */ 222 | 223 | y0 -= coeff[stages][A1] * buf->y1[stages]; /* A1 * y(n-1) */ 224 | y0 -= coeff[stages][A2] * buf->y2[stages]; /* A2 * y(n-2) */ 225 | 226 | /* Shuffle values along one place for next time */ 227 | 228 | buf->y2[stages] = buf->y1[stages]; /* y(n-2) = y(n-1) */ 229 | buf->y1[stages] = y0; /* y(n-1) = y(n) */ 230 | 231 | /* temp is used as next section input*/ 232 | temp = y0; 233 | } 234 | 235 | return (short)temp; 236 | } 237 | /* 238 | int main(int argc, char *argv[]) 239 | { 240 | struct FilterData fl[4]; 241 | short inp, out; 242 | filter_design(-1, 300, 3); 243 | out = IIR_direct_form_I(inp, &fl[2]); 244 | } 245 | */ 246 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | CC = gcc 2 | channel7: channel7.c beamformer.c signal.c noise.c IIR_filter.c FIR_filter.c 3 | $(CC) -g -o $@ $^ -lm -DFLAT 4 | 5 | clean: 6 | $(RM) channel7 7 | 8 | .PHONY: clean 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # beamforming 2 | -------------------------------------------------------------------------------- /ana.m: -------------------------------------------------------------------------------- 1 | CHANNEL = 63; 2 | TAPS = 67; 3 | fl = fopen("filter.dat", "r"); 4 | val = fread(fl, [CHANNEL, TAPS], 'single'); 5 | fclose(fl); 6 | j = ones(1, CHANNEL); 7 | h0 = j*val; 8 | 9 | Fs = 16000; 10 | cs = 34000; 11 | Dx = 35.0; dx = 35.0/8; 12 | Dy = 25.0; dy = 25.0/6; 13 | 14 | freq = 100:100:8000; 15 | freq = 3000; 16 | w = 2*pi*(freq / Fs); 17 | 18 | plane = zeros(35, 35); 19 | for theta = -85:5:85 20 | for phi = -85:5:85 21 | output = w*0; 22 | for ch=0:CHANNEL-1 23 | jch = floor(ch/9); 24 | ich = ch - jch*9; 25 | 26 | t = (dx*ich*sin(pi*theta/180) + dy*jch*sin(pi*phi/180))/cs; 27 | dz = t*Fs; 28 | 29 | hi = val(ch+1,:); 30 | pz = exp(i*(0:-1:(1-TAPS))'*w); 31 | dl = exp(-i*dz*w); 32 | hm = dl.*(hi*pz); 33 | output = output + hm; 34 | end; 35 | plane(theta/5+18, phi/5+18) = abs(output); 36 | end; 37 | end; 38 | surface(-85:5:85, -85:5:85, plane); 39 | -------------------------------------------------------------------------------- /ana7.m: -------------------------------------------------------------------------------- 1 | CHANNEL = 7; 2 | TAPS = 67; 3 | alpha = [270 210 150 90 30 -30 0]; 4 | fl = fopen("filter.dat", "rb"); 5 | j = fread(fl, TAPS*CHANNEL, 'float32'); 6 | fclose(fl); 7 | val=[]; 8 | for i = 0:CHANNEL-1 9 | val = [val j(i*TAPS+1:(i+1)*TAPS)]; 10 | end; 11 | val = val'; 12 | j = ones(1, CHANNEL); 13 | h0 = j*val; 14 | 15 | Fs = 16000; 16 | cs = 34000; 17 | radius = 4.25; 18 | 19 | freq = 100:100:8000; 20 | freq = 500; 21 | w = 2*pi*(freq / Fs); 22 | 23 | plane = []; 24 | for theta = 0:5:360 25 | output = w*0; 26 | pz = exp(i*(0:-1:(1-TAPS))'*w); 27 | for ch=1:CHANNEL-1 28 | dz = Fs*radius/cs*cos((theta - alpha(ch))*pi/180.0); 29 | hi = val(ch,:); 30 | dl = exp(-i*dz*w); 31 | hm = dl.*(hi*pz); 32 | output = output + hm; 33 | end; 34 | hi = val(CHANNEL,:); 35 | hm = hi*pz; 36 | output = output + hm; 37 | plane = [plane abs(output)]; 38 | end; 39 | 40 | %plot(plane); 41 | polar((0:5:360)*pi/180.0, plane/360); 42 | -------------------------------------------------------------------------------- /beamer.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | File Name: filtertest.h 3 | Author: Fang Yuan 4 | Mail: yfang@nju.edu.cn 5 | Created Time: Thu 07 Jul 2016 03:39:58 PM CST 6 | ************************************************************************/ 7 | 8 | #ifndef _FILTERTEST_H 9 | #define _FILTERTEST_H 10 | 11 | #include 12 | 13 | #define LOWPASS 1 14 | #define HIGHPASS (-1) 15 | 16 | #define FS 16000 /* sampling rate */ 17 | 18 | #define CHANNEL (7*9) /* Microphone matrix */ 19 | #define TAPS 67 /* FIRs TAPS */ 20 | 21 | struct FilterData { 22 | float x1[2], x2[2]; 23 | float y1[2], y2[2]; 24 | } ; 25 | 26 | void filter_design(int low_pass, int freq, int type); 27 | short IIR_direct_form_I(short input, struct FilterData *buf); 28 | 29 | void Blackman_window(float f1, float f2, float *filter); 30 | 31 | void beamerinit(int argc); 32 | void beamformer(); 33 | 34 | short noise_add(int channel); 35 | 36 | #endif 37 | 38 | -------------------------------------------------------------------------------- /beamer7.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | File Name: beamer7.h 3 | Author: Fang Yuan 4 | Mail: yfang@nju.edu.cn 5 | Created Time: Fri 15 Jul 2016 04:31:23 PM CST 6 | ************************************************************************/ 7 | 8 | #ifndef _BEAMER7_H 9 | #define _BEAMER7_H 10 | 11 | #include 12 | 13 | #define LOWPASS 1 14 | #define HIGHPASS (-1) 15 | 16 | #define FS 16000 /* sampling rate */ 17 | 18 | #define CHANNEL 7 /* Microphone matrix */ 19 | #define TAPS 67 /* FIRs TAPS */ 20 | #define BUFL 50 /* buffer length in sampling data,both signal and noise */ 21 | #define SAMPLE (BUFL/2) /* current sample to be processed */ 22 | #define CS 34000.0 /* speed of sound, cm/s*/ 23 | 24 | struct FilterData { 25 | float x1[2], x2[2]; 26 | float y1[2], y2[2]; 27 | } ; 28 | 29 | void filter_design(int low_pass, int freq, int type); 30 | short IIR_direct_form_I(short input, struct FilterData *buf); 31 | 32 | void Blackman_window(float f1, float f2, float *filter); 33 | 34 | void beamerinit(int argc); 35 | void beamformer(); 36 | float beamformer_filter(); 37 | void savefile(int); 38 | 39 | short noise_add(int channel); 40 | void noise_gen(); 41 | void filter_init(); 42 | 43 | void init_delays(); 44 | float signal_resample(int channel); 45 | void update_signal(); 46 | 47 | #endif // _BEAMER7_H 48 | 49 | -------------------------------------------------------------------------------- /beamformer.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | File Name: beamformer.c 3 | Author: Fang Yuan 4 | Mail: yfang@nju.edu.cn 5 | Created Time: Sun 17 Jul 2016 12:59:23 PM CST 6 | ************************************************************************/ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "beamer7.h" 13 | 14 | float mu_beamer = 2.e-11; // 阵列自适应步长 15 | float beamer[CHANNEL][TAPS]; // 滤波器系数(双字对齐) 16 | short input[CHANNEL][TAPS]; // 阵列输入序列,带训练噪声源叠加 17 | short mic[CHANNEL][TAPS]; // 阵列输入序列,不带训练噪声源 18 | short sum_input[TAPS]; // signal tap sum 19 | 20 | float tap_sum[TAPS]; // filter tap sum 21 | float desired[TAPS]; 22 | 23 | float r_chn = 1.0/CHANNEL; 24 | 25 | void beamerinit(int argc) 26 | { 27 | FILE *fp; 28 | int t, ch; 29 | pid_t pid; 30 | 31 | #ifdef FLAT 32 | for(t = TAPS; --t;) 33 | desired[t] = 0.0; 34 | desired[0] = 1.0; 35 | #else 36 | Blackman_window(300.0, FS/2.0, desired); 37 | #endif 38 | for(t = TAPS; t--;) { 39 | desired[t] *= r_chn; 40 | for(ch = CHANNEL; ch--;) { 41 | beamer[ch][t] = desired[t]; 42 | } 43 | } 44 | 45 | if(argc == 2) { 46 | fp = fopen("filter.dat", "rb"); 47 | fread(beamer, 4, CHANNEL*TAPS, fp); 48 | fclose(fp); 49 | } 50 | } 51 | 52 | // y=sum(x .* w); 53 | // mu_y= mu*y; 54 | // for j=1:J 55 | // w(:,j)=w(:,j) - mu_y*(x(:,j)-sum(x(:,j))/K) + (f(j)-sum(w(:,j)))/K; 56 | // end; 57 | 58 | void beamformer() 59 | { 60 | int ch, t, n, i; 61 | int tmp; 62 | float res; 63 | static int cnt = 0; 64 | static float energy = 0; 65 | static float ei = 0; 66 | 67 | res = 0; 68 | for(t = TAPS; t--;) { 69 | tap_sum[t] = 0.0; 70 | tmp = 0; 71 | for(ch = CHANNEL; ch--;) { 72 | tap_sum[t] += beamer[ch][t]; 73 | tmp += input[ch][t]; 74 | res += input[ch][t] * beamer[ch][t]; 75 | } 76 | tap_sum[t] *= r_chn; 77 | sum_input[t] = (short)(tmp * r_chn); 78 | } 79 | 80 | energy += res*res; 81 | ei += (float)input[0][0]*input[0][0]; 82 | cnt++; 83 | if(cnt == 5120) { 84 | printf("%8.1f %8.1f\n", sqrt(energy), sqrt(ei)); 85 | energy = 0; 86 | ei = 0; 87 | cnt = 0; 88 | } 89 | 90 | res = mu_beamer * res; 91 | 92 | for (ch = CHANNEL; ch--;) { 93 | for(t = TAPS; --t;) { 94 | beamer[ch][t] += res*(sum_input[t]-input[ch][t]) - 95 | (tap_sum[t] - desired[t]); 96 | input[ch][t] = input[ch][t-1]; 97 | mic[ch][t] = mic[ch][t-1]; 98 | } 99 | 100 | beamer[ch][0] += res*(sum_input[0]-input[ch][0]) - 101 | (tap_sum[0] - desired[0]); 102 | mic[ch][0] = signal_resample(ch); 103 | input[ch][0] = mic[ch][0] + noise_add(ch); 104 | } 105 | } 106 | 107 | /* only filter microphone signal */ 108 | float beamformer_filter() 109 | { 110 | int ch, t; 111 | float res = 0; 112 | 113 | for(t = TAPS; t--;) { 114 | for(ch = CHANNEL; ch--;) { 115 | res += mic[ch][t] * beamer[ch][t]; 116 | } 117 | } 118 | 119 | return res; 120 | } 121 | 122 | void savefile(int num) 123 | { 124 | FILE *fp; 125 | fp = fopen("filter.dat", "wb"); 126 | fwrite(beamer, 4, CHANNEL*TAPS, fp); 127 | fclose(fp); 128 | 129 | exit(0); 130 | } 131 | 132 | -------------------------------------------------------------------------------- /channel7.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yfang1644/beamforming/5d9354005ae9ceed96a4eb4d57342c33d6a33681/channel7.c -------------------------------------------------------------------------------- /filter.dat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yfang1644/beamforming/5d9354005ae9ceed96a4eb4d57342c33d6a33681/filter.dat -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yfang1644/beamforming/5d9354005ae9ceed96a4eb4d57342c33d6a33681/main.c -------------------------------------------------------------------------------- /noise.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | File Name: noise.c 3 | Author: Fang Yuan 4 | Mail: yfang@nju.edu.cn 5 | Created Time: Sun 17 Jul 2016 01:24:28 PM CST 6 | ************************************************************************/ 7 | 8 | #include 9 | #include 10 | #include "beamer7.h" 11 | 12 | short noise[4][BUFL]; // 4路噪声源，每路保留100点(左右两边最大延迟) 13 | struct FilterData fl[4]; 14 | extern float radius; 15 | extern float alpha[]; /* microphone positions defined in signal.c */ 16 | extern float delay[]; /* delay */ 17 | float noisedir[4] = { // 噪声源方位, theta 18 | 120, 19 | -50, 20 | 50, 21 | -45, 22 | }; 23 | 24 | void filter_init() 25 | { 26 | int ch, sec; 27 | for(ch = 0; ch < 4; ch++) { 28 | for(sec = 0; sec < 2; sec++) { 29 | fl[ch].x1[sec] = fl[ch].x2[sec] = 0.0; 30 | fl[ch].y1[sec] = fl[ch].y2[sec] = 0.0; 31 | } 32 | } 33 | 34 | filter_design(HIGHPASS, 300, 3); 35 | } 36 | 37 | void noise_gen() 38 | { 39 | int i, j; 40 | int Ltmp; 41 | short tmp; 42 | static int cnt = 0; 43 | 44 | for(i = 0; i < 4; i++) { 45 | for(j = BUFL; --j;) 46 | noise[i][j] = noise[i][j-1]; 47 | Ltmp = rand() -(RAND_MAX/2); 48 | tmp = (short)(Ltmp >> 18); 49 | // noise[i][0] = IIR_direct_form_I(tmp, &fl[i]); 50 | // noise[i][0] = 5000*sin(2*M_PI*(i+1)*329/FS*cnt) 51 | // + 3000*cos(2*M_PI*(i+1)*763/FS*cnt); 52 | noise[i][0] = tmp; 53 | } 54 | cnt++; 55 | } 56 | 57 | short noise_add(int ch) 58 | { 59 | int n, nt; 60 | float dnt, x1, x2, t; 61 | float sample = 0; 62 | 63 | if(ch == CHANNEL-1) { 64 | for(n = 0; n < 4; n++) { 65 | sample += noise[n][SAMPLE]; 66 | } 67 | } else { 68 | for(n = 0; n < 2; n++) { 69 | dnt = FS * radius*cos((alpha[ch]-noisedir[n])*M_PI/180.)/CS; 70 | dnt = delay[ch] - dnt; 71 | nt = (int)dnt; 72 | dnt -= (float)nt; 73 | x1 = noise[n][SAMPLE+nt]; 74 | x2 = noise[n][SAMPLE+nt+1]; 75 | sample += x1 + (x2 - x1)*dnt; 76 | } 77 | } 78 | return (short)sample; 79 | } 80 | 81 | -------------------------------------------------------------------------------- /signal.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | File Name: signal.c 3 | Author: Fang Yuan 4 | Mail: yfang@nju.edu.cn 5 | Created Time: Sun 17 Jul 2016 01:36:52 PM CST 6 | ************************************************************************/ 7 | 8 | #include 9 | #include 10 | #include 11 | #include "beamer7.h" 12 | 13 | float sig_dir = -108.0; // 信号方向 14 | 15 | // microphone positions in degree. R=4.25cm 16 | float radius = 4.25; /* cm */ 17 | float alpha[CHANNEL] = {270, 210, 150, 90, 30, -30, 0}; 18 | float delay[CHANNEL]; // 每路延迟，单位为采样点，浮点数 19 | 20 | short source[BUFL][CHANNEL]; // 信号源 21 | 22 | FILE *fp_signal; 23 | void init_delays() 24 | { 25 | int ch; 26 | printf("ok\n"); 27 | for(ch = CHANNEL; --ch; ) 28 | delay[ch] = FS * radius*cos((alpha[ch]-sig_dir)*M_PI/180.)/CS; 29 | 30 | fp_signal = fopen("/home/fang/Downloads/zero_degree.wav", "rb"); 31 | fseek(fp_signal, 44, SEEK_SET); 32 | } 33 | 34 | void update_signal() 35 | { 36 | int chn, tap; 37 | for(chn = CHANNEL; chn--; ) 38 | for(tap = BUFL; --tap;) 39 | source[tap][chn] = source[tap-1][chn]; 40 | 41 | chn = fread(source, 2, CHANNEL, fp_signal); 42 | if(chn < CHANNEL) { 43 | fseek(fp_signal, 44, SEEK_SET); 44 | printf("#########\n"); 45 | } 46 | } 47 | 48 | float signal_resample(int ch) 49 | { 50 | int n; 51 | float t, dnt, x1, x2; 52 | float sample = 0; 53 | 54 | if(ch == CHANNEL-1) 55 | sample = source[ch][SAMPLE]; 56 | else { 57 | n = (int)delay[ch]; 58 | dnt = delay[ch] - (float)n; 59 | x1 = source[ch][SAMPLE+n]; 60 | x2 = source[ch][SAMPLE+n+1]; 61 | sample = x1 + (x2 - x1)*dnt; 62 | } 63 | 64 | return sample; 65 | } 66 | 67 | -------------------------------------------------------------------------------- /tags: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yfang1644/beamforming/5d9354005ae9ceed96a4eb4d57342c33d6a33681/tags --------------------------------------------------------------------------------