| Line 202... |
Line 202... |
| 202 |
snd_pcm_sw_params_t *swparams;
|
202 |
snd_pcm_sw_params_t *swparams;
|
| 203 |
|
203 |
|
| 204 |
long int *correlationl, *correlationr;
|
204 |
long int *correlationl, *correlationr;
|
| 205 |
int *L_signal, *R_signal;
|
205 |
int *L_signal, *R_signal;
|
| 206 |
short *chirp, *signal;
|
206 |
short *chirp, *signal;
|
| 207 |
float *chirp_spect, *echo_spect;
|
207 |
float *chirp_spect, *lecho_spect, *recho_spect;
|
| 208 |
unsigned int i,j,m,n;
|
208 |
unsigned int i,j,m,n;
|
| 209 |
unsigned int delayl[10],delayr[10]; //store delay of signifed correlation
|
209 |
unsigned int delayl[10],delayr[10]; //store delay of signifed correlation
|
| 210 |
long int l,r; // store correlation at strict time
|
210 |
long int l,r; // store correlation at strict time
|
| 211 |
double df; //frequency resolution
|
211 |
double df; //frequency resolution
|
| 212 |
unsigned int frequency_bins; // number of output frequency bins
|
212 |
unsigned int frequency_bins; // number of output frequency bins
|
| Line 276... |
Line 276... |
| 276 |
chirp_size = linear_windowed_chirp(chirp);
|
276 |
chirp_size = linear_windowed_chirp(chirp);
|
| 277 |
|
277 |
|
| 278 |
frequency_bins = chirp_size / 2 + 1;
|
278 |
frequency_bins = chirp_size / 2 + 1;
|
| 279 |
df = (double) rate / (double) chirp_size;
|
279 |
df = (double) rate / (double) chirp_size;
|
| 280 |
chirp_spect = malloc(frequency_bins * sizeof(float));
|
280 |
chirp_spect = malloc(frequency_bins * sizeof(float));
|
| - |
|
281 |
lecho_spect = malloc(frequency_bins * sizeof(float));
|
| 281 |
echo_spect = malloc(frequency_bins * sizeof(float));
|
282 |
recho_spect = malloc(frequency_bins * sizeof(float));
|
| 282 |
|
283 |
|
| 283 |
inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT
|
284 |
inchirp = fftw_malloc(sizeof(double) * chirp_size); // allocate input array for FFT
|
| 284 |
outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);
|
285 |
outchirp = fftw_malloc(sizeof(fftw_complex) * frequency_bins);
|
| 285 |
|
286 |
|
| 286 |
fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);
|
287 |
fft_plan_chirp = fftw_plan_dft_r2c_1d(chirp_size, inchirp, outchirp, FFTW_ESTIMATE);
|
| Line 375... |
Line 376... |
| 375 |
}
|
376 |
}
|
| 376 |
}
|
377 |
}
|
| 377 |
|
378 |
|
| 378 |
for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];
|
379 |
for(i=delayl[1]; i < delayl[1] + chirp_size; i++) inchirp[i-delayl[1]] = L_signal[i];
|
| 379 |
fftw_execute(fft_plan_chirp);
|
380 |
fftw_execute(fft_plan_chirp);
|
| 380 |
for(i=0; i < frequency_bins; i++) echo_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
|
381 |
for(i=0; i < frequency_bins; i++) lecho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
|
| - |
|
382 |
|
| - |
|
383 |
for(i=delayr[1]; i < delayr[1] + chirp_size; i++) inchirp[i-delayr[1]] = R_signal[i];
|
| - |
|
384 |
fftw_execute(fft_plan_chirp);
|
| - |
|
385 |
for(i=0; i < frequency_bins; i++) recho_spect[i] = sqrt(outchirp[i][0] * outchirp[i][0] + outchirp[i][1] * outchirp[i][1]);
|
| 381 |
|
386 |
|
| 382 |
printf("Writing output files\n");
|
387 |
printf("Writing output files\n");
|
| 383 |
out=fopen("/tmp/sonar.txt","w");
|
388 |
out=fopen("/tmp/sonar.txt","w");
|
| 384 |
for (i=0; i <= (period_size - 1); i++)
|
389 |
for (i=0; i <= (period_size - 1); i++)
|
| 385 |
{
|
390 |
{
|
| Line 393... |
Line 398... |
| 393 |
fprintf(out,"%6d %6d\n", i, chirp[i]);
|
398 |
fprintf(out,"%6d %6d\n", i, chirp[i]);
|
| 394 |
}
|
399 |
}
|
| 395 |
fclose(out);
|
400 |
fclose(out);
|
| 396 |
|
401 |
|
| 397 |
out=fopen("/tmp/echo.txt","w");
|
402 |
out=fopen("/tmp/echo.txt","w");
|
| 398 |
for(i=delayl[1]; i < delayl[1] + chirp_size; i++) fprintf(out,"%6d %6d\n", i-delayl[1], L_signal[i]);
|
403 |
for(i=0; i < chirp_size; i++) fprintf(out,"%6d %6d %6d\n", i, L_signal[i + delayl[1]], R_signal[i + delayr[1]]);
|
| 399 |
fclose(out);
|
404 |
fclose(out);
|
| 400 |
|
405 |
|
| 401 |
out=fopen("/tmp/spektra.txt","w");
|
406 |
out=fopen("/tmp/spektra.txt","w");
|
| 402 |
for (i=0; i < frequency_bins; i++)
|
407 |
for (i=0; i < frequency_bins; i++)
|
| 403 |
{
|
408 |
{
|
| 404 |
fprintf(out,"%4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], echo_spect[i]);
|
409 |
fprintf(out,"%4.3f %4.3f %4.3f %4.3f\n", (i+0.5) * df, chirp_spect[i], lecho_spect[i], recho_spect[i]);
|
| 405 |
}
|
410 |
}
|
| 406 |
fclose(out);
|
411 |
fclose(out);
|
| 407 |
|
412 |
|
| 408 |
printf("Echo zacina na: %d vzorku.\n", delayl[1]);
|
413 |
printf("Echo zacina na: %d vzorku.\n", delayl[1]);
|
| 409 |
printf("Casove na: %f s\n", ((float)delayl[1]/rate));
|
414 |
printf("Casove na: %f s\n", ((float)delayl[1]/rate));
|