119 lines
3.9 KiB
C
119 lines
3.9 KiB
C
/* peak-detection.c
|
|
* atsa: ATS analysis implementation
|
|
* Oscar Pablo Di Liscia / Pete Moss / Juan Pampin
|
|
*/
|
|
|
|
#include "atsa.h"
|
|
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
/* private function prototypes */
|
|
void parabolic_interp(double alpha, double beta, double gamma, double *offset, double *height);
|
|
double phase_interp(double PeakPhase, double OtherPhase, double offset);
|
|
void to_polar(ATS_FFT *ats_fft, double *mags, double *phase, int N, double norm);
|
|
|
|
/* peak_detection
|
|
* ==============
|
|
* detects peaks in a ATS_FFT block
|
|
* returns pointer to an array of detected peaks.
|
|
* ats_fft: pointer to ATS_FFT structure
|
|
* lowest_bin: lowest fft bin to start detection
|
|
* highest_bin: highest fft bin to end detection
|
|
* lowest_mag: lowest magnitude to detect peaks
|
|
* norm: analysis window norm
|
|
* peaks_size: pointer to size of the returned peaks array
|
|
*/
|
|
ATS_PEAK *peak_detection(ATS_FFT *ats_fft, int lowest_bin, int highest_bin, double lowest_mag, double norm, int *peaks_size)
|
|
{
|
|
int k, N = (highest_bin ? highest_bin : ats_fft->size/2);
|
|
int first_bin = (lowest_bin ? ((lowest_bin>2)?lowest_bin:2) : 2);
|
|
double fft_mag = ((double)ats_fft->rate/ats_fft->size), *fftmags, *fftphase;
|
|
double right_bin, left_bin, central_bin, offset;
|
|
ATS_PEAK ats_peak, *peaks = NULL;
|
|
|
|
lowest_mag = (double)db2amp(lowest_mag);
|
|
|
|
// init peak
|
|
ats_peak.amp = 0.0;
|
|
ats_peak.frq = 0.0;
|
|
ats_peak.pha = 0.0;
|
|
ats_peak.smr = 0.0;
|
|
|
|
fftmags = (double *)malloc(N * sizeof(double));
|
|
fftphase = (double *)malloc(N * sizeof(double));
|
|
/* convert spectrum to polar coordinates */
|
|
to_polar(ats_fft, fftmags, fftphase, N, norm);
|
|
central_bin = fftmags[first_bin - 2];
|
|
right_bin = fftmags[first_bin - 1];
|
|
/* peak detection */
|
|
for (k=first_bin; k<N; k++) {
|
|
left_bin = central_bin;
|
|
central_bin = right_bin;
|
|
right_bin = fftmags[k];
|
|
if((central_bin > (double)lowest_mag) && (central_bin > right_bin) && (central_bin > left_bin)) {
|
|
parabolic_interp(left_bin, central_bin, right_bin, &offset, &ats_peak.amp);
|
|
ats_peak.frq = fft_mag * ((k - 1) + offset);
|
|
ats_peak.pha = (offset < 0.0) ? phase_interp(fftphase[k-2], fftphase[k-1], fabs(offset)) : phase_interp(fftphase[k-1], fftphase[k], offset);
|
|
ats_peak.track = -1;
|
|
/* push peak into peaks list */
|
|
peaks = push_peak(&ats_peak, peaks, peaks_size);
|
|
}
|
|
}
|
|
/* free up fftmags and fftphase */
|
|
free(fftmags);
|
|
free(fftphase);
|
|
return(peaks);
|
|
}
|
|
|
|
/* to_polar
|
|
* ========
|
|
* rectangular to polar conversion
|
|
* values are also scaled by window norm
|
|
* and stored into separate arrays of
|
|
* magnitudes and phases.
|
|
* ats_fft: pointer to ATS_FFT structure
|
|
* mags: pointer to array of magnitudes
|
|
* phase: pointer to array of phases
|
|
* N: highest bin in fft data array
|
|
* norm: window norm used to scale magnitudes
|
|
*/
|
|
void to_polar(ATS_FFT *ats_fft, double *mags, double *phase, int N, double norm)
|
|
{
|
|
int k;
|
|
double x, y;
|
|
|
|
for(k=0; k<N; k++) {
|
|
x = ats_fft->fdr[k]; y = ats_fft->fdi[k];
|
|
mags[k] = norm * sqrt(x*x + y*y);
|
|
// wrong method for phase computation!!!
|
|
// must use atan2(-y, x) JP
|
|
// phase[k] = ((x==0.0 && y==0.0) ? 0.0 : atan(-y/x));
|
|
phase[k] = ((x==0.0 && y==0.0) ? 0.0 : atan2(-y, x));
|
|
}
|
|
}
|
|
|
|
|
|
/* parabolic_interp
|
|
* ================
|
|
* parabolic peak interpolation
|
|
*/
|
|
void parabolic_interp(double alpha, double beta, double gamma, double *offset, double *height)
|
|
{
|
|
double dbAlpha = amp2db(alpha), dbBeta = amp2db(beta), dbGamma = amp2db(gamma);
|
|
*offset = .5 * ((dbAlpha - dbGamma) / (dbAlpha - 2*dbBeta + dbGamma));
|
|
*height = db2amp(dbBeta - ((dbAlpha - dbGamma) * .25 * *offset));
|
|
}
|
|
|
|
/* phase_interp
|
|
* ============
|
|
* phase interpolation
|
|
*/
|
|
double phase_interp(double PeakPhase, double RightPhase, double offset)
|
|
{
|
|
if ((PeakPhase - RightPhase) > PI*1.5) RightPhase += TWOPI;
|
|
else if ((RightPhase - PeakPhase) > PI*1.5) PeakPhase += TWOPI;
|
|
return ((RightPhase - PeakPhase) * offset + PeakPhase);
|
|
}
|
|
|