Subversion Repositories shark

Rev

Blame | Last modification | View Log | RSS feed

// 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
}