fft-spectra.c

linux下的FFT 频谱分析

#include "fft-spectra.h"
#include <fftw3.h>
#include <math.h>
#include <fcntl.h>

int esd_rate = ESD_DEFAULT_RATE;
int ndata_in = 2*4096; 
int bps = 2;
int channels = 1;
int do_not_play = 0;
int signed_data = 1;

int *sound_data_end=NULL;
char *sound_data_buffer=NULL;
int sound_data_size;
int *msg_from_gui=NULL;
int *msg_from_reader=NULL;
int esd_stream_rec=-1;
int esd_stream_play=-1;
int file_stream=-1;

static double *fft_data_in = NULL;
static fftw_complex *fft_data_out = NULL;
static fftw_plan fft_plan;
static int fft_nsamples=0;


void init_fft(int nsamples)
{
    ASSERT(nsamples);

    fprintf(stderr,"Initializing fft...");
    fft_nsamples = nsamples;
    fft_data_in = (double*) fftw_malloc(sizeof(double) * nsamples);
    fft_data_out = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * nsamples/2+1);
    fft_plan = fftw_plan_dft_r2c_1d(nsamples,fft_data_in,fft_data_out,FFTW_MEASURE);
    //fft_plan = fftw_plan_dft_r2c_1d(nsamples,fft_data_in,fft_data_out,FFTW_ESTIMATE);
    fprintf(stderr," done.\n");

    ASSERT( fft_data_in );
    ASSERT( fft_data_out );
}

void finish_fft()
{
    fftw_destroy_plan(fft_plan);
    fftw_free(fft_data_in);
    fftw_free(fft_data_out);
}


void open_sound_file_stream()
{
    esd_format_t format;

    format = ESD_STREAM|ESD_PLAY;
    if ( bps==1 )
        format |= ESD_BITS8;
    else if ( bps==2 )
        format |= ESD_BITS16;
    else
        exit_nicely("Bps %d not supported.\n", bps);

    if ( channels==1 )
        format |= ESD_MONO;
    else if ( channels==2 )
        format |= ESD_STEREO;
    else
        exit_nicely("Channels %d not supported.\n", channels);

    fprintf(stderr,"opening socket, format=0x%08x at %d Hz\n", format, esd_rate);

    esd_stream_play =  esd_play_stream_fallback(format, esd_rate, NULL, "fft-spectra");
    if (esd_stream_play<=0)
        exit_nicely("Could not open esd stream for playing.\n");
}



void open_sound_record_stream()
{
    esd_format_t format;

    format = ESD_STREAM|ESD_RECORD;
    if ( bps==1 )
        format |= ESD_BITS8;
    else if ( bps==2 )
        format |= ESD_BITS16;
    else
        exit_nicely("Bps %d not supported.\n", bps);

    if ( channels==1 )
        format |= ESD_MONO;
    else if ( channels==2 )
        format |= ESD_STEREO;
    else
        exit_nicely("Channels %d not supported.\n", channels);

    esd_stream_rec =  esd_record_stream_fallback(format, esd_rate, NULL, "fft-spectra");
    if (esd_stream_rec<=0)
        exit_nicely("Could not open esd stream for recording: try again!\n");
}


void close_sound_record_stream()
{
    /*    fprintf(stderr,"closing the record stream...\n"); */
    close(esd_stream_rec);
    esd_stream_rec = -1;
}



/* This routine mimics behaviour of sound_data_client_record, except that
    it does not actually record anything, it just pretends that it reads
    data: It moves pointer to a mmap()ed file and playes it, if requested.
*/
void sound_data_client_play_file()
{
    int nto_write,n;
    float delay;

    if (!do_not_play)
    {
        open_sound_file_stream();
        *msg_from_reader = MSG_OK;

        *sound_data_end = 0;
        while ( 1 )
        {
            if ( *msg_from_gui == MSG_EXIT_REQUEST ) 
            {
                *msg_from_reader = MSG_EXIT_REQUEST;
                return;
            }
            if ( *msg_from_gui == MSG_PAUSE_REQUEST ) 
            {
                usleep(250000);
                continue;
            }

            nto_write = ESD_BUF_SIZE;
            if ( nto_write + *sound_data_end > sound_data_size )
                nto_write = sound_data_size - *sound_data_end;

            n=write(esd_stream_play,sound_data_buffer+(*sound_data_end),nto_write);
            if ( n<0 )
                exit_nicely("error playing esd sample..\n");

            /* Wait a while before updating the position in the stream -
                it is confusing to see the plot BEFORE it is played.
                Better is to show it few ms after it has been played.
            */
            usleep(25000); 

            n += (*sound_data_end);
            if ( n>=sound_data_size )
                n = 0;
            (*sound_data_end) = n;
        }
    }
    else
    {
        delay = 1e6*ndata_in/esd_rate;
        *sound_data_end = 0;
        *msg_from_reader = MSG_OK;
        while (1)
        {
            if ( *msg_from_gui == MSG_EXIT_REQUEST ) 
            {
                *msg_from_reader = MSG_EXIT_REQUEST;
                return;
            }
            if ( *msg_from_gui == MSG_PAUSE_REQUEST ) 
            {
                usleep(250000);
                continue;
            }
            
            usleep(delay);
            n = *sound_data_end + ndata_in*bps*channels;
            if ( n>=sound_data_size )
                n = 0;
            *sound_data_end = n;
        }
    }
}



