| /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(); |
| } |