WebSVN - shark - Rev 1085 - /demos/branches/pj/thdemo/fftplay.c

/* Project: HARTIK 3.0 */
/* Description: Hard Real TIme Kernel for 386 & higher machines */
/* Author: Paolo Gai <pgai@rsk.it> */
/* Advanced Linux Sound Architecture (ALSA) */
/* Copyright (c) by Jaroslav Kysela <perex@jcu.cz> */
/* Luca Abeni */
/* FFTW by M. Frigo and S. G. Johnson */

/* Date: 08/09/1999 */

/* File: fftplay.c */
/* Revision: 1.00 (Kernel 0.1.4; Library 0.0.9; Util 0.0.4) */

/*

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

/* now the load constants... */

#define WCET_WAVE 11500
#define WCET_FFT 3000
#define WCET_EQU 500
#define WCET_EQU2D 500

#define PERIOD_WAVE 40000
#define PERIOD_FFT 10000
#define PERIOD_EQU 40000
#define PERIOD_EQU2D 10000

/* define if shorts critical sections wanted */
#define SHORT_CRITICAL_SECTIONS(x) \
if (!((x)%64)) \
{ \
sem_post(&mutex); \
sem_wait(&mutex); \
}

/* define if you want NRT or SOFT... */
#define TASK_TYPE SOFT
//#define TASK_TYPE NRT

/* Only 4 Debug... */
/*#define NO_GRX */

/* Samples are 16-bit signed integers */
typedef short SAMPLE;
#define MAX_SAMPLE 32768

/* Informations about the sampling rate and buffers */
WORD rawdata_nsamples;
WORD rawdata_buffer_size;
WORD rawdata_freq;

/* Numbers of samples of the sample window */
#define WINDATA_NSAMPLES 1024

/* task WAVE */
/* the point (wave_x,wave_y) is on the center left of the area... */
#define WAVE_NSAMPLES 1024
#define WAVE_X 0
#define WAVE_Y 130
#define WAVE_HEIGHT 80

/* task FFT */
#define FFT_NSAMPLES WINDATA_NSAMPLES
#define PWR_NSAMPLES (FFT_NSAMPLES/2+1)

/* task EQU */
/* the point (equ_x, equ_y) is the top right corner */
#define EQU_NSAMPLES PWR_NSAMPLES
#define EQU_X 170
#define EQU_Y 255
#define EQU_HEIGHT 170
#define EQU_SHADE

/* task EQU2D */
/* the point (equ2d_x, equ2d_y) is the top left corner */
#define EQU2D_NSAMPLES EQU_NSAMPLES
#define EQU2D_X 255
#define EQU2D_Y EQU_Y
#define EQU2D_WIDTH 768
#define EQU2D_CLIP 255

/* scenario */
#define SCENARIO_NLABEL 16

/* Scale factors */
#define FFT_SCALE (16384.0)
#define EQU_SCALE (32.0)
#define EQU2D_SCALE (8.0)
//#define EQU_SCALE (64.0)
//#define EQU2D_SCALE (16.0)

/* 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;

/* graphic mutex... */
sem_t mutex;

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

/* useful colors... */
int white;
int black;
int red;

static void version( void )
{
cprintf( "Hartik FFT Play 1.0\n" );
cprintf( "-----------------------\n" );
cprintf( "by Paolo Gai 1999\n" );
cprintf( " <pj@hartik.sssup.it>\n" );
cprintf( "-----------------------\n" );
}

void reverse(char s[])
{
int c, i, j;

for (i = 0, j = strlen(s)-1; i<j; i++, j--)
{
c = s[i];
s[i] = s[j];
s[j] = c;
}
}

char * itoa(int n, char *s)
{
int i, sign;

if ((sign = n) < 0)
n = -n;

i = 0;

do
{
s[i++] = n % 10 + '0';
} while ((n /= 10) > 0);

if (sign < 0)
s[i++] = '-';

s[i] = 0;

reverse(s);

return s;
}

/*
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 */
sem_wait(&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);
}
sem_post(&mutex);

task_endcycle();

/* let's erase the wave */
sem_wait(&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);
}
sem_post(&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 */
sem_wait(&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
}
sem_post(&mutex);

task_endcycle();

/* erase the lines... */
sem_wait(&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);
}
sem_post(&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 */
sem_wait(&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,white);

sem_post(&mutex);

cab_unget(cab_pwrdata,(char *)p);

task_endcycle();
}
}

TASK prova_task()
{
window *p;

while(1)
{
p = (window *)cab_getmes(cab_windata);
cprintf("%d %d %d\t", p->start /*sample[0]*/,p->sample[1],p->sample[2]);
cab_unget(cab_windata,(char *)p);

task_endcycle();
}
}