/* 
    sound_data_end  - Points to an integer, which indicates end of the buffered data,
        or, to be more precise, (*sound_data_end)-1 is index of the last written number.
        It should be interpreted as an index to the array sound_data_buffer: 0..size-1.
*/
void sound_data_client_record()
{
    int nto_read,n;

    open_sound_record_stream();
    *sound_data_end = 0;

    /* Fill the buffer: read so many times untill it is full.. */
    while ( *sound_data_end<sound_data_size )
    {
        if ( *msg_from_gui == MSG_ECHO_REQUEST )
        {
            fprintf(stderr,"reader: echo\n");
            *msg_from_gui = MSG_OK;
        }

        if ( *msg_from_gui == MSG_EXIT_REQUEST ) 
        {
            *msg_from_reader = MSG_EXIT_REQUEST;
            return;
        }
        if ( *msg_from_gui == MSG_PAUSE_REQUEST )
        {
            usleep(250000);
            continue;
        }

        nto_read = ESD_BUF_SIZE;
        if ( nto_read + *sound_data_end > sound_data_size )
            nto_read = sound_data_size - *sound_data_end;

        n = read(esd_stream_rec,sound_data_buffer+(*sound_data_end),nto_read);
        (*sound_data_end) += n;
        if ( n<=0 )
        {
            perror("read");
            exit_nicely("Error reading from the sound socket.\n");
        }
    }

    if ( *sound_data_end!=sound_data_size ) exit_nicely("FIXME!");
    *sound_data_end = 0;
    *msg_from_reader = MSG_OK;

    while (1)
    {
        if ( *msg_from_gui == MSG_ECHO_REQUEST )
        {
            fprintf(stderr,"reader: echo\n");
            *msg_from_gui = MSG_OK;
        }

        if ( *msg_from_gui == MSG_PAUSE_REQUEST )
        {
            usleep(250000);
            continue;
        }

        nto_read = ESD_BUF_SIZE;
        if ( nto_read + *sound_data_end > sound_data_size )
            nto_read = sound_data_size - *sound_data_end;

        if ( nto_read )
        {
            n = read(esd_stream_rec,sound_data_buffer+(*sound_data_end),nto_read);
            (*sound_data_end) += n;
        }
        else
        {
            n = read(esd_stream_rec,sound_data_buffer,ESD_BUF_SIZE);
            *sound_data_end = n;
        }
        if ( n<=0 )
        {
            perror("read");
            exit_nicely("Error reading from the sound socket.\n");
        }
 
        if ( *sound_data_end>=sound_data_size )
            *sound_data_end = 0;

        if ( *msg_from_gui == MSG_EXIT_REQUEST )
        {
            *msg_from_reader = MSG_EXIT_REQUEST;
            break;
        }
    }

    close(esd_stream_rec);
    esd_stream_rec = -1;
}


