Subversion Repositories shark

// FFT Part

/*

FFTPlay 1.0

-----------

This application reads data from the audio microphone and then put it

to the screen in a graphical oscilloscope-like form (both standard Hartik

SB driver and Alsa driver can be used).

The application also calculate a FFT on the latest values, and displays

the power spectrum in tho ways, one like an equalizator, and the other in a

2D form.

A resolution of 1024x768, 64K colors is used.

The task set is composed by these tasks and functions:

Self buffering functions (SB only)

----------------------------------

this function takes the samples read by the mic and then makes a window

with the last WINDATA_NSAMPLES samples. The window is then put in

the CAB windata.

Task raw and task mixer (ALSA only)

-----------------------------------

These tasks are used with the Alsa driver; because it doesn't support the

self-buffering mode, we have to do a forever cycle in witch we have to read

all the data. Then the data are managed like the self-buffering functions of

the SB driver or sent to the mixer task (with a STREAM mailbox) which makes

the window.

(This approach is not good for realtime...)

Task wave

---------

This task read the last window and then put it on the screen in a wave form

on the top of the screen.

The task's period is set to 40 ms (25 fps).

Task fft

--------

This task read the last window and then it computes the FFT.

With the FFT data it computes the power spectrum, whitch is sent to the

CAB pwrdata.

The task's period is set to 10 ms (good for the 2D story task).

This task is the only Hard Task.

Task equ

--------

This task read the last power spectrum and displays it in a graphical

form, like a hi-fi equalizator.

The Histograms can be white or coloured like the equ2D task (see EQU_SHADE)

The task's period is set to 40 ms (25 fps).

Task equ2D

----------

This task read the lasf power spectrum and displays it in a graphical

one-line form. Each pixel is a power coefficient, and its colour shade

from black (no power) to red (high power) passing through green and blue.

The task display the last EQU2D_WIDTH power lines.

The task's period is set to 10 ms (good for the 2D story task).

****************************************************************************

TASK LOAD

****************************************************************************

                     period              wcet

task             tick     (ms)      us          %

--------------------------------------------------------

sound driver    24->3    12->1.5   200     0.016->0.133

wave              80       40     11500       0.2875

fft               20       10      3000       0.3000

equ               80       40      7000       0.1750

equ2D             20       10       500       0.0500

                                          -------------

                                              0.812 (last 4)

*/

#include "demo.h"

//#include <ll/ll.h>

//#include <kernel/types.h>

#include <kernel/model.h>

#include <kernel/func.h>

#include <modules/cabs.h>

//#include <string.h>

//#include <stdlib.h>

#include <semaphore.h>

//#include <drivers/keyb.h>

//#include <drivers/crtwin.h>

#include <drivers/glib.h>

#include <drivers/sound.h>

#include <ports/rfftw.h>

/* CAB ports... */

CAB cab_windata;   /* a window on the last WINDATA_DIM samples */

CAB cab_pwrdata;   /* the last power spectrum                  */

/* for the cab_windata */

typedef struct {

  int start;

  SAMPLE sample[WINDATA_NSAMPLES];

} window;

/* for the cab_pwrdata */

typedef struct {

  fftw_real p[PWR_NSAMPLES];

} power;

// win is global... because is used by raw_infun...

window win;

/*

   This is the self-buffering function: read the samples and put their mean

   value in a CAB

*/

int raw_infun(void *b)

{

    int i;

    char *w;

    SAMPLE *audiobuf = (SAMPLE *)b;

    for (i=0; i<rawdata_nsamples/2; i++) {

      win.sample[win.start] = audiobuf[i];

      win.start = (win.start+1) % WINDATA_NSAMPLES;

    }

    w = cab_reserve(cab_windata);

    memcpy(w, &win, sizeof(window));

    cab_putmes(cab_windata,w);

  #if defined(NO_GRX)

    cprintf("X"); //"XXX%d\n",win.sample[win.start]);

  #endif

    return 0;

}

void init_rawdata()

