註：跑這個程式前請先將 Respeaker 輸入 Ch7、Ch8 的音量調為0。
已知問題：當沒有偵測到訊號時會無線放大雜訊，將加入 SNR 抑制這個狀況 Real-time Beamforming with SNR Detect Using Portaudio, FFTW And gsl_lib
#Adjust Respeaker input volume.(set Ch7 Ch8 to 0)
alsamixer
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_linalg.h>
#include "portaudio.h"
#define REAL(z,i) ((z)[2*(i)])
#define IMAG(z,i) ((z)[2*(i)+1])
#define DEG2RAD(x) (x*M_PI/180)
#define FRAME_BLOCK_LEN 256
#define SAMPLING_RATE 44100
#define INPUT_CHANNEL 8
#define USED_CH 6
#define OUTPUT_CHANNEL 1
#define LDA 343./1000.
#define Radii 0.0463
PaStream *audioStream;
gsl_complex double2complex(double re, double im){
gsl_complex b;
b.dat[0]=re;
b.dat[1]=im;
return b;
}
void PrintMat(gsl_matrix_complex *m, char info[]){
printf("Print Matrix %s: \\n",info);
for(int i=0; i<m->size1;i++){
for(int j=0;j<m->size2;j++){
printf("%6.2e+%.2fi\\t", gsl_matrix_complex_get(m,i,j).dat[0],gsl_matrix_complex_get(m,i,j).dat[1]);
}
printf("\\n");
}
}
void autocorr(gsl_matrix_complex *Rxx, gsl_matrix_complex *m){
gsl_matrix_complex *mH=gsl_matrix_complex_alloc(m->size2, m->size1);
gsl_matrix_complex *Imat=gsl_matrix_complex_alloc(m->size1, m->size1);
gsl_matrix_complex_set_identity(Imat);
gsl_blas_zgemm(CblasConjTrans,CblasNoTrans,double2complex(1.,0.),m,Imat,double2complex(0.,0.),mH);
gsl_blas_zgemm (CblasNoTrans, CblasNoTrans,
double2complex(1./m->size2,0.), m, mH,
double2complex(0.,0.), Rxx);
}
void invMat(gsl_matrix_complex *matrix,gsl_matrix_complex *inv){
gsl_permutation *p = gsl_permutation_alloc(matrix->size1);
int s;
gsl_linalg_complex_LU_decomp(matrix, p, &s);
gsl_linalg_complex_LU_invert(matrix, p, inv);
gsl_permutation_free(p);
}
gsl_complex eulers_formula(double x){ //e^(xi) = cos(x) + i*sin(x)
gsl_complex a;
GSL_SET_COMPLEX(&a, cos(x), sin(x));
return a;
}
int audio_callback(const void *inputBuffer,
void *outputBuffer,
unsigned long framesPerBuffer,
const PaStreamCallbackTimeInfo* timeInfo,
PaStreamCallbackFlags statusFlags,
void *userData){
float *in = (float*) inputBuffer, *out = (float*)outputBuffer;
double y_arr_f_to_t_domain[2*FRAME_BLOCK_LEN];
double *y_arr_p = y_arr_f_to_t_domain;
static double phase = 0;
float in_unsort[INPUT_CHANNEL][FRAME_BLOCK_LEN];
float in_sort[USED_CH][FRAME_BLOCK_LEN];
double fft_data[USED_CH][2*FRAME_BLOCK_LEN];
double ifft_data[USED_CH][2*FRAME_BLOCK_LEN];
int printmat=0;
gsl_matrix_complex *fft_data_mat=gsl_matrix_complex_alloc(USED_CH, FRAME_BLOCK_LEN);
gsl_matrix_complex *in_sort_mat=gsl_matrix_complex_alloc(USED_CH, FRAME_BLOCK_LEN);
//-------Sorting the input signal-------
/*
Portaudio reads 8 input. These 8 input are ordered as circular but randomly.
For example, the input array could be:
[ch1 ch2 ch3 ch4 ch5 ch6 ch7 ch8]^T,
[ch4 ch5 ch6 ch7 ch8 ch1 ch2 ch3]^T...
we want to sort them and get rid of ch7 and ch8, like [ch1 ch2 ch3 ch4 ch5 ch6]^T.
*/
int ch_tag=0;
int zero_count=0;
// write unsorted input into an array
for (int i=0;i<INPUT_CHANNEL;i++){
float is_zero=0.;
for (int j=0;j<FRAME_BLOCK_LEN;j++){
in_unsort[i][j]=in[INPUT_CHANNEL*j+i];
is_zero+=in[INPUT_CHANNEL*j+i];
}
if(is_zero==0.){
// ch7 and ch8 would be zero, skip them
zero_count++;
continue;
}
if(zero_count>=2){
// after ch8 would be ch1~ch n, write them into sorted array
for(int j=0;j<FRAME_BLOCK_LEN;j++){
in_sort[ch_tag][j] = in_unsort[i][j];
REAL(fft_data[ch_tag],j) = in_sort[ch_tag][j];
IMAG(fft_data[ch_tag],j) = 0.0;
}
ch_tag++;
}
}
// before ch7 would be ch n+1~ch6, write them into sorted array
for(int i=ch_tag;i<USED_CH;i++){
for(int j=0;j<FRAME_BLOCK_LEN;j++){
in_sort[i][j] = in_unsort[i-ch_tag][j];
REAL(fft_data[i],j) = in_sort[i][j];
IMAG(fft_data[i],j) = 0.0;
gsl_matrix_complex_set (in_sort_mat, i, j, double2complex(in_sort[i][j], 0.));
}
}
//--------------
//fft
for(int i=0;i<USED_CH;i++)
gsl_fft_complex_radix2_forward(fft_data[i], 1, FRAME_BLOCK_LEN);
for(int k=0;k<FRAME_BLOCK_LEN;k++){
gsl_matrix_complex_set (fft_data_mat, i, k, double2complex(REAL(fft_data[i],k), IMAG(fft_data[i],k)));
}
}
//Processing in f domain
//-------Beamforming-------
gsl_matrix_complex *Rxx=gsl_matrix_complex_alloc(fft_data_mat->size1, fft_data_mat->size1);
gsl_matrix_complex *invR=gsl_matrix_complex_alloc(fft_data_mat->size1, fft_data_mat->size1);
gsl_matrix_complex *AA=gsl_matrix_complex_alloc(1,USED_CH);
gsl_matrix_complex *tempww=gsl_matrix_complex_alloc(1,USED_CH);
gsl_matrix_complex *ww=gsl_matrix_complex_alloc(1,1);
gsl_matrix_complex *w=gsl_matrix_complex_alloc(USED_CH,1);
gsl_matrix_complex *y=gsl_matrix_complex_alloc(1, fft_data_mat->size2);
float doa=0.;
autocorr(Rxx,fft_data_mat);
invMat(Rxx,invR);
for(int i=0;i<USED_CH;i++){
float tempaa = sin(M_PI/2)*cos(DEG2RAD(doa)-2*i*M_PI)/USED_CH;
gsl_matrix_complex_set (AA, 0, i, eulers_formula(-1*2*M_PI*Radii*tempaa/LDA));
}
gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, double2complex(1.,0.), AA, invR, double2complex(0.,0.), tempww);
gsl_blas_zgemm(CblasNoTrans, CblasConjTrans, double2complex(1.,0.), tempww, AA, double2complex(0.,0.), ww);
gsl_blas_zgemm(CblasNoTrans, CblasConjTrans, gsl_matrix_complex_get(ww,0,0), invR, AA, double2complex(0.,0.), w);
//--------------
//-------y=w*x-------
gsl_blas_zgemm(CblasConjTrans, CblasNoTrans, double2complex(1.,0.), w, in_sort_mat, double2complex(0.,0.), y);
for(int i=0;i<FRAME_BLOCK_LEN;i++){
REAL(y_arr_f_to_t_domain,i)=gsl_matrix_complex_get(y,0,i).dat[0];
IMAG(y_arr_f_to_t_domain,i)=gsl_matrix_complex_get(y,0,i).dat[1];
}
//gsl_fft_complex_radix2_inverse(y_arr_f_to_t_domain, 1, FRAME_BLOCK_LEN);
for(int i=0;i<FRAME_BLOCK_LEN;i++){
*out++ = *y_arr_p++;
*y_arr_p++;
}
return paContinue;
}
void init_stuff(){
int i,id;
const PaDeviceInfo *info;
const PaHostApiInfo *hostapi;
PaStreamParameters outputParameters, inputParameters;
Pa_Initialize(); // initialize portaudio
id = Pa_GetDefaultOutputDevice();
info = Pa_GetDeviceInfo(id);
hostapi = Pa_GetHostApiInfo(info->hostApi);
printf("output device [%s] %s\\n",hostapi->name, info->name);
outputParameters.device = id;
outputParameters.channelCount = OUTPUT_CHANNEL;
outputParameters.sampleFormat = paFloat32;
outputParameters.suggestedLatency = 0;
outputParameters.hostApiSpecificStreamInfo = NULL; /*no specific info*/
sleep(0.5);
id = Pa_GetDefaultInputDevice();
info = Pa_GetDeviceInfo(id); /* get chosen device information struct */
hostapi = Pa_GetHostApiInfo(info->hostApi); /* get host API struct */
printf("input device [%s] %s\\n",
hostapi->name, info->name);
inputParameters.device = id; /* chosen device id */
inputParameters.channelCount = INPUT_CHANNEL; /* stereo input */
inputParameters.sampleFormat = paFloat32; /* 32 bit float input */
inputParameters.suggestedLatency = 0;
inputParameters.hostApiSpecificStreamInfo = NULL;
Pa_OpenStream(
&audioStream,
&inputParameters, // output parameters
&outputParameters, // input parameters
SAMPLING_RATE, // set sampling rate
FRAME_BLOCK_LEN, // set frames per buffer
paClipOff, // set no clip
audio_callback, // callback function
NULL ); // provide no data for the callback
Pa_StartStream(audioStream); /* start the callback mechanism */
printf("running. . . press space bar and enter to exit\\n");
printf("Latency: %f\\n",info->defaultLowInputLatency);
}
void terminate_stuff(){
Pa_StopStream( audioStream ); /* stop the callback mechanism */
Pa_CloseStream( audioStream ); /* destroy the audio stream object */
Pa_Terminate(); /* terminate portaudio */
}
int main(){
init_stuff();
while(getchar() != ' ') Pa_Sleep(100);
terminate_stuff();
return 0;
}
#Compile and link
gcc callback_in_C.c -lportaudio -lgsl -lgslcblas -lm