/* Read audio data of the request length and run FFT on them. If logscale is
    set to nonzero value, logarithm of the FFT data will be returned.
    Minimal and maximal value from the range (max_from,max_to) of the returned
    data will be saved in the pointers min,max.
*/
void get_fft_sample(int nsamples, float *data_buffer, int max_from,int max_to, float *min,float *max,int logscale)
{
    int i,n,reading_from;
    float val;

    int isample,ibyte,ichannel;
    int num;
    char *buff = (char *) &num;
    char *ptr_data;

#if 0
    ASSERT( fft_data_in );
    ASSERT( fft_data_out );
    ASSERT( data_buffer );
    ASSERT( nsamples );
#endif

    if ( *msg_from_reader == MSG_EXIT_REQUEST ) 
        exit_nicely(NULL);

    reading_from = (*sound_data_end) - nsamples*bps*channels;
    if ( reading_from<0 ) 
        reading_from += sound_data_size;

    ptr_data = (char*) (sound_data_buffer+reading_from);
    for (isample=0; isample<nsamples; isample++)
    {
        num = 0;
        for (ibyte=0; ibyte<bps; ibyte++)
        {
            ASSERT( ibyte<sizeof(int) );
            buff[ibyte] = *ptr_data;
            ptr_data++;

            if ( ptr_data>=sound_data_buffer+sound_data_size )
                ptr_data = sound_data_buffer;
        }

        if ( signed_data )
        {
            if ( num&(1<<(8*bps-1)) )
            {
                while (ibyte<sizeof(num))
                    buff[ibyte++] = 255;

                num = -(~num)-1;
            }
        }

        fft_data_in[isample] = num;

        for (ichannel=1; ichannel<channels; ichannel++)
            ptr_data += bps;
    }

    fftw_execute(fft_plan);

    /* FIXME: Proc dava prvni frekvence tak vysokou hodnotu?
        data[0] = sqrt(fft_data_out[0][0]*fft_data_out[0][0] + fft_data_out[0][1]*fft_data_out[0][1]);
        max = data[0];
    */
    *max = 0;
    *min = 1e40;
    
    n = nsamples/2+1;
    for (i=1; i<n; i++)
    {
        val = sqrt(fft_data_out[i][0]*fft_data_out[i][0] + fft_data_out[i][1]*fft_data_out[i][1]);
        if (logscale) val=log(val);
        if ( i>=max_from && i<=max_to )
        {
            if ( *max<val ) *max=val;
            if ( *min>val ) *min=val;
        }
        data_buffer[i] = val;
    }
    data_buffer[0] = data_buffer[1];
}


/* Read the requested amount of audio data and report the minimal and
    maximal value which occured. pos_max is index of the maximum value
    in the buffer.
*/
void get_data_sample(int nsamples, int *buffer, int *min, int *max,int *pos_max)
{
    int reading_from;
    int isample,ibyte,ichannel;
    int num;
    char *buff = (char *) &num;
    char *ptr_data;

    *min = INT_MAX;
    *max = -INT_MAX;

    if ( *msg_from_reader == MSG_EXIT_REQUEST ) 
        exit_nicely(NULL);

    reading_from = (*sound_data_end) - nsamples*bps*channels;
    if ( reading_from<0 ) 
        reading_from += sound_data_size;

    ptr_data = (char*) (sound_data_buffer+reading_from);
    for (isample=0; isample<nsamples; isample++)
    {
        num = 0;
        for (ibyte=0; ibyte<bps; ibyte++)
        {
            ASSERT( ibyte<sizeof(int) );
            buff[ibyte] = *ptr_data;
            ptr_data++;

            if ( ptr_data>=sound_data_buffer+sound_data_size )
                ptr_data = sound_data_buffer;
        }

        if ( signed_data )
        {
            if ( num&(1<<(8*bps-1)) )
            {
                while (ibyte<sizeof(num))
                    buff[ibyte++] = 255;

                num = -(~num)-1;
            }
        }

        buffer[isample] = num;

        if ( *min>num ) *min=num;
        if ( *max<num ) 
        {
            *max=num;
            *pos_max = isample;
        }

        for (ichannel=1; ichannel<channels; ichannel++)
            ptr_data += bps;
    }
}


/* The same as get_data_sample, but the routine will also run FFT to
    find out frequency with maximal intensity. The index of the corresponding
    data sample will be returned.
    The pointers min,max will be filled with the least and the largest value (for
    propper scaling) and pos_max will contain index of the largest value.
*/
int get_data_sample_fft_max(int nsamples, int *sound_data, int *min, int *max, int *pos_max)
{
    int imax,isample,i,n;
    float maxval,val;
    
    get_data_sample(nsamples,sound_data,min,max,pos_max);

    for (isample=0; isample<nsamples; isample++)
        fft_data_in[isample] = sound_data[isample];

    fftw_execute(fft_plan);

    maxval = 0;
    n = nsamples/2+1;
    imax = n-1;
    for (i=1; i<n; i++)
    {
        val = fabs(fft_data_out[i][0]*fft_data_out[i][0] + fft_data_out[i][1]*fft_data_out[i][1]);
        // TODO: dynamic range must be given!
        if ( maxval<val && i>50 )
        {
            imax = i;
            maxval  = val;
        }
    }

    return imax;
}