{

  int i;

  char *w;

  win.start = 0;

  for (i=0; i<WINDATA_NSAMPLES; i++)

    win.sample[i] = 0;

  w = cab_reserve(cab_windata);

  memcpy(w, &win, sizeof(window));

  cab_putmes(cab_windata,w);

}

TASK wave_task()

{

  window *p;

  int x,y;

  int s;

  while(1)

  {

    p = (window *)cab_getmes(cab_windata);

    /* let's print the wave */

    mutex_lock(&mutex);

    for(x = WAVE_X, s = p->start;

        x < WAVE_X+WAVE_NSAMPLES;

        x++, s = (s+1)%WINDATA_NSAMPLES )

    {

      y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE;

      SHORT_CRITICAL_SECTIONS(x);

      grx_plot(x,y,white);

    }

    mutex_unlock(&mutex);

    task_endcycle();

    /* let's erase the wave */

    mutex_lock(&mutex);

    for(x = WAVE_X, s = p->start;

        x < WAVE_X+WAVE_NSAMPLES;

        x++, s = (s+1)%WINDATA_NSAMPLES )

    {

      y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE;

      SHORT_CRITICAL_SECTIONS(x);

      grx_plot(x,y,black);

    }

    mutex_unlock(&mutex);

    cab_unget(cab_windata,(char *)p);

  }

}

rfftw_plan plan;

void fft_close(void *arg)

{

  rfftw_destroy_plan(plan);

}

TASK fft_task()

{

  fftw_real in[FFT_NSAMPLES], out[FFT_NSAMPLES];

  power power_spectrum;

  #if defined(NO_GRX)

    fftw_real max = 0.0;

  #endif

  char *m;

  int k, i;

  window *p;

  plan = rfftw_create_plan(FFT_NSAMPLES, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE);

  sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT);

  while(1)

  {

    /* Let's prepare the intput FFT data */

    p = (window *)cab_getmes(cab_windata);

    for (k = 0, i = p->start;

        k < FFT_NSAMPLES;

        k++, i = (i+1)%WINDATA_NSAMPLES)

      in[k] = p->sample[i]/FFT_SCALE;

    cab_unget(cab_windata,(char *)p);

    /* zero-padding if needed */

    for (k=WINDATA_NSAMPLES; k < FFT_NSAMPLES; k++)

      in[k] = 0.0;

    rfftw_one(plan, in, out);

    /* power spectrum computation */

    power_spectrum.p[0] = out[0]*out[0];  /* DC component */

    for (k = 1; k < PWR_NSAMPLES; ++k)  /* (k < N/2 rounded up) */

          power_spectrum.p[k] = out[k]*out[k] + out[FFT_NSAMPLES-k]*out[FFT_NSAMPLES-k];

    if (FFT_NSAMPLES % 2 == 0) /* N is even */

          power_spectrum.p[FFT_NSAMPLES/2] = out[FFT_NSAMPLES/2]*out[FFT_NSAMPLES/2];  /* Nyquist freq. */

    m = cab_reserve(cab_pwrdata);

    memcpy(m, &power_spectrum, sizeof(power));

    cab_putmes(cab_pwrdata,m);

    #if defined(NO_GRX)

    max = 0.0;

    for (k=0; k<PWR_NSAMPLES; k++)

      if (power_spectrum.p[k] > max)

        max = power_spectrum.p[k];

    //cprintf("%f %f\n",max,(max / EQU_SCALE) );

    #endif

    task_endcycle();

  }

}

/* structure is like the wave task... */

TASK equ_task()