void scenario(int f)
{
int i,y;
char s[6];

grx_line(0,WAVE_Y-WAVE_HEIGHT-1,1023,WAVE_Y-WAVE_HEIGHT-1,red);
grx_line(0,WAVE_Y+WAVE_HEIGHT+1,1023,WAVE_Y+WAVE_HEIGHT+1,red);
grx_line(0,EQU_Y-11 ,1023,EQU_Y-11 ,red);

/* lines near the frequencies */
grx_line(EQU_X +1,EQU_Y,EQU_X +1,EQU_Y+EQU_NSAMPLES,red);
grx_line(EQU2D_X-1,EQU_Y,EQU2D_X-1,EQU_Y+EQU_NSAMPLES,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 +10,EQU_Y+y,red);
grx_line(EQU2D_X-1,EQU_Y+y,EQU2D_X-10,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("FFTPlay 1.0 - by Paolo Gai 1999 <pj@hartik.sssup.it>", 0,8, rgb16(0,255,0), black );
grx_text("...press ENTER key to exit..." , 0,24, rgb16(0,255,0), black );

grx_text("FFT Power Spectrum", 0 , EQU_Y-21, rgb16(0,0,255), black);
grx_text("FFT Power Story", EQU2D_X+16, 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, WORD *per)
{
if (*freq< 2000)
{
cprintf("WARNING: frequency less than 2000Hz\n ---> frequency set to 2000Hz\n");
*freq = 2000;
}
if (*freq<= 8000) { *nsamp = 128; *per = 10; return; }
if (*freq<=16000) { *nsamp = 256; *per = 10; return; }
if (*freq<=24000) { *nsamp = 512; *per = 10; return; }
if (*freq>48000)
{
cprintf("WARNING: frequency greather than 48000Hz\n ---> frequency set to 48000Hz\n");
*freq = 48000;
}
if (*freq<=48000) { *nsamp = 1024;*per = 10; return; }
}

void my_close(void *arg)
{
grx_close();
sys_status(3);
}

void endfun(KEY_EVT *k)
{
cprintf("Ctrl-Brk pressed! Ending...\n");
sys_end();
}

int main(int argc, char **argv)
{
int modenum;

int f;

/* irq period... */
WORD period;

KEY_EVT k;

SOFT_TASK_MODEL m3, m4, m5, m6;

PID p3,p4,p5,p6;

version();

if (argc == 1)
{
cprintf("type x fftplay <freq>");
return 0;
}

f = atoi(argv[1]);
compute_params(&f,&rawdata_nsamples,&period);

keyb_set_map(itaMap);
k.flag = CNTR_BIT;
k.scan = KEY_C;
k.ascii = 'c';
keyb_hook(k,endfun);
k.flag = CNTL_BIT;
k.scan = KEY_C;
k.ascii = 'c';
keyb_hook(k,endfun);

cab_windata = cab_create("windata", sizeof(window), 4);
cab_pwrdata = cab_create("pwr", sizeof(power), 4);

/* 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, f, 0, DMA_OP | PCM16 | MYFUN, NULL);

cprintf("Press Enter...");
while (keyb_getchar() != 13);

sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT);

#if !defined(NO_GRX)
grx_init();
modenum = grx_getmode(1024, 768, 16);
grx_setmode(modenum);

/* init the graphic mutex */
sem_init(&mutex, 0, 1);

/* useful colors ... */
white = rgb16(255,255,255);
black = rgb16(0,0,0);
red = rgb16(255,0,0);

scenario(f/2);

#endif

#if !defined(NO_GRX)
soft_task_default_model(m3);
soft_task_def_period(m3, PERIOD_WAVE);
soft_task_def_met(m3, WCET_WAVE);
soft_task_def_group(m3, 1);
p3 = task_create("wave", wave_task, &m3, NULL);
if (p3 == -1) {
perror("FFTPlay: Could not create task <wave>\n");
sys_end();
}
#endif

soft_task_default_model(m4);
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);
p4 = task_create("fft", fft_task, &m4, NULL);
if (p4 == -1) {
perror("FFTPlay: Could not create task <fft>\n");
sys_end();
}

#if !defined(NO_GRX)
soft_task_default_model(m5);
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);
p5 = task_create("equ", equ_task, &m5, NULL);
if (p5 == -1) {
perror("FFTPlay: Could not create task <equ>\n");
sys_end();
}
#endif

#if !defined(NO_GRX)
soft_task_default_model(m6);
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);
p6 = task_create("equ2D", equ2d_task, &m5, NULL);
if (p6 == -1) {
perror("FFTPlay: Could not create task <equ2D>\n");
sys_end();
}
#else
/* Start the prova task */
//task_def_wcet(m6,1000);
//task_activate(task_create("prova",prova_task,TASK_TYPE,PERIODIC,200,&m6));
#endif

group_activate(1);

/* Wait until the user get bored */
while (keyb_getchar() != 13);

sys_end();

return 0;
}

Subversion Repositories shark

(root)/demos/branches/pj/thdemo/fftplay.c @ 1450 - Rev 1085