/demos/trunk/thdemo/makefile |
---|
0,0 → 1,17 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= demo |
include $(BASE)/config/example.mk |
demo: |
make -f $(SUBMAKE) APP=demo INIT= OTHEROBJS="fft.o camera.o initfile.o jetctrl.o ball.o" SHARKOPT="__FFT__ __OLDCHAR__ __PXC__ __GRX__" |
/demos/trunk/thdemo/fft.c |
---|
0,0 → 1,533 |
// 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 <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 |
} |
/demos/trunk/thdemo/demo.c |
---|
0,0 → 1,260 |
/* 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) */ |
#include "demo.h" |
//#include <kernel/types.h> |
//#include <kernel/model.h> |
#include <kernel/func.h> |
//#include <modules/cabs.h> |
#include <string.h> |
#include <stdlib.h> |
#include <drivers/keyb.h> |
//#include <drivers/crtwin.h> |
#include <drivers/glib.h> |
//#include <drivers/sound.h> |
//#include <ports/rfftw.h> |
/* Informations about the sampling rate and buffers */ |
WORD rawdata_nsamples; |
WORD rawdata_buffer_size; |
WORD rawdata_freq; |
/* graphic mutex... */ |
mutex_t mutex; |
/* useful colors... */ |
int white; |
int black; |
int red; |
int gray; |
void app_mutex_init(mutex_t *m); |
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; |
} |
void display_jet(PID i) |
{ |
char st[200]; |
TIME sum, max; |
int n; |
if (jet_getstat(i, &sum, &max, &n, NULL) != -1) { |
if (n==0) n=1; |
sprintf(st, "PID=%2d st=%3d Mean=%5d Max=%5d na=%10s", |
(int)i, |
(int)proc_table[i].status, (int)sum/n, (int)max, |
proc_table[i].name); |
mutex_lock(&mutex); |
grx_text(st, 0, 400+i*8, 255, 0); |
mutex_unlock(&mutex); |
} |
} |
void scenario(int f) |
{ |
grx_text("HARTIK 4.0 - LEGO Version" , 0,0, rgb16(0,255,0), black ); |
grx_text("Thesis Demo", 0,8, rgb16(0,255,0), black ); |
#ifdef FFT_ON |
scenario_fftplay(f); |
#endif |
#ifdef FRAMEGRABBER_ON |
scenario_framegrabber(); |
#endif |
#ifdef JET_ON |
scenario_jetcontrol(); |
#endif |
#ifdef BALL_ON |
scenario_ball(); |
#endif |
} |
void my_close(void *arg) |
{ |
grx_close(); |
} |
void endfun(KEY_EVT *k) |
{ |
grx_close(); |
cprintf("Ctrl-Brk pressed! Ending...\n"); |
sys_end(); |
} |
void zerofun(KEY_EVT *k) |
{ |
int i; |
for (i=0; i<MAX_PROC; i++) jet_delstat(i); |
} |
void printeventqueue(void *arg) |
{ |
struct event *p; |
extern struct event *firstevent; |
kern_cli(); |
grx_close(); |
kern_cli(); |
for (p = firstevent; p != NULL; p = p->next) { |
kern_printf("par:%d time:%ld.%ld p:%d handler:%d\n", |
(int)p->par, p->time.tv_sec, p->time.tv_nsec/1000, (int)p, (int)p->handler); |
} |
kern_sti(); |
} |
int main(int argc, char **argv) |
{ |
int modenum; |
int f; |
KEY_EVT k; |
srand(4); |
version(); |
#ifdef FRAMEGRABBER_ON |
if (argc == 1) |
{ |
cprintf("type x fftplay <freq>"); |
return 0; |
} |
f = atoi(argv[1]); |
compute_params(&f,&rawdata_nsamples); |
#endif |
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); |
k.flag = ALTL_BIT; |
k.scan = KEY_C; |
k.ascii = 'c'; |
keyb_hook(k,zerofun); |
k.flag = 0; |
k.scan = KEY_ENT; |
k.ascii = 13; |
keyb_hook(k,endfun); |
sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
grx_init(); |
modenum = grx_getmode(640, 480, 16); |
grx_setmode(modenum); |
/* init the graphic mutex */ |
app_mutex_init(&mutex); |
/* useful colors ... */ |
white = rgb16(255,255,255); |
black = rgb16(0,0,0); |
red = rgb16(255,0,0); |
gray = rgb16(128,128,128); |
scenario(f/2); |
// grx_close(); |
// clear(); |
#ifdef FFT_ON |
init_fftplay(f); |
#endif |
#ifdef FRAMEGRABBER_ON |
init_framegrabber(); |
#endif |
#ifdef JET_ON |
init_jetcontrol(); |
#endif |
#ifdef BALL_ON |
init_ball(); |
#endif |
group_activate(1); |
#ifdef FRAMEGRABBER_ON |
start_framegrabber(); |
#endif |
return 0; |
} |
/demos/trunk/thdemo/fftplay.c |
---|
0,0 → 1,769 |
/* 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(); |
} |
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; |
} |
/demos/trunk/thdemo/initfile.c |
---|
0,0 → 1,146 |
/* |
* Project: HARTIK (HA-rd R-eal TI-me K-ernel) |
* |
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
* Gerardo Lamastra <gerardo@sssup.it> |
* |
* Authors : Paolo Gai <pj@hartik.sssup.it> |
* (see authors.txt for full list of hartik's authors) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://hartik.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: initfile.c,v 1.1.1.1 2002-09-02 09:37:41 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.1.1.1 $ |
Last update: $Date: 2002-09-02 09:37:41 $ |
------------ |
System initialization file |
h3pi.c |
This file is equal to hartik3.c plus the resources module... |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
//#define PI |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#ifndef PI |
#include "modules/rrsoft.h" |
#endif |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "modules/pi.h" |
#include "modules/pc.h" |
#include "modules/srp.h" |
#include "modules/npp.h" |
#include "modules/nop.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 1000 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
//#define RRTICK 3000 |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
#ifdef PI |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
#else |
RRSOFT_register_level(RRTICK, RR_MAIN_NO, mb, RRSOFT_ONLY_HARD|RRSOFT_ONLY_SOFT); |
RRSOFT_register_level(RRTICK, RR_MAIN_NO, mb, RRSOFT_ONLY_SOFT); //cbs |
#endif |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
PI_register_module(); |
NOP_register_module(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
HARTPORT_init(); |
kern_printf("TIME=%ld\n",sys_gettime(NULL)); |
KEYB_init(NULL); |
kern_printf("TIME=%ld\n",sys_gettime(NULL)); |
// KEYB_init(NULL); |
__call_main__(mb); |
return (void *)0; |
} |
#ifdef PI |
void app_mutex_init(mutex_t *m) |
{ |
PI_mutexattr_t attr; |
PI_mutexattr_default(attr); |
mutex_init(m, &attr); |
} |
#else |
void app_mutex_init(mutex_t *m) |
{ |
NOP_mutexattr_t attr; |
NOP_mutexattr_default(attr); |
mutex_init(m, &attr); |
} |
#endif |
/demos/trunk/thdemo/jetctrl.c |
---|
0,0 → 1,200 |
// JetControl |
#include "demo.h" |
#include "kernel/func.h" |
TASK jetdummy_task(void *arg) |
{ |
TIME now_dummy, last_dummy, diff_dummy, slice; |
struct timespec now, last, diff; |
int x = 0; |
int height; |
NULL_TIMESPEC(&last); |
last_dummy = 0; |
for (;;) { |
task_nopreempt(); |
jet_getstat(DUMMY_PID, NULL, NULL, NULL, &now_dummy); |
sys_gettime(&now); |
task_preempt(); |
SUBTIMESPEC(&now, &last, &diff); |
slice = diff.tv_sec * 1000000 + diff.tv_nsec/1000; |
diff_dummy = now_dummy - last_dummy; |
height = (int)(JET_DUMMY_HEIGHT*((float)diff_dummy)/((float)slice)); |
TIMESPEC_ASSIGN(&last, &now); |
last_dummy = now_dummy; |
mutex_lock(&mutex); |
grx_line(JET_DUMMY_X+x,JET_DUMMY_Y, |
JET_DUMMY_X+x,JET_DUMMY_Y+height ,black); |
grx_line(JET_DUMMY_X+x,JET_DUMMY_Y+height, |
JET_DUMMY_X+x,JET_DUMMY_Y+JET_DUMMY_HEIGHT,white); |
grx_line(JET_DUMMY_X+(x+1)%JET_DUMMY_WIDTH,JET_DUMMY_Y, |
JET_DUMMY_X+(x+1)%JET_DUMMY_WIDTH,JET_DUMMY_Y+JET_DUMMY_HEIGHT,255); |
mutex_unlock(&mutex); |
x = (x+1)%JET_DUMMY_WIDTH; |
task_endcycle(); |
} |
} |
TASK jetctrl_task(void *arg) |
{ |
char st[50]; |
TIME sum, max; |
int n; |
PID i; |
int printed = 0; |
for (;;) { |
for (i=2, printed=0; i<MAX_PROC && printed<JET_NTASK; i++) { |
if (jet_getstat(i, &sum, &max, &n, NULL) != -1) { |
if (!n) n=1; |
sprintf(st, "%6d %6d %10s", (int)sum/n, (int)max, proc_table[i].name); |
mutex_lock(&mutex); |
grx_text(st, 384, JET_Y_NAME+16+printed*8, gray, black); |
mutex_unlock(&mutex); |
printed++; |
} |
} |
while (printed<JET_NTASK) { |
mutex_lock(&mutex); |
grx_text(" ", |
384, JET_Y_NAME+16+printed*8, gray, black); |
mutex_unlock(&mutex); |
printed++; |
} |
task_endcycle(); |
} |
} |
TASK jetslide_task(void *arg) |
{ |
TIME sum, curr, max; |
TIME total[JET_NTASK]; |
int slides[JET_NTASK]; |
PID i; |
int printed = 0; |
for (;;) { |
// Fill the total array in a nonpreemptive section |
task_nopreempt(); |
for (i=2, printed=0; i<MAX_PROC && printed<JET_NTASK; i++) { |
if (jet_getstat(i, &sum, NULL, NULL, &curr) != -1) { |
total[printed] = sum+curr; |
printed++; |
} |
} |
task_preempt(); |
while (printed < JET_NTASK) |
total[printed++] = 0; |
// Compute the Max elapsed time |
max = 0; |
for (i=0; i<JET_NTASK; i++) |
if (total[i] > max) max = total[i]; |
if (!max) max = 1; |
// Compute the slides width |
for (i=0; i<JET_NTASK; i++) |
slides[i] = (int)( (((float)total[i])/max) * JET_SLIDE_WIDTH); |
// print the data |
mutex_lock(&mutex); |
for (i=0; i<JET_NTASK; i++) { |
grx_box(JET_SLIDE_X, JET_Y_NAME+16+i*8, |
JET_SLIDE_X+slides[i], JET_Y_NAME+23+i*8, gray); |
grx_box(JET_SLIDE_X+slides[i], JET_Y_NAME+16+i*8, |
JET_SLIDE_X+JET_SLIDE_WIDTH, JET_Y_NAME+23+i*8, black); |
} |
while (i<JET_NTASK) { |
grx_box(JET_SLIDE_X, JET_Y_NAME+16+i*8, |
JET_SLIDE_X+JET_SLIDE_WIDTH, JET_Y_NAME+20+i*8, black); |
i++; |
} |
mutex_unlock(&mutex); |
task_endcycle(); |
} |
} |
void scenario_jetcontrol(void) |
{ |
grx_text("System load" , 384, 248, rgb16(0,0,255), black); |
grx_line(384,258,639,258,red); |
grx_text(" Mean Max Name Slide", 384, JET_Y_NAME, gray, black); |
grx_line(384,JET_Y_NAME+10,639,JET_Y_NAME+10,gray); |
grx_rect(JET_DUMMY_X-1, JET_DUMMY_Y-1, |
JET_DUMMY_X+JET_DUMMY_WIDTH, JET_DUMMY_Y+JET_DUMMY_HEIGHT+1, gray); |
grx_text("100%", JET_DUMMY_X-40, JET_DUMMY_Y, gray, black); |
grx_text(" 0%", JET_DUMMY_X-40, JET_DUMMY_Y+JET_DUMMY_HEIGHT-8, gray, black); |
grx_line(JET_DUMMY_X-1, JET_DUMMY_Y, JET_DUMMY_X-5, JET_DUMMY_Y, gray); |
grx_line(JET_DUMMY_X-1, JET_DUMMY_Y+JET_DUMMY_HEIGHT, JET_DUMMY_X-5, JET_DUMMY_Y+JET_DUMMY_HEIGHT, gray); |
grx_line(384,258,639,258,red); |
} |
void init_jetcontrol(void) |
{ |
SOFT_TASK_MODEL m3, m4, m5; |
PID p3, p4, p5; |
soft_task_default_model(m3); |
soft_task_def_level(m3,1); |
soft_task_def_period(m3, PERIOD_JETCTRL); |
soft_task_def_met(m3, WCET_JETCTRL); |
soft_task_def_ctrl_jet(m3); |
soft_task_def_group(m3, 1); |
p3 = task_create("jctrl", jetctrl_task, &m3, NULL); |
if (p3 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <jetctrl>\n"); |
ll_abort(54); |
sys_end(); |
} |
soft_task_default_model(m4); |
soft_task_def_level(m4,1); |
soft_task_def_period(m4, PERIOD_JETDUMMY); |
soft_task_def_met(m4, WCET_JETDUMMY); |
soft_task_def_group(m4, 1); |
soft_task_def_usemath(m4); |
soft_task_def_ctrl_jet(m4); |
p4 = task_create("jdmy", jetdummy_task, &m4, NULL); |
if (p4 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <jetdummy>\n"); |
ll_abort(54); |
sys_end(); |
} |
soft_task_default_model(m5); |
soft_task_def_level(m5,1); |
soft_task_def_period(m5, PERIOD_JETSLIDE); |
soft_task_def_met(m5, WCET_JETSLIDE); |
soft_task_def_group(m5, 1); |
soft_task_def_usemath(m5); |
soft_task_def_ctrl_jet(m5); |
p5 = task_create("jsli", jetslide_task, &m5, NULL); |
if (p5 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <jetslide>\n"); |
ll_abort(54); |
sys_end(); |
} |
} |
/demos/trunk/thdemo/camera.c |
---|
0,0 → 1,516 |
// framegrabber stuffs |
/* File name ......... : ELABOR.C |
* Project............ : |
* Object ............ : |
* Author ............ : Facchinetti Tullio |
* Language .......... : C |
* Compiler .......... : GNU C |
* Operative system .. : MS-DOS/HARTIK |
* Creation data ..... : 04/03/2000 |
* Last modify ....... : 19/11/99 |
*/ |
#include <kernel/func.h> |
#include <modules/cabs.h> |
#include <stdio.h> |
#include <drivers/pxc.h> |
#include "demo.h" |
PID camera_PID; |
PID tracking_PID; |
static CAB frameCAB; // CAB di deposito delle immagini |
static CAB trackingCAB; // CAB di deposito delle info di tracking |
int img_border = 10; |
int window_width = 40; |
int window_height = 40; |
TPixel pix_threshold = 128; |
// a 256 grayscale palette |
WORD gray_palette[256]; |
// the image to be putted on the screen |
WORD converted_image[IMG_COL*IMG_ROW]; |
TDataObj sequence[N_FRAMES]; |
void border_up_function(KEY_EVT key) |
{ |
img_border++; |
} |
void border_down_function(KEY_EVT key) |
{ |
img_border--; |
} |
void threshold_up_function(KEY_EVT key) |
{ |
char st[50]; |
pix_threshold++; |
sprintf(st, "threshold %4d", pix_threshold); |
mutex_lock(&mutex); |
grx_text(st, 400, 100, 255, 0); |
mutex_unlock(&mutex); |
} |
void threshold_down_function(KEY_EVT key) |
{ |
char st[50]; |
pix_threshold--; |
sprintf(st, "threshold %4d", pix_threshold); |
mutex_lock(&mutex); |
grx_text(st, 400, 100, 255, 0); |
mutex_unlock(&mutex); |
} |
float distance(unsigned int x1, unsigned int y1, |
unsigned int x2, unsigned int y2) |
{ |
return(sqrt(((y2 - y1) * (y2 - y1)) + ((x2 - x1) * (x2 - x1)))); |
} |
char scan_window_frame(TDataObj *data, TPixel *in_frame, |
unsigned int xc, unsigned int yc, int border) |
{ |
unsigned long int offset; |
unsigned int i, j; |
TPixel pix; |
double sum_x = 0.0, sum_y = 0.0; |
unsigned int n_pix = 0; |
int x1, y1, x2, y2; // Must be int!!! |
char found; |
data->x1 = N_COL; |
data->y1 = N_ROW; |
data->x2 = data->y2 = 0; |
data->xb = data->yb = -1; |
data->time_stamp = -1; |
found = 0; |
x1 = MAX_NUM((xc - window_width / 2), (border)); |
y1 = MAX_NUM((yc - window_height / 2), (border)); |
x2 = MIN_NUM((xc + window_width / 2), (N_COL - border)); |
y2 = MIN_NUM((yc + window_height / 2), (N_ROW - border)); |
for (i = y1; i < y2; i++) { |
for (j = x1; j < x2; j++) { |
offset = i * N_COL + j; |
pix = *(in_frame + offset); |
#ifdef __BLACK_ON_WHITE |
// Pixel found (object is black, background is white) |
if (pix < pix_threshold) { |
#else |
// Pixel found (object is white, background is black) |
if (pix > pix_threshold) { |
#endif |
data->time_stamp = sys_gettime(NULL); |
found = 1; |
n_pix++; |
sum_x += j; |
sum_y += i; |
// *(in_frame + offset) = 0; |
if (i < data->y1) |
data->y1 = i; |
if (i > data->y2) |
data->y2 = i; |
if (j < data->x1) |
data->x1 = j; |
if (j > data->x2) |
data->x2 = j; |
} else { |
// *(in_frame + offset) = 255; |
} |
} |
} |
data->xb = sum_x / n_pix; |
data->yb = sum_y / n_pix; |
return(found); |
} |
char scan_all_frame(TDataObj *data, TPixel *in_frame) |
{ |
unsigned long int offset; |
unsigned int i, j; |
TPixel pix; |
double sum_x = 0.0, sum_y = 0.0; |
unsigned int n_pix = 0; |
char found; |
data->x1 = N_COL; |
data->y1 = N_ROW; |
data->x2 = data->y2 = 0; |
data->xb = data->yb = -1; |
data->time_stamp = -1; |
found = 0; |
// In a single image scanning it performs thresholding and computation |
for (i = img_border; i < N_ROW - img_border; i++) { |
for (j = img_border; j < N_COL - img_border; j++) { |
offset = i * N_COL + j; |
pix = *(in_frame + offset); |
#ifdef __BLACK_ON_WHITE |
// Pixel found (object is black, background is white) |
if (pix < pix_threshold) { |
#else |
// Pixel found (object is white, background is black) |
if (pix > pix_threshold) { |
#endif |
data->time_stamp = sys_gettime(NULL); |
found = 1; |
n_pix++; |
sum_x += j; |
sum_y += i; |
// *(in_frame + offset) = 0; |
if (i < data->y1) |
data->y1 = i; |
if (i > data->y2) |
data->y2 = i; |
if (j < data->x1) |
data->x1 = j; |
if (j > data->x2) |
data->x2 = j; |
} else { |
// *(in_frame + offset) = 255; |
} |
} |
} |
data->xb = sum_x / n_pix; |
data->yb = sum_y / n_pix; |
return(found); |
} |
void tracking(int top_frame, int *track_x, int *track_y, int *int_vx, int *int_vy, int time_to) |
{ |
float vx, vy; |
vx = (float)(sequence[top_frame - 1].xb - sequence[top_frame - 2].xb) / |
(float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp); |
vx *= 1000; |
vy = (float)(sequence[top_frame - 1].yb - sequence[top_frame - 2].yb) / |
(float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp); |
vy *= 1000; |
*track_x = sequence[top_frame - 1].xb + vx * time_to; |
*track_y = sequence[top_frame - 1].yb + vy * time_to; |
*int_vx = vx * 1000; |
*int_vy = vy * 1000; |
} |
TASK tracking_task(void *arg) |
{ |
// static unsigned int n_frame = 0; |
char found; |
TPixel *grabber_frame; |
int top_frame = 0; |
TDataObj current; |
TTracking *track; |
frameCAB = PXC_GetCab(); |
grabber_frame = cab_getmes(frameCAB); |
// Executes first time |
found = scan_all_frame(¤t, grabber_frame); |
if (found) { |
memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
top_frame++; |
} |
cab_unget(frameCAB, grabber_frame); |
task_endcycle(); |
while (1) { |
// Acquisizione immagine corrente |
grabber_frame = (TPixel *)cab_getmes(frameCAB); |
track = (TTracking *)cab_reserve(trackingCAB); |
// Estrazione della nuova trasformata sul frame corrente |
if (found) { |
found = scan_window_frame(¤t, grabber_frame, current.xb, current.yb, img_border); |
} else { |
found = scan_all_frame(¤t, grabber_frame); |
} |
track->found = found; |
if (found) { |
if (top_frame < N_FRAMES) { |
memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
top_frame++; |
} else { |
top_frame = 0; |
memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
} |
track->top_frame = top_frame; |
memcpy(&track->current, ¤t, sizeof(TDataObj)); |
if (top_frame > 1) { |
tracking(top_frame, &track->predx, &track->predy, |
&track->vx, &track->vy, 100); |
} |
} else { |
track->top_frame = top_frame = 0; |
} |
// Release CABs |
cab_putmes(trackingCAB, (char *)track); |
cab_unget(frameCAB, grabber_frame); |
task_endcycle(); |
} |
} |
/* |
* |
* |
* |
* Camera task |
* |
* |
* |
* |
*/ |
TASK camera_task(void *arg) |
{ |
register int i,j,col,row; |
static unsigned int n_frame = 0; |
TPixel *grabber_frame; |
TTracking *track; |
char st[50]; |
// Inizializzazione del task |
frameCAB = PXC_GetCab(); |
while (1) { |
n_frame++; |
sprintf(st, "frame n. %5d", n_frame); |
grx_text(st, 400, 224, white, 0); |
// Acquisizione immagine corrente |
grabber_frame = cab_getmes(frameCAB); |
for (i=1; i<IMG_ROW-1; i++) |
for (j=0; j<IMG_COL; j++) { |
col = (j*(N_COL-1))/(IMG_COL-1); |
row = (i*(N_ROW-1))/(IMG_ROW-1); |
converted_image[i*IMG_COL+j] = gray_palette[*(grabber_frame+row*N_COL+col)]; |
} |
// Release CAB |
cab_unget(frameCAB, grabber_frame); |
for (j=0; j<IMG_COL; j++) { |
converted_image[j] = gray_palette[0]; |
converted_image[(IMG_ROW-1)*IMG_COL+j] = gray_palette[0]; |
} |
mutex_lock(&mutex); |
grx_putimage(IMG_X, IMG_Y, IMG_X+IMG_COL-1, IMG_Y+IMG_ROW-1, |
(BYTE *)converted_image); |
mutex_unlock(&mutex); |
track = (TTracking *)cab_getmes(trackingCAB); |
if (track->found) { |
mutex_lock(&mutex); |
// sprintf(st, "found: %d", track->found); |
// grx_text(st, 400, 280, 255, 0); |
if (track->top_frame > 1) { |
int px, py; |
// sprintf(st, "top_frame %5d", track->top_frame); |
// grx_text(st, 400, 270, 255, 0); |
if (track->predx < img_border) |
px = img_border; |
else if (track->predx > N_COL-img_border) |
px = N_COL-img_border; |
else |
px = track->predx; |
if (track->predy < img_border) |
py = img_border; |
else if (track->predy > N_ROW-img_border) |
py = N_ROW-img_border; |
else |
py = track->predy; |
grx_disc(IMG_X+(px*2)/3, IMG_Y+(py*2)/3, 3, 127); |
// grx_disc(IMG_X+(current.xb*2)/3, IMG_Y+(current.yb*2)/3, 3, 127); |
grx_rect(IMG_X+(track->current.x1*2)/3, IMG_Y+(track->current.y1*2)/3, |
IMG_X+(track->current.x2*2)/3, IMG_Y+(track->current.y2*2)/3, 127); |
sprintf(st, "speed = (%5d, %5d) pix/s", track->vx, track->vy); |
grx_text(st, 400, 232, white, 0); |
} |
mutex_unlock(&mutex); |
} |
cab_unget(trackingCAB, (char *)track); |
task_endcycle(); |
} |
} |
/* |
* |
* |
* |
* Framegrabber Initialization |
* |
* |
* |
* |
*/ |
void start_listener(TIME p); |
void framegrabber_close(void *arg) |
{ |
PXC_Close(); |
} |
void scenario_framegrabber() |
{ |
grx_text("Camera" , 384, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black); |
grx_line(384,WAVE_Y-WAVE_HEIGHT-1,639,WAVE_Y-WAVE_HEIGHT-1,red); |
} |
void init_framegrabber(void) |
{ |
register int i; |
KEY_EVT my_key; |
TIME period; |
my_key.ascii = 'a'; |
my_key.scan = KEY_A; |
keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_up_function); |
my_key.ascii = 'z'; |
my_key.scan = KEY_Z; |
keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_down_function); |
my_key.ascii = 's'; |
my_key.scan = KEY_S; |
keyb_hook(my_key, (void (*)(KEY_EVT *))border_up_function); |
my_key.ascii = 'x'; |
my_key.scan = KEY_X; |
keyb_hook(my_key, (void (*)(KEY_EVT *))border_down_function); |
// Aggiusta la palette |
for (i = 0; i < 256; i++) |
gray_palette[i] = rgb16(i,i,i); |
//for (i = 0; i < 256; i++) |
// grx_setcolor(i, i/4, i/4, i/4); |
mutex_lock(&mutex); |
// grx_text("Grabber enabled: no test!", 10, 10, 255, 0); |
// Some messages on screen |
// grx_text("A-Z change threshold", 400, 240, 255, 0); |
// grx_text("S-X change window borders", 400, 250, 255, 0); |
mutex_unlock(&mutex); |
period = PXC_Initiate(4); |
if (!period) { |
grx_close(); |
cprintf("Problemi nell'inizializzazione del framegrabber\n"); |
halt(); |
sys_end(); |
} else { |
TTracking *trdata; |
// tracking CAB init |
trackingCAB = cab_create("trackingCAB", sizeof(TTracking), 3); |
trdata = (TTracking *)cab_reserve(trackingCAB); |
trdata->found = 0; |
cab_putmes(trackingCAB, (char *)trdata); |
start_listener(period); |
} |
sys_atrunlevel(framegrabber_close, NULL, RUNLEVEL_BEFORE_EXIT); |
} |
void start_listener(TIME period) |
{ |
SOFT_TASK_MODEL m1, m2; |
soft_task_default_model(m1); |
soft_task_def_level(m1,1); |
soft_task_def_met(m1,WCET_TRACKING); |
soft_task_def_usemath(m1); |
// soft_task_def_aperiodic(m1); |
soft_task_def_period(m1,(PERIOD_TRACKING)); |
soft_task_def_group(m1,1); |
soft_task_def_ctrl_jet(m1); |
soft_task_def_skip_arrivals(m1); |
tracking_PID = task_create("track", tracking_task, &m1, NULL); |
if (tracking_PID == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <tra>\n"); |
ll_abort(54); |
perror("FFTPlay: Could not create task <tracking>\n"); |
sys_end(); |
} |
soft_task_default_model(m2); |
soft_task_def_level(m2,1); |
soft_task_def_met(m2,WCET_CAMERA); |
soft_task_def_usemath(m2); |
// soft_task_def_aperiodic(m2); |
soft_task_def_period(m2,PERIOD_CAMERA); |
soft_task_def_group(m2,1); |
soft_task_def_ctrl_jet(m2); |
// soft_task_def_skip_arrivals(m2); |
camera_PID = task_create("cam", camera_task, &m2, NULL); |
if (camera_PID == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <came>\n"); |
ll_abort(54); |
perror("FFTPlay: Could not create task <camera>\n"); |
sys_end(); |
} |
} |
void start_framegrabber() |
{ |
// PXC_Push_Listener(tracking_PID,2); |
PXC_Start(); |
} |
/demos/trunk/thdemo/demo.h |
---|
0,0 → 1,280 |
#include <ll/ll.h> |
#include <kernel/types.h> |
#include <kernel/descr.h> |
#include <math.h> |
#include <drivers/glib.h> |
#include <drivers/keyb.h> |
#define FFT_ON |
#define FRAMEGRABBER_ON |
#define JET_ON |
#define BALL_ON |
/* |
* |
* WCET, Periods and Models |
* |
*/ |
/* define if you want NRT or SOFT... */ |
#define TASK_TYPE SOFT |
//#define TASK_TYPE NRT |
// on Celeron 366 |
#define WCET_WAVE 450 |
#define WCET_FFT 722 |
#define WCET_EQU 1000 |
#define WCET_EQU2D 318 |
#define PERIOD_WAVE 40000 |
#define PERIOD_FFT 3000 |
#define PERIOD_EQU 40000 |
#define PERIOD_EQU2D 3000 |
#define WCET_TRACKING 4000 |
#define WCET_CAMERA 7000 |
#define PERIOD_TRACKING 40000 |
#define PERIOD_CAMERA 40000 |
#define WCET_JETCTRL 2600 |
#define WCET_JETDUMMY 1000 |
#define WCET_JETSLIDE 1000 |
#define PERIOD_JETCTRL 100000 |
#define PERIOD_JETDUMMY 100000 |
#define PERIOD_JETSLIDE 100000 |
#define WCET_BALL 60 |
#define PERIOD_BALL 10000 |
/* |
* |
* Soundcard related defines |
* |
*/ |
/* Samples are 16-bit signed integers */ |
typedef short SAMPLE; |
#define MAX_SAMPLE 32768 |
/* |
* |
* FFT defines |
* |
*/ |
/* Numbers of samples of the sample window */ |
#define WINDATA_NSAMPLES 512 |
/* task WAVE */ |
/* the point (wave_x,wave_y) is on the center left of the area... */ |
#define WAVE_NSAMPLES 384 |
#define WAVE_X 0 |
#define WAVE_Y 64 |
#define WAVE_HEIGHT 32 |
/* task FFT */ |
#define FFT_NSAMPLES 512 |
#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 64 |
#define EQU_Y 128 |
#define EQU_HEIGHT 64 |
#define EQU_SHADE |
/* task EQU2D */ |
/* the point (equ2d_x, equ2d_y) is the top left corner */ |
#define EQU2D_NSAMPLES EQU_NSAMPLES |
#define EQU2D_X 128 |
#define EQU2D_Y EQU_Y |
#define EQU2D_WIDTH 255 |
#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) |
/* Informations about the sampling rate and buffers */ |
extern WORD rawdata_nsamples; |
extern WORD rawdata_buffer_size; |
extern WORD rawdata_freq; |
/* |
* |
* Global Stuffs |
* |
*/ |
/* graphic mutex... */ |
extern mutex_t mutex; |
/* useful colors... */ |
extern int white; |
extern int black; |
extern int red; |
extern int gray; |
/* define if shorts critical sections wanted */ |
#define SHORT_CRITICAL_SECTIONS(x) \ |
if (!((x)%64)) \ |
{ \ |
mutex_lock(&mutex); \ |
mutex_unlock(&mutex); \ |
} |
void init_fftplay(int freq); |
void init_framegrabber(); |
void init_jetcontrol(); |
void init_ball(void); |
void start_framegrabber(); |
void scenario_jetcontrol(void); |
void scenario_fftplay(int f); |
void scenario_framegrabber(); |
void scenario_ball(); |
void compute_params(int *freq,WORD *nsamp); |
char * itoa(int n, char *s); |
/* |
* |
* Framegrabber stuffs |
* |
*/ |
// if defined... object black on a white background |
#ifndef __BLACK_ON_WHITE |
#define __BLACK_ON_WHITE |
#endif |
#define ABS_NUM(a) ((a >= 0) ? a : -a) |
#define MIN_NUM(a, b) ((a < b) ? a : b) |
#define MAX_NUM(a, b) ((a > b) ? a : b) |
// Cols and rows of the framegrabber image |
#define N_COL 384 |
#define N_ROW 288 |
#define N_BPS 8 // Bits per pixel |
#define N_GRIGI 256 |
#define N_FRAMES 100 |
/* pixel of the video image */ |
#define IMG_COL 256 |
#define IMG_ROW 192 |
/* position of the video image */ |
#define IMG_X 384 |
#define IMG_Y 32 |
// I singoli pixel sono caratteri a 8 bit |
typedef unsigned char TPixel; |
typedef struct { |
int x1, y1; |
int x2, y2; |
int xb, yb; |
TIME time_stamp; |
} TDataObj; |
typedef struct { |
int found; |
int top_frame; |
int vx,vy; |
int predx; |
int predy; |
TDataObj current; |
} TTracking; |
float distance(unsigned int x1, unsigned int y1, |
unsigned int x2, unsigned int y2); |
char scan_all_frame(TDataObj *data, TPixel *in_frame); |
char scan_window_frame(TDataObj *data, TPixel *in_frame, |
unsigned int xc, unsigned int yc, int border); |
void threshold_up_function(KEY_EVT key); |
void threshold_down_function(KEY_EVT key); |
void border_up_function(KEY_EVT key); |
void border_down_function(KEY_EVT key); |
void tracking(int top_frame, int *track_x, int *track_y, int *int_vx, int *int_vy, int time_to); |
TASK elab_image_TASK(void); |
/* |
* |
* JETCONTROL stuffs |
* |
*/ |
#define JET_NTASK 18 |
#define JET_Y_NAME 320 |
#define DUMMY_PID 1 |
#define JET_DUMMY_WIDTH 210 |
#define JET_DUMMY_HEIGHT 40 |
/* the point (x, y) is the top left corner */ |
#define JET_DUMMY_X 428 |
#define JET_DUMMY_Y 264 |
#define JET_SLIDE_WIDTH 50 |
#define JET_SLIDE_X 576 |
/* |
* |
* BALL stuffs |
* |
*/ |
// x and y corners are specified whithout consider a border of 3 pixels |
#define BALL_Y 475 /* position of the floor */ |
#define BALL_HEIGHT 70 /* initial height of the ball */ |
#define BALL_XMIN 3 /* min position X of the ball */ |
#define BALL_XMAX 380 /* max position X of the ball */ |
#define BALL_VELX 5. /* horizontal ball velocity */ |
#define BALL_VYMIN 11. /* velocitÂ… minima per suono */ |
#define BALL_MAX_P 50 /* max number of balls */ |
#define BALL_GROUP 2 /* task group of the balls */ |
/demos/trunk/thdemo/ball.c |
---|
0,0 → 1,164 |
/*--------------------------------------------------------------*/ |
/* SIMULATION OF JUMPING BALLS */ |
/*--------------------------------------------------------------*/ |
/* CVS $Id: ball.c,v 1.1.1.1 2002-09-02 09:37:41 pj Exp $ */ |
#include "demo.h" |
#include <kernel/func.h> |
#include <stdlib.h> |
#define R 2 /* dimension of a ball */ |
#define G 9.8 /* acceleration of gravity */ |
static int ballexit = 0; |
static int npc = 0; /* number of tasks created */ |
/*--------------------------------------------------------------*/ |
/* Periodic task for ball simulation */ |
/*--------------------------------------------------------------*/ |
TASK palla(int i) |
{ |
int x, y; /* coordinate grafiche pallina */ |
int ox, oy; /* vecchia posizione pallina */ |
int x0, y0; /* posizione iniziale X pallina */ |
float vx, vy; /* velocitÂ… della pallina */ |
float vy0; /* velocita' pallina al primo rimbalzo */ |
float ty, tx; /* variabile temporale */ |
float dt; /* incremento temporale */ |
y = oy = y0 = BALL_HEIGHT; |
x = ox = x0 = BALL_XMIN; |
vy0= sqrt(2. * G * (float)BALL_HEIGHT); |
vy = 0; |
vx = BALL_VELX + rand()%9; |
tx = 0; |
ty = 0; |
dt = ((float)PERIOD_BALL)/100000; |
while (1) { |
y = y0 + vy*ty - .5*G*ty*ty; |
x = x0 + vx * tx; |
if (y < 0) { |
y = 0; |
if (vy == 0.0) |
vy = vy0; |
else if (vy < BALL_VYMIN) |
vy = vy0 * (1.0 - (rand()%50)/100.0); |
else |
vy = 0.9 * vy; |
ty = 0.0; |
y0 = 0; |
} |
if (x > BALL_XMAX) { |
tx = 0.0; |
x0 = BALL_XMAX; |
vx = -vx; |
x = x0 + vx * tx; |
} |
if (x < BALL_XMIN) { |
tx = 0.0; |
x0 = BALL_XMIN; |
vx = -vx; |
x = x0 + vx * tx; |
} |
mutex_lock(&mutex); |
grx_disc(ox, oy, R, 0); |
ox = x; |
oy = BALL_Y - y; |
mutex_unlock(&mutex); |
if (ballexit) { |
npc--; |
return 0; |
} |
mutex_lock(&mutex); |
grx_disc(ox, oy, R, i); |
mutex_unlock(&mutex); |
{ |
int xxx; |
for (xxx=0; xxx<10000; xxx++); |
} |
ty += dt; |
tx += dt; |
task_endcycle(); |
} |
} |
void killball(KEY_EVT *k) |
{ |
ballexit = 1; |
} |
void ballfun(KEY_EVT *k) |
{ |
SOFT_TASK_MODEL mp; |
int r,g,b; |
PID pid; |
if (npc == BALL_MAX_P) return; |
ballexit = 0; |
r = 64 + rand()%192; |
g = 64 + rand()%192; |
b = 64 + rand()%192; |
soft_task_default_model(mp); |
soft_task_def_level(mp,1); |
soft_task_def_ctrl_jet(mp); |
soft_task_def_arg(mp, (void *)rgb16(r,g,b)); |
soft_task_def_group(mp, BALL_GROUP); |
soft_task_def_met(mp, WCET_BALL); |
soft_task_def_period(mp,PERIOD_BALL); |
soft_task_def_usemath(mp); |
pid = task_create("palla", palla, &mp, NULL); |
if (pid != NIL) { |
task_activate(pid); |
npc++; |
} |
} |
/*--------------------------------------------------------------*/ |
/* MAIN process */ |
/*--------------------------------------------------------------*/ |
void scenario_ball() |
{ |
grx_text("Noise", 0, BALL_Y-BALL_HEIGHT-15, rgb16(0,0,255), black); |
grx_line(0,BALL_Y-BALL_HEIGHT-6,383,BALL_Y-BALL_HEIGHT-6,red); |
} |
void init_ball(void) |
{ |
KEY_EVT k; |
mutex_lock(&mutex); |
grx_rect(BALL_XMIN-R-1, BALL_Y-BALL_HEIGHT-R-1, |
BALL_XMAX+R+1, BALL_Y+R+1, rgb16(0,200,0)); |
mutex_unlock(&mutex); |
k.flag = 0; |
k.scan = KEY_SPC; |
k.ascii = ' '; |
keyb_hook(k,ballfun); |
k.flag = 0; |
k.scan = KEY_BKS; |
k.ascii = ' '; |
keyb_hook(k,killball); |
} |
/*--------------------------------------------------------------*/ |
/demos/trunk/thdemo/camera2.c |
---|
0,0 → 1,424 |
// framegrabber stuffs |
/* File name ......... : ELABOR.C |
* Project............ : |
* Object ............ : |
* Author ............ : Facchinetti Tullio |
* Language .......... : C |
* Compiler .......... : GNU C |
* Operative system .. : MS-DOS/HARTIK |
* Creation data ..... : 04/03/2000 |
* Last modify ....... : 19/11/99 |
*/ |
#include <kernel/func.h> |
#include <modules/cabs.h> |
#include <stdio.h> |
#include <drivers/pxc.h> |
#include "demo.h" |
PID image_elab_PID; |
TIME periodo; |
CAB PXC_CAB; |
static CAB frameCAB; // CAB di deposito delle immagini |
static TDataObj current, older; |
// extern in INIT.C |
int img_border = 10; |
int window_width = 40; |
int window_height = 40; |
// a 256 grayscale palette |
WORD gray_palette[256]; |
// the image to be putted on the screen |
WORD converted_image[IMG_COL*IMG_ROW]; |
#ifdef __BLACK_ON_WHITE |
TPixel pix_threshold = 64; |
#else |
TPixel pix_threshold = 243; |
#endif |
// Global for testing!!! |
static char st[50]; |
TIME before; |
TDataObj sequence[N_FRAMES]; |
int top_frame = 0; |
double dist, speed; |
static TPixel *grabber_frame; |
void border_up_function(KEY_EVT key) |
{ |
img_border++; |
} |
void border_down_function(KEY_EVT key) |
{ |
img_border--; |
} |
void threshold_up_function(KEY_EVT key) |
{ |
pix_threshold++; |
sprintf(st, "threshold %4d", pix_threshold); |
mutex_lock(&mutex); |
//grx_text(st, 400, 300, 255, 0); |
mutex_unlock(&mutex); |
} |
void threshold_down_function(KEY_EVT key) |
{ |
pix_threshold--; |
sprintf(st, "threshold %4d", pix_threshold); |
mutex_lock(&mutex); |
//grx_text(st, 400, 300, 255, 0); |
mutex_unlock(&mutex); |
} |
float distance(unsigned int x1, unsigned int y1, |
unsigned int x2, unsigned int y2) |
{ |
return(sqrt(((y2 - y1) * (y2 - y1)) + ((x2 - x1) * (x2 - x1)))); |
} |
char scan_window_frame(TDataObj *data, TPixel *in_frame, |
unsigned int xc, unsigned int yc, int border) |
{ |
unsigned long int offset; |
unsigned int i, j; |
TPixel pix; |
double sum_x = 0.0, sum_y = 0.0; |
unsigned int n_pix = 0; |
int x1, y1, x2, y2; // Must be int!!! |
char found; |
data->x1 = N_COL; |
data->y1 = N_ROW; |
data->x2 = data->y2 = 0; |
data->xb = data->yb = -1; |
data->time_stamp = -1; |
found = 0; |
x1 = MAX_NUM((xc - window_width / 2), (border)); |
y1 = MAX_NUM((yc - window_height / 2), (border)); |
x2 = MIN_NUM((xc + window_width / 2), (N_COL - border)); |
y2 = MIN_NUM((yc + window_height / 2), (N_ROW - border)); |
for (i = y1; i < y2; i++) { |
for (j = x1; j < x2; j++) { |
offset = i * N_COL + j; |
pix = *(in_frame + offset); |
#ifdef __BLACK_ON_WHITE |
// Pixel found (object is black, background is white) |
if (pix < pix_threshold) { |
#else |
// Pixel found (object is white, background is black) |
if (pix > pix_threshold) { |
#endif |
data->time_stamp = sys_gettime(NULL); |
found = 1; |
n_pix++; |
sum_x += j; |
sum_y += i; |
*(in_frame + offset) = 0; |
if (i < data->y1) |
data->y1 = i; |
if (i > data->y2) |
data->y2 = i; |
if (j < data->x1) |
data->x1 = j; |
if (j > data->x2) |
data->x2 = j; |
} else { |
*(in_frame + offset) = 255; |
} |
} |
} |
data->xb = sum_x / n_pix; |
data->yb = sum_y / n_pix; |
return(found); |
} |
char scan_all_frame(TDataObj *data, TPixel *in_frame) |
{ |
unsigned long int offset; |
unsigned int i, j; |
TPixel pix; |
double sum_x = 0.0, sum_y = 0.0; |
unsigned int n_pix = 0; |
char found; |
data->x1 = N_COL; |
data->y1 = N_ROW; |
data->x2 = data->y2 = 0; |
data->xb = data->yb = -1; |
data->time_stamp = -1; |
found = 0; |
// In a single image scanning it performs thresholding and computation |
for (i = img_border; i < N_ROW - img_border; i++) { |
for (j = img_border; j < N_COL - img_border; j++) { |
offset = i * N_COL + j; |
pix = *(in_frame + offset); |
#ifdef __BLACK_ON_WHITE |
// Pixel found (object is black, background is white) |
if (pix < pix_threshold) { |
#else |
// Pixel found (object is white, background is black) |
if (pix > pix_threshold) { |
#endif |
data->time_stamp = sys_gettime(NULL); |
found = 1; |
n_pix++; |
sum_x += j; |
sum_y += i; |
*(in_frame + offset) = 0; |
if (i < data->y1) |
data->y1 = i; |
if (i > data->y2) |
data->y2 = i; |
if (j < data->x1) |
data->x1 = j; |
if (j > data->x2) |
data->x2 = j; |
} else { |
*(in_frame + offset) = 255; |
} |
} |
} |
data->xb = sum_x / n_pix; |
data->yb = sum_y / n_pix; |
return(found); |
} |
void tracking(int *track_x, int *track_y, int time_to) |
{ |
float vx, vy; |
vx = (float)(sequence[top_frame - 1].xb - sequence[top_frame - 2].xb) / |
(float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp); |
vx *= 1000000; |
vy = (float)(sequence[top_frame - 1].yb - sequence[top_frame - 2].yb) / |
(float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp); |
vy *= 1000000; |
(*track_x) = sequence[top_frame - 1].xb + vx * time_to; |
(*track_y) = sequence[top_frame - 1].yb + vy * time_to; |
sprintf(st, "speed = (%5d, %5d) pix/s", (int)vx, (int)vy); |
mutex_lock(&mutex); |
//grx_text(st, 400, 410, 255, 0); |
mutex_unlock(&mutex); |
} |
void put_frame(TPixel *frame) |
{ |
register int i,j,col,row; |
for (i=0; i<IMG_ROW; i++) |
for (j=0; j<IMG_COL; j++) { |
col = (j*(N_COL-1))/(IMG_COL-1); |
row = (i*(N_ROW-1))/(IMG_ROW-1); |
converted_image[i*IMG_COL+j] = gray_palette[*(frame+row*N_COL+col)]; |
} |
mutex_lock(&mutex); |
//grx_putimage(IMG_X, IMG_Y, IMG_X+IMG_COL-1, IMG_Y+IMG_ROW-1, |
// (BYTE *)converted_image); |
mutex_unlock(&mutex); |
} |
TASK elab_image_TASK(void) |
{ |
// register int i, j; |
static unsigned int n_frame = 0; |
char found; |
int pred_x, pred_y; |
// Inizializzazione del task |
frameCAB = PXC_GetCab(); |
grabber_frame = cab_getmes(frameCAB); |
// Executes first time |
found = scan_all_frame(¤t, grabber_frame); |
// found =0; |
if (found) { |
memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
top_frame++; |
} |
cab_unget(frameCAB, grabber_frame); |
task_endcycle(); |
while (1) { |
// before = sys_gettime(NULL); |
n_frame++; |
sprintf(st, "frame n. %5d", n_frame); |
mutex_lock(&mutex); |
//grx_text(st, 400, 290, 255, 0); |
sprintf(st, "top_frame %5d", top_frame); |
//grx_text(st, 400, 270, 255, 0); |
sprintf(st, "found: %d!", found); |
//grx_text(st, 400, 280, 255, 0); |
mutex_unlock(&mutex); |
// Acquisizione immagine corrente |
grabber_frame = cab_getmes(frameCAB); |
// copy current in older |
memcpy(&older, ¤t, sizeof(TDataObj)); |
// Estrazione della nuova trasformata sul frame corrente |
if (found) { |
found = scan_window_frame(¤t, grabber_frame, current.xb, current.yb, img_border); |
} else { |
found = scan_all_frame(¤t, grabber_frame); |
} |
// //grx_putimage(0, 0, N_COL - 1, N_ROW - 1, grabber_frame); |
put_frame(grabber_frame); |
if (found) { |
if (top_frame < N_FRAMES) { |
memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
top_frame++; |
} else { |
top_frame = 0; |
memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
} |
if (top_frame > 1) { |
tracking(&pred_x, &pred_y, 100); |
mutex_lock(&mutex); |
// //grx_disc(IMG_X+(pred_x*2)/3, IMG_Y+(pred_y*2)/3, 3, 127); |
//grx_disc(IMG_X+(current.xb*2)/3, IMG_Y+(current.yb*2)/3, 3, 127); |
//grx_rect(IMG_X+(current.x1*2)/3, IMG_Y+(current.y1*2)/3, |
// IMG_X+(current.x2*2)/3, IMG_Y+(current.y2*2)/3, 127); |
mutex_unlock(&mutex); |
} |
} else { |
top_frame = 0; |
} |
// Release CAB |
cab_unget(frameCAB, grabber_frame); |
// sprintf(st, "durata = %3d ms", (int)(sys_gettime(NULL) - before)/1000); |
// mutex_lock(&mutex); |
// //grx_text(st, 400, 400, 255, 0); |
// mutex_unlock(&mutex); |
task_endcycle(); |
} |
} |
void start_listener(void); |
void framegrabber_close(void *arg) |
{ |
PXC_Close(); |
} |
void init_framegrabber(void) |
{ |
register int i; |
KEY_EVT my_key; |
my_key.ascii = 'a'; |
my_key.scan = KEY_A; |
keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_up_function); |
my_key.ascii = 'z'; |
my_key.scan = KEY_Z; |
keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_down_function); |
my_key.ascii = 's'; |
my_key.scan = KEY_S; |
keyb_hook(my_key, (void (*)(KEY_EVT *))border_up_function); |
my_key.ascii = 'x'; |
my_key.scan = KEY_X; |
keyb_hook(my_key, (void (*)(KEY_EVT *))border_down_function); |
// Aggiusta la palette |
for (i = 0; i < 256; i++) |
gray_palette[i] = rgb16(i,i,i); |
//for (i = 0; i < 256; i++) |
// grx_setcolor(i, i/4, i/4, i/4); |
mutex_lock(&mutex); |
// grx_text("Grabber enabled: no test!", 10, 10, 255, 0); |
// Some messages on screen |
//grx_text("A-Z change threshold", 400, 240, 255, 0); |
//grx_text("S-X change window borders", 400, 250, 255, 0); |
mutex_unlock(&mutex); |
periodo = PXC_Initiate(3); |
PXC_CAB = PXC_GetCab(); |
if (!periodo) { |
//grx_close(); |
cprintf("Problemi nell'inizializzazione del driver\n"); |
sys_end(); |
} else { |
start_listener(); |
} |
sys_atrunlevel(framegrabber_close, NULL, RUNLEVEL_BEFORE_EXIT); |
} |
void start_listener(void) |
{ |
SOFT_TASK_MODEL m_soft; |
soft_task_default_model(m_soft); |
soft_task_def_met(m_soft,IMAGING_WCET); |
soft_task_def_usemath(m_soft); |
soft_task_def_aperiodic(m_soft); |
soft_task_def_period(m_soft,(periodo*3)); |
soft_task_def_group(m_soft,1); |
soft_task_def_ctrl_jet(m_soft); |
image_elab_PID = task_create("imaging", elab_image_TASK, &m_soft, NULL); |
/* task_activate( image_elab_PID); |
PXC_Push_Listener(image_elab_PID,2); |
PXC_Start();*/ |
} |
void start_framegrabber() |
{ |
PXC_Push_Listener(image_elab_PID,2); |
PXC_Start(); |
} |
/demos/trunk/thdemo/readme.txt |
---|
0,0 → 1,17 |
PJ's Thesis demo |
---------------- |
This is my Thesis Demo. The demo works correctly on my PC :-) |
(a celeron 366 Mhz), Probably It will have some problems with other slower PCs. |
The demo simply reads data from the soundblaster, then print the sound wave, compute a power spectrun and display it on the screen. |
Then there is another few tasks that reads data from a framegrabber and track a black image on a white board. |
Moreover, there are some noise balls to test the system in overload conditions, and there are finally a few tasks that displays the system load. |
If you have any problems using this demo, please contact pj@sssup.it |
bye |
Paolo |
/demos/trunk/thdemo/ball2.c |
---|
0,0 → 1,156 |
/*--------------------------------------------------------------*/ |
/* SIMULATION OF JUMPING BALLS */ |
/*--------------------------------------------------------------*/ |
/* CVS $Id: ball2.c,v 1.1.1.1 2002-09-02 09:37:41 pj Exp $ */ |
#include "demo.h" |
#include <kernel/func.h> |
#include <stdlib.h> |
//#include <drivers/glib.h> |
//#include <drivers/keyb.h> |
//#include <math.h> |
#define R 2 /* dimension of a ball */ |
#define G 9.8 /* acceleration of gravity */ |
#define BASE 30 /* position of the floor */ |
#define TOP 80 /* initial height of the ball */ |
#define XMIN 3 /* min position X of the ball */ |
#define XMAX 380 /* max position X of the ball */ |
#define VELX 5. /* horizontal ball velocity */ |
#define VMIN 11. /* velocitÂ… minima per suono */ |
#define ESC 27 /* ASCII code of ESCAPE key */ |
#define MAX_P 50 /* max number of balls */ |
double v0[MAX_P]; /* impact velocity with floor */ |
/*--------------------------------------------------------------*/ |
/* Periodic task for ball simulation */ |
/*--------------------------------------------------------------*/ |
TASK palla(int i) |
{ |
int x, y; /* coordinate grafiche pallina */ |
int ox, oy; /* vecchia posizione pallina */ |
int x0; /* posizione iniziale X pallina */ |
float vx, vy; /* velocitÂ… della pallina */ |
float t, tx; /* variabile temporale */ |
float dt; /* incremento temporale */ |
double arg; /* variabile di appoggio */ |
y = oy = TOP; |
x = ox = x0 = XMIN; |
arg = 2. * G * (float)TOP; |
vy = sqrt(arg); |
vx = VELX;// + rand()%10; |
tx = 0.0; |
t = vy / G; |
dt = 0.1; |
while (1) { |
y = TOP + vy*t - .5*G*t*t; |
x = x0 + vx * tx; |
if (y < BASE) { |
t = 0.0; |
v0[i] = .9 * v0[i]; |
// if (v0[i]<VMIN) { |
// v0[i] = sqrt(2. * G * (float)TOP); |
// v0[i] = sqrt(arg);// * (1-((float)(rand()%20))/100); |
//vx = vx + rand()%5 - 2; |
//tx = 0.0; |
//x0 = x; |
// } |
vy = v0[i]; |
y = TOP + vy*t - .5*G*t*t; |
} |
if (x > XMAX) { |
tx = 0.0; |
x0 = XMAX; |
vx = -vx; |
x = x0 + vx * tx; |
} |
if (x < XMIN) { |
tx = 0.0; |
x0 = XMIN; |
vx = -vx; |
x = x0 + vx * tx; |
} |
y = 480-y; |
mutex_lock(&mutex); |
grx_disc(ox, oy, R, 0); |
grx_disc(x, y, R, i); |
mutex_unlock(&mutex); |
oy = y; ox = x; |
t += dt; |
tx += dt; |
task_endcycle(); |
} |
} |
void ballfun(KEY_EVT *k) |
{ |
static int npc = 0; /* number of tasks created */ |
SOFT_TASK_MODEL mp; |
int r,g,b; |
PID pid; |
if (npc == MAX_P) return; |
r = 64 + rand()%192; |
g = 64 + rand()%192; |
b = 64 + rand()%192; |
soft_task_default_model(mp); |
soft_task_def_level(mp,1); |
soft_task_def_ctrl_jet(mp); |
soft_task_def_arg(mp, rgb16(r,g,b)); |
soft_task_def_group(mp, 1); |
soft_task_def_met(mp, WCET_BALL); |
soft_task_def_period(mp,PERIOD_BALL); |
soft_task_def_usemath(mp); |
pid = task_create("palla", palla, &mp, NULL); |
if (pid != NIL) { |
task_activate(pid); |
npc++; |
} |
} |
/*--------------------------------------------------------------*/ |
/* MAIN process */ |
/*--------------------------------------------------------------*/ |
void init_ball(void) |
{ |
char c; /* character from keyboard */ |
int i; /* pressed number */ |
double arg; /* temporary variable */ |
KEY_EVT k; |
arg = 2. * G * (float)TOP; |
for (i=0; i<MAX_P; i++) v0[i] = sqrt(arg); |
mutex_lock(&mutex); |
grx_rect(XMIN-R-1, 480-TOP-BASE-R-1, XMAX+R+1, 480-BASE+R+1, 14); |
mutex_unlock(&mutex); |
k.flag = 0; |
k.scan = KEY_SPC; |
k.ascii = ' '; |
keyb_hook(k,ballfun); |
} |
/*--------------------------------------------------------------*/ |
/demos/trunk/thdemo/camera4.c |
---|
0,0 → 1,168 |
// framegrabber stuffs |
/* File name ......... : ELABOR.C |
* Project............ : |
* Object ............ : |
* Author ............ : Facchinetti Tullio |
* Language .......... : C |
* Compiler .......... : GNU C |
* Operative system .. : MS-DOS/HARTIK |
* Creation data ..... : 04/03/2000 |
* Last modify ....... : 19/11/99 |
*/ |
#include <kernel/func.h> |
#include <modules/cabs.h> |
#include <stdio.h> |
#include <drivers/pxc.h> |
#include "demo.h" |
PID image_elab_PID; |
TIME periodo; |
CAB PXC_CAB; |
static CAB frameCAB; // CAB di deposito delle immagini |
static TDataObj current, older; |
// extern in INIT.C |
int img_border = 10; |
int window_width = 40; |
int window_height = 40; |
// a 256 grayscale palette |
WORD gray_palette[256]; |
// the image to be putted on the screen |
WORD converted_image[IMG_COL*IMG_ROW]; |
#ifdef __BLACK_ON_WHITE |
TPixel pix_threshold = 64; |
#else |
TPixel pix_threshold = 243; |
#endif |
// Global for testing!!! |
static char st[50]; |
TIME before; |
TDataObj sequence[N_FRAMES]; |
int top_frame = 0; |
double dist, speed; |
static TPixel *grabber_frame; |
void put_frame(TPixel *frame) |
{ |
register int i,j,col,row; |
for (i=1; i<IMG_ROW-1; i++) |
for (j=0; j<IMG_COL; j++) { |
col = (j*(N_COL-1))/(IMG_COL-1); |
row = (i*(N_ROW-1))/(IMG_ROW-1); |
converted_image[i*IMG_COL+j] = gray_palette[*(frame+row*N_COL+col)]; |
} |
for (j=0; j<IMG_COL; j++) { |
converted_image[j] = gray_palette[0]; |
converted_image[(IMG_ROW-1)*IMG_COL+j] = gray_palette[0]; |
} |
mutex_lock(&mutex); |
grx_putimage(IMG_X, IMG_Y, IMG_X+IMG_COL-1, IMG_Y+IMG_ROW-1, |
(BYTE *)converted_image); |
mutex_unlock(&mutex); |
} |
TASK elab_image_TASK(void) |
{ |
// register int i, j; |
static unsigned int n_frame = 0; |
char found; |
int pred_x, pred_y; |
// Inizializzazione del task |
frameCAB = PXC_GetCab(); |
while (1) { |
n_frame++; |
sprintf(st, "frame n. %5d", n_frame); |
mutex_lock(&mutex); |
grx_text(st, 400, 290, 255, 0); |
mutex_unlock(&mutex); |
// Acquisizione immagine corrente |
grabber_frame = cab_getmes(frameCAB); |
put_frame(grabber_frame); |
// Release CAB |
cab_unget(frameCAB, grabber_frame); |
task_endcycle(); |
} |
} |
void start_listener(void); |
void framegrabber_close(void *arg) |
{ |
PXC_Close(); |
} |
void init_framegrabber(void) |
{ |
register int i; |
KEY_EVT my_key; |
// Aggiusta la palette |
for (i = 0; i < 256; i++) |
gray_palette[i] = rgb16(i,i,i); |
periodo = PXC_Initiate(3); |
PXC_CAB = PXC_GetCab(); |
if (!periodo) { |
grx_close(); |
cprintf("Problemi nell'inizializzazione del driver\n"); |
sys_end(); |
} else { |
start_listener(); |
} |
sys_atrunlevel(framegrabber_close, NULL, RUNLEVEL_BEFORE_EXIT); |
} |
void start_listener(void) |
{ |
SOFT_TASK_MODEL m_soft; |
soft_task_default_model(m_soft); |
soft_task_def_met(m_soft,IMAGING_WCET); |
soft_task_def_usemath(m_soft); |
soft_task_def_aperiodic(m_soft); |
soft_task_def_period(m_soft,(periodo)); |
soft_task_def_group(m_soft,1); |
soft_task_def_ctrl_jet(m_soft); |
image_elab_PID = task_create("imaging", elab_image_TASK, &m_soft, NULL); |
/* task_activate( image_elab_PID); |
PXC_Push_Listener(image_elab_PID,2); |
PXC_Start();*/ |
} |
void start_framegrabber() |
{ |
PXC_Push_Listener(image_elab_PID,1); |
PXC_Start(); |
} |