{

  power *p;

  int x[PWR_NSAMPLES];

  int y;

  int s;

  int r,g,b;

  while(1)

  {

    p = (power *)cab_getmes(cab_pwrdata);

    /* print the lines */

    mutex_lock(&mutex);

    for(y = EQU_Y, s = 0;

        s < EQU_NSAMPLES;

        y++, s++ )

    {

      x[s] = (int)(p->p[s] / EQU_SCALE);

      if (x[s] > EQU_HEIGHT)

        x[s] = EQU_HEIGHT;

      x[s] = EQU_X - x[s];

      #if defined(EQU_SHADE)

       /* like the task equ2d... */

       r = (int)(p->p[s] / EQU2D_SCALE);

       if (r > EQU2D_CLIP)

         r = EQU2D_CLIP;

            if (r< 64)  g = r * 4;

       else if (r<128)  g = (128-r) * 4;

       else             g = 0;

            if (r<128)  b = 0;

       else if (r<192)  b = (r-128) * 4;

       else             b = (256-r) * 4;

       SHORT_CRITICAL_SECTIONS(y);

       grx_line(EQU_X,y,x[s],y,rgb16(r,g,b));

      #else

       SHORT_CRITICAL_SECTIONS(y);

       grx_line(EQU_X,y,x[s],y,white);

      #endif

    }

    mutex_unlock(&mutex);

    task_endcycle();

    /* erase the lines... */

    mutex_lock(&mutex);

    for(y = EQU_Y, s = 0;

        s < EQU_NSAMPLES;

        y++, s++ )

    {

      SHORT_CRITICAL_SECTIONS(y);

      grx_line(EQU_X,y,x[s],y,black);

    }

    mutex_unlock(&mutex);

    cab_unget(cab_pwrdata,(char *)p);

  }

}

TASK equ2d_task()

{

  power *p;

  int pwrint;

  int x = 0;

  int y,s;

  int r,g,b;

  while(1)

  {

    p = (power *)cab_getmes(cab_pwrdata);

    /* print the line */

    mutex_lock(&mutex);

    for(y = EQU2D_Y, s = 0;

        s < EQU2D_NSAMPLES;

        y++, s++ )

    {

      pwrint = (int)(p->p[s] / EQU2D_SCALE);

      if (pwrint > EQU2D_CLIP)

        pwrint = EQU2D_CLIP;

      r = pwrint;

           if (pwrint< 64)  g = pwrint * 4;

      else if (pwrint<128)  g = (128-pwrint) * 4;

      else                  g = 0;

           if (pwrint<128)  b = 0;

      else if (pwrint<192)  b = (pwrint-128) * 4;

      else                  b = (256-pwrint) * 4;

      SHORT_CRITICAL_SECTIONS(y);

      grx_plot(EQU2D_X+x,y,rgb16(r,g,b));

    }

    x = (x+1) % EQU2D_WIDTH;

    grx_line(EQU2D_X+x,EQU2D_Y,EQU2D_X+x,EQU2D_Y+EQU2D_NSAMPLES-1,white);

    mutex_unlock(&mutex);

    cab_unget(cab_pwrdata,(char *)p);

    task_endcycle();

  }

}

void init_fftplay(int freq)

{

    SOFT_TASK_MODEL m3, m4, m5, m6;

    PID p3,p4,p5,p6;

    cab_windata = cab_create("windata", sizeof(window), 5);

    cab_pwrdata = cab_create("pwr", sizeof(power), 5);

    /* Init the sound lib */

    sound_init((rawdata_nsamples * sizeof(SAMPLE)), NULL);

    sound_info();

    /* Init the data used by the raw_infun */

    init_rawdata();

    /* Start the self-buffering sampling operation */

    sound_setfun(raw_infun, (int (*)(void *))-1);

    sound_sample(NULL, freq, 0, DMA_OP | PCM16 | MYFUN, NULL);

    soft_task_default_model(m3);

    soft_task_def_level(m3,1);

    soft_task_def_period(m3, PERIOD_WAVE);

    soft_task_def_met(m3, WCET_WAVE);

    soft_task_def_ctrl_jet(m3);

    soft_task_def_group(m3, 1);

    p3 = task_create("wave", wave_task, &m3, NULL);

    if (p3 == -1) {

        grx_close();

        perror("FFTPlay: Could not create task <wave>\n");

        ll_abort(54);

        sys_end();

    }

    soft_task_default_model(m4);

    soft_task_def_level(m4,1);

    soft_task_def_period(m4, PERIOD_FFT);

    soft_task_def_met(m4, WCET_FFT);

    soft_task_def_group(m4, 1);

    soft_task_def_stack(m4,32*1024);

    soft_task_def_usemath(m4);

    soft_task_def_ctrl_jet(m4);

    p4 = task_create("fft", fft_task, &m4, NULL);

    if (p4 == -1) {

        grx_close();

        perror("FFTPlay: Could not create task <fft>\n");

        ll_abort(54);

        sys_end();

    }

    soft_task_default_model(m5);

    soft_task_def_level(m5,1);

    soft_task_def_period(m5, PERIOD_EQU);

    soft_task_def_met(m5, WCET_EQU);

    soft_task_def_group(m5, 1);

    soft_task_def_stack(m5,32*1024);

    soft_task_def_usemath(m5);

    soft_task_def_ctrl_jet(m5);

    p5 = task_create("equ", equ_task, &m5, NULL);

    if (p5 == -1) {

        grx_close();

        perror("FFTPlay: Could not create task <equ>\n");

        ll_abort(54);

        perror("FFTPlay: Could not create task <equ>\n");

        sys_end();

    }

    soft_task_default_model(m6);

    soft_task_def_level(m6,1);

    soft_task_def_period(m6, PERIOD_EQU2D);

    soft_task_def_met(m6, WCET_EQU2D);

    soft_task_def_group(m6, 1);

    soft_task_def_stack(m6,32*1024);

    soft_task_def_usemath(m6);

    soft_task_def_ctrl_jet(m6);

    p6 = task_create("equ2D", equ2d_task, &m6, NULL);

    if (p6 == -1) {

        grx_close();

        perror("FFTPlay: Could not create task <equ2d>\n");

        ll_abort(54);

        perror("FFTPlay: Could not create task <equ2D>\n");

        sys_end();

    }

}

void scenario_fftplay(int f)

{

  int i,y;

  char s[50];

  grx_line(0,WAVE_Y-WAVE_HEIGHT-1,383,WAVE_Y-WAVE_HEIGHT-1,red);

  grx_line(0,WAVE_Y+WAVE_HEIGHT+1,383,WAVE_Y+WAVE_HEIGHT+1,red);

  grx_line(0,EQU_Y-11            ,383,EQU_Y-11            ,red);

  /* lines near the frequencies */

  grx_line(EQU_X  +1,EQU_Y,EQU_X  +1,EQU_Y+EQU_NSAMPLES-1,red);

  grx_line(EQU2D_X-1,EQU_Y,EQU2D_X-1,EQU_Y+EQU_NSAMPLES-1,red);

  for (i=0; i<SCENARIO_NLABEL; i++)

  {

    y = (i*EQU_NSAMPLES)/(SCENARIO_NLABEL-1);

    if (i == SCENARIO_NLABEL-1) y--;

    grx_line(EQU_X  +1,EQU_Y+y,EQU_X  +4,EQU_Y+y,red);

    grx_line(EQU2D_X-1,EQU_Y+y,EQU2D_X-4,EQU_Y+y,red);

    itoa((i*f)/(SCENARIO_NLABEL-1),s);

    grx_text(s,EQU_X+20,EQU_Y+y-8,white,black);

  }

  grx_text("Power Spectrum"      , 0, EQU_Y-21, rgb16(0,0,255), black);

  grx_text("Power Spectrum Story", EQU2D_X+EQU2D_WIDTH-160, EQU_Y-21, rgb16(0,0,255), black);

  grx_text("Waveform"            , 0, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black);

}

void compute_params(int *freq,WORD *nsamp)

{

  if (*freq< 2000)

  {

    cprintf("WARNING: frequency less than 2000Hz\n  ---> frequency set to 2000Hz\n");

    *freq = 2000;

  }

  if (*freq<= 8000) { *nsamp = 64; return; } //128

  if (*freq<=16000) { *nsamp = 64; return; } //256

  if (*freq<=24000) { *nsamp = 64; return; } //512

  if (*freq>48000)

  {

    cprintf("WARNING: frequency greather than 48000Hz\n  ---> frequency set to 48000Hz\n");

    *freq = 48000;

  }

  if (*freq<=48000) { *nsamp = 64; return; } //1024

}