/demos/branches/pj/orbit/orbit.c |
---|
0,0 → 1,344 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/* |
------------ |
CVS : $Id: orbit.c,v 1.1.1.1 2002-09-02 09:37:45 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.1.1.1 $ |
Last update: $Date: 2002-09-02 09:37:45 $ |
------------ |
*/ |
/* |
* Copyright (C) 2000 Giorgio Buttazzo and 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 |
* |
*/ |
/****************************************************************/ |
/* SIMULAZIONE DI MASSE CHE ORBITANO */ |
/****************************************************************/ |
#include <kernel/kern.h> |
#include <drivers/glib.h> |
#include <drivers/keyb.h> |
#include <semaphore.h> |
#include <stdlib.h> |
#include <math.h> |
#define MAX 11 /* numero massimo pianeti */ |
#define ESC 27 /* codice ASCII tasto ESCAPE */ |
#define XMAX 639 /* valore massimo coordinata X */ |
#define YMAX 479 /* valore massimo coordinata Y */ |
struct coord { |
double x; |
double y; |
}; |
char fbuf[1000]; // buffer for reading a file |
int flen; // file length |
double mass[MAX]; /* vettore masse pianeti */ |
struct coord pos[MAX]; /* vettore posizioni attuali */ |
struct coord vel[MAX]; /* vettore velocita' iniziali */ |
int XGS, YGS; /* Coordinate centro spazio */ |
int RP, RT; /* raggio pianeta, raggio Terra */ |
int np; /* numero attuale di pianeti */ |
double G; /* Gravitazione Universale */ |
double tick; /* tick di sistema */ |
double delta; /* incremento temporale */ |
double scala; /* fattore grafico di scala */ |
// ------------------------------------------------------- |
// NOTA: %f o %lf significa double e %nf significa float |
// ------------------------------------------------------- |
PID pid; |
int period; /* task period */ |
int wcet; /* task wcet */ |
sem_t mutex; /* semaforo di mutua esclusione */ |
void get_par(void); |
/*--------------------------------------------------------------*/ |
void my_fine(void *arg) |
{ |
grx_close(); |
} |
/*--------------------------------------------------------------*/ |
int inside(int x, int y) |
{ |
return ((x > RP) && (x < XMAX-RP) && |
(y > RP) && (y < YMAX-RP)); |
} |
/*--------------------------------------------------------------*/ |
TASK massa(void *xxx) |
{ |
int i = (int)xxx; /* parametro del task */ |
int gx, gy; /* coordinate grafiche pallina */ |
int ox, oy; /* vecchia posizione pallina */ |
int j; |
int r, col; /* raggio e colore pianeta */ |
double dt; /* incremento temporale */ |
double dist=0.0, dx, dy; /* distanze pianeta-pianeta */ |
double dist0=0.0; /* distanze pianeta-Terra */ |
double x, y; /* posizione del pianeta */ |
double vx, vy; /* velocita' del pianeta */ |
double ax, ay; /* accelerazione del pianeta */ |
double k; /* variabile ausiliaria */ |
double arg; /* variabile di appoggio */ |
x = pos[i].x; y = pos[i].y; |
vx = vel[i].x; vy = vel[i].y; |
ox = XGS + x / scala; |
oy = YGS + y / scala; |
dt = delta; |
do { |
x = pos[i].x; |
y = pos[i].y; |
ax = ay = 0.0; |
for (j=0; j<np; j++) { |
if (j != i) { |
dx = pos[j].x - x; |
dy = pos[j].y - y; |
arg = dx*dx + dy*dy; |
dist = sqrt(arg); |
if (dist < RP*scala) dist = RP*scala; |
k = G * mass[j] / (dist*dist*dist); |
ax += k * dx; |
ay += k * dy; |
} |
if (j == 0) dist0 = dist - (RP+RT)*scala; |
} |
x += vx*dt + 0.5*ax*dt*dt; |
y += vy*dt + 0.5*ay*dt*dt; |
vx += ax * dt; |
vy += ay * dt; |
gx = XGS + x / scala; |
gy = YGS + y / scala; |
r = RP; |
col = i + 1; |
sem_wait(&mutex); |
grx_disc(ox,oy,r,0); |
grx_disc(gx,gy,r,col); |
sem_post(&mutex); |
pos[i].x = x; pos[i].y = y; |
ox = gx; oy = gy; |
task_endcycle(); |
} while ((dist0 > 0) && inside(gx,gy)); |
sem_wait(&mutex); |
grx_disc(ox,oy,r,0); |
grx_disc(XGS,YGS,RT,12); |
grx_circle(XGS,YGS,RT,14); |
sem_post(&mutex); |
return NULL; |
} |
/*--------------------------------------------------------------*/ |
/* MAIN */ |
/*--------------------------------------------------------------*/ |
TASK main() |
{ |
HARD_TASK_MODEL m; |
char c; /* carattere letto da tastiera */ |
set_exchandler_grx(); |
sys_atrunlevel(my_fine, NULL, RUNLEVEL_BEFORE_EXIT); |
sem_init(&mutex,0,1); |
get_par(); |
keyb_getch(BLOCK); |
grx_init(); |
grx_open(640, 480, 8); |
grx_disc(XGS,YGS,RT,12); |
grx_circle(XGS,YGS,RT,14); |
np = 0; |
do { |
if (np < MAX-1) { |
np++; |
hard_task_default_model(m); |
hard_task_def_arg (m, (void *)np); |
hard_task_def_wcet (m, wcet); |
hard_task_def_mit (m, period); |
hard_task_def_usemath (m); |
pid = task_create("massa", massa, &m, NULL); |
if (pid == NIL) { |
grx_close(); |
perror("Could not create task"); |
c = keyb_getch(BLOCK); |
sys_abort(1); |
} |
task_activate(pid); |
} |
c = keyb_getch(BLOCK); |
} while (c != ESC); |
sys_end(); |
return 0; |
} |
/*------------------------------------------------------*/ |
/* file reading */ |
/*------------------------------------------------------*/ |
void read_file(void) |
{ |
int err; |
DOS_FILE *fp; |
fp = DOS_fopen("orbit.dat","r"); |
if (!fp) { |
err = DOS_error(); |
cprintf("Error %d opening myfile.txt...\n", err); |
flen = 0; |
return; |
} |
flen = DOS_fread(&fbuf, 1, 1000, fp); |
cprintf("Read %d bytes from orbit.dat\n", flen); |
DOS_fclose(fp); |
} |
/*------------------------------------------------------*/ |
/* get data from buffer */ |
/*------------------------------------------------------*/ |
void get_par(void) |
{ |
int x = 0; |
int i; |
double vx, vy; |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d", &period); |
cprintf("period = %d\n", period); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d", &wcet); |
cprintf("wcet = %d\n", wcet); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &delta); |
cprintf("delta = %f\n", delta); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &scala); |
cprintf("scala = %f\n", scala); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &G); |
cprintf("G = %20.15f\n", G); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d", &RP); |
cprintf("RP = %d\n", RP); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d", &RT); |
cprintf("RT = %d\n", RT); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d", &XGS); |
cprintf("XGS = %d\n", XGS); |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d", &YGS); |
cprintf("YGS = %d\n", YGS); |
for (i=0; i<MAX; i++) { |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; x++; |
sscanf(&fbuf[x], "%f", &mass[i]); |
while ((fbuf[x] != '\t') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &vx); |
while ((fbuf[x] != '\t') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &vy); |
pos[i].x = vx; pos[i].y = vy; |
while ((fbuf[x] != '\t') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &vx); |
while ((fbuf[x] != '\t') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%f", &vy); |
vel[i].x = vx; vel[i].y = vy; |
cprintf("mass[%d] = %f\t", i, mass[i]); |
cprintf("pos: %f, %f\t", pos[i].x, pos[i].y); |
cprintf("vel: %f, %f\n", vel[i].x, vel[i].y); |
} |
} |
/*--------------------------------------------------------------*/ |
/demos/branches/pj/orbit/initfile.c |
---|
0,0 → 1,124 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/* |
------------ |
CVS : $Id: initfile.c,v 1.1.1.1 2002-09-02 09:37:45 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.1.1.1 $ |
Last update: $Date: 2002-09-02 09:37:45 $ |
------------ |
System initialization file |
This file contains the 2 functions needed to initialize the system. |
These functions register the following levels: |
an EDF (Earliest Deadline First) level |
a RR (Round Robin) level |
a CBS (Costant Bandwidth Server) level |
a Dummy level |
It can accept these task models: |
HARD_TASK_MODEL (wcet+mit) at level 0 |
SOFT_TASK_MODEL (met, period) at level 1 |
NRT_TASK_MODEL at level 2 |
This file is similar to the configuration of kernel/init/hartik3.c |
TICK is set to 0 (one-shot timer is used) |
*/ |
/* |
* 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 |
* |
*/ |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
void read_file(); |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(0); |
CBS_register_level(0, 0); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
read_file(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS kparms = BASE_KEYB; |
HARTPORT_init(); |
keyb_def_ctrlC(kparms, NULL); |
keyb_def_map(kparms,itaMap); |
KEYB_init(&kparms); |
__call_main__(mb); |
return (void *)0; |
} |
/demos/branches/pj/orbit/orbit.dat |
---|
0,0 → 1,21 |
---------------------------------------------------- |
period: 10000 wcet: 100 |
delta: 1. scala: 150. |
G: 6.6e-15 |
r_pianeta: 4 r_Terra: 8 |
X_centro: 320 Y_centro: 240 |
------------------- pianeti ------------------------ |
massa pos.x pos.y vel.x vel.y |
0: 6.0e21 0. 0. 0. 0. |
1: 1.0e21 10000. 8000. -60. 0. |
2: 1.0e8 5000. 0. 0. 80. |
3: 5.0e18 10000. 8000. -50. 0. |
4: 1.0e9 10000. 8000. -40. 20. |
5: 1.0e15 1000. 5000. -80. 0. |
6: 1.0e5 1000. 5000. -80. 0. |
7: 1.0e17 1000. 5000. -80. 0. |
8: 1.0e5 1000. 5000. -80. 0. |
9: 1.0e5 1000. 5000. -80. 0. |
10: 1.0e5 1000. 5000. -80. 0. |
---------------------------------------------------- |
/demos/branches/pj/orbit/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= orbit |
include $(BASE)/config/example.mk |
orbit: |
make -f $(SUBMAKE) APP=orbit INIT= OTHEROBJS="initfile.o" OTHERINCL= |
/demos/branches/pj/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 <ports/rfftw.h> |
/* CAB ports... */ |
CAB cab_windata; /* a window on the last WINDATA_DIM samples */ |
CAB cab_pwrdata; /* the last power spectrum */ |
/* for the cab_windata */ |
typedef struct { |
int start; |
SAMPLE sample[WINDATA_NSAMPLES]; |
} window; |
/* for the cab_pwrdata */ |
typedef struct { |
fftw_real p[PWR_NSAMPLES]; |
} power; |
// win is global... because is used by raw_infun... |
window win; |
/* |
This is the self-buffering function: read the samples and put their mean |
value in a CAB |
*/ |
int raw_infun(void *b) |
{ |
int i; |
char *w; |
SAMPLE *audiobuf = (SAMPLE *)b; |
for (i=0; i<rawdata_nsamples/2; i++) { |
win.sample[win.start] = audiobuf[i]; |
win.start = (win.start+1) % WINDATA_NSAMPLES; |
} |
w = cab_reserve(cab_windata); |
memcpy(w, &win, sizeof(window)); |
cab_putmes(cab_windata,w); |
#if defined(NO_GRX) |
cprintf("X"); //"XXX%d\n",win.sample[win.start]); |
#endif |
return 0; |
} |
void init_rawdata() |
{ |
int i; |
char *w; |
win.start = 0; |
for (i=0; i<WINDATA_NSAMPLES; i++) |
win.sample[i] = 0; |
w = cab_reserve(cab_windata); |
memcpy(w, &win, sizeof(window)); |
cab_putmes(cab_windata,w); |
} |
TASK wave_task() |
{ |
window *p; |
int x,y; |
int s; |
while(1) |
{ |
p = (window *)cab_getmes(cab_windata); |
/* let's print the wave */ |
mutex_lock(&mutex); |
for(x = WAVE_X, s = p->start; |
x < WAVE_X+WAVE_NSAMPLES; |
x++, s = (s+1)%WINDATA_NSAMPLES ) |
{ |
y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE; |
SHORT_CRITICAL_SECTIONS(x); |
grx_plot(x,y,white); |
} |
mutex_unlock(&mutex); |
task_endcycle(); |
/* let's erase the wave */ |
mutex_lock(&mutex); |
for(x = WAVE_X, s = p->start; |
x < WAVE_X+WAVE_NSAMPLES; |
x++, s = (s+1)%WINDATA_NSAMPLES ) |
{ |
y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE; |
SHORT_CRITICAL_SECTIONS(x); |
grx_plot(x,y,black); |
} |
mutex_unlock(&mutex); |
cab_unget(cab_windata,(char *)p); |
} |
} |
rfftw_plan plan; |
void fft_close(void *arg) |
{ |
rfftw_destroy_plan(plan); |
} |
TASK fft_task() |
{ |
fftw_real in[FFT_NSAMPLES], out[FFT_NSAMPLES]; |
power power_spectrum; |
#if defined(NO_GRX) |
fftw_real max = 0.0; |
#endif |
char *m; |
int k, i; |
window *p; |
plan = rfftw_create_plan(FFT_NSAMPLES, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE); |
sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT); |
while(1) |
{ |
/* Let's prepare the intput FFT data */ |
p = (window *)cab_getmes(cab_windata); |
for (k = 0, i = p->start; |
k < FFT_NSAMPLES; |
k++, i = (i+1)%WINDATA_NSAMPLES) |
in[k] = p->sample[i]/FFT_SCALE; |
cab_unget(cab_windata,(char *)p); |
/* zero-padding if needed */ |
for (k=WINDATA_NSAMPLES; k < FFT_NSAMPLES; k++) |
in[k] = 0.0; |
rfftw_one(plan, in, out); |
/* power spectrum computation */ |
power_spectrum.p[0] = out[0]*out[0]; /* DC component */ |
for (k = 1; k < PWR_NSAMPLES; ++k) /* (k < N/2 rounded up) */ |
power_spectrum.p[k] = out[k]*out[k] + out[FFT_NSAMPLES-k]*out[FFT_NSAMPLES-k]; |
if (FFT_NSAMPLES % 2 == 0) /* N is even */ |
power_spectrum.p[FFT_NSAMPLES/2] = out[FFT_NSAMPLES/2]*out[FFT_NSAMPLES/2]; /* Nyquist freq. */ |
m = cab_reserve(cab_pwrdata); |
memcpy(m, &power_spectrum, sizeof(power)); |
cab_putmes(cab_pwrdata,m); |
#if defined(NO_GRX) |
max = 0.0; |
for (k=0; k<PWR_NSAMPLES; k++) |
if (power_spectrum.p[k] > max) |
max = power_spectrum.p[k]; |
//cprintf("%f %f\n",max,(max / EQU_SCALE) ); |
#endif |
task_endcycle(); |
} |
} |
/* structure is like the wave task... */ |
TASK equ_task() |
{ |
power *p; |
int x[PWR_NSAMPLES]; |
int y; |
int s; |
int r,g,b; |
while(1) |
{ |
p = (power *)cab_getmes(cab_pwrdata); |
/* print the lines */ |
mutex_lock(&mutex); |
for(y = EQU_Y, s = 0; |
s < EQU_NSAMPLES; |
y++, s++ ) |
{ |
x[s] = (int)(p->p[s] / EQU_SCALE); |
if (x[s] > EQU_HEIGHT) |
x[s] = EQU_HEIGHT; |
x[s] = EQU_X - x[s]; |
#if defined(EQU_SHADE) |
/* like the task equ2d... */ |
r = (int)(p->p[s] / EQU2D_SCALE); |
if (r > EQU2D_CLIP) |
r = EQU2D_CLIP; |
if (r< 64) g = r * 4; |
else if (r<128) g = (128-r) * 4; |
else g = 0; |
if (r<128) b = 0; |
else if (r<192) b = (r-128) * 4; |
else b = (256-r) * 4; |
SHORT_CRITICAL_SECTIONS(y); |
grx_line(EQU_X,y,x[s],y,rgb16(r,g,b)); |
#else |
SHORT_CRITICAL_SECTIONS(y); |
grx_line(EQU_X,y,x[s],y,white); |
#endif |
} |
mutex_unlock(&mutex); |
task_endcycle(); |
/* erase the lines... */ |
mutex_lock(&mutex); |
for(y = EQU_Y, s = 0; |
s < EQU_NSAMPLES; |
y++, s++ ) |
{ |
SHORT_CRITICAL_SECTIONS(y); |
grx_line(EQU_X,y,x[s],y,black); |
} |
mutex_unlock(&mutex); |
cab_unget(cab_pwrdata,(char *)p); |
} |
} |
TASK equ2d_task() |
{ |
power *p; |
int pwrint; |
int x = 0; |
int y,s; |
int r,g,b; |
while(1) |
{ |
p = (power *)cab_getmes(cab_pwrdata); |
/* print the line */ |
mutex_lock(&mutex); |
for(y = EQU2D_Y, s = 0; |
s < EQU2D_NSAMPLES; |
y++, s++ ) |
{ |
pwrint = (int)(p->p[s] / EQU2D_SCALE); |
if (pwrint > EQU2D_CLIP) |
pwrint = EQU2D_CLIP; |
r = pwrint; |
if (pwrint< 64) g = pwrint * 4; |
else if (pwrint<128) g = (128-pwrint) * 4; |
else g = 0; |
if (pwrint<128) b = 0; |
else if (pwrint<192) b = (pwrint-128) * 4; |
else b = (256-pwrint) * 4; |
SHORT_CRITICAL_SECTIONS(y); |
grx_plot(EQU2D_X+x,y,rgb16(r,g,b)); |
} |
x = (x+1) % EQU2D_WIDTH; |
grx_line(EQU2D_X+x,EQU2D_Y,EQU2D_X+x,EQU2D_Y+EQU2D_NSAMPLES-1,white); |
mutex_unlock(&mutex); |
cab_unget(cab_pwrdata,(char *)p); |
task_endcycle(); |
} |
} |
void init_fftplay(int freq) |
{ |
SOFT_TASK_MODEL m3, m4, m5, m6; |
PID p3,p4,p5,p6; |
cab_windata = cab_create("windata", sizeof(window), 5); |
cab_pwrdata = cab_create("pwr", sizeof(power), 5); |
/* Init the sound lib */ |
sound_init((rawdata_nsamples * sizeof(SAMPLE)), NULL); |
sound_info(); |
/* Init the data used by the raw_infun */ |
init_rawdata(); |
/* Start the self-buffering sampling operation */ |
sound_setfun(raw_infun, (int (*)(void *))-1); |
sound_sample(NULL, freq, 0, DMA_OP | PCM16 | MYFUN, NULL); |
soft_task_default_model(m3); |
soft_task_def_level(m3,1); |
soft_task_def_period(m3, PERIOD_WAVE); |
soft_task_def_met(m3, WCET_WAVE); |
soft_task_def_ctrl_jet(m3); |
soft_task_def_group(m3, 1); |
p3 = task_create("wave", wave_task, &m3, NULL); |
if (p3 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <wave>\n"); |
ll_abort(54); |
sys_end(); |
} |
soft_task_default_model(m4); |
soft_task_def_level(m4,1); |
soft_task_def_period(m4, PERIOD_FFT); |
soft_task_def_met(m4, WCET_FFT); |
soft_task_def_group(m4, 1); |
soft_task_def_stack(m4,32*1024); |
soft_task_def_usemath(m4); |
soft_task_def_ctrl_jet(m4); |
p4 = task_create("fft", fft_task, &m4, NULL); |
if (p4 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <fft>\n"); |
ll_abort(54); |
sys_end(); |
} |
soft_task_default_model(m5); |
soft_task_def_level(m5,1); |
soft_task_def_period(m5, PERIOD_EQU); |
soft_task_def_met(m5, WCET_EQU); |
soft_task_def_group(m5, 1); |
soft_task_def_stack(m5,32*1024); |
soft_task_def_usemath(m5); |
soft_task_def_ctrl_jet(m5); |
p5 = task_create("equ", equ_task, &m5, NULL); |
if (p5 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <equ>\n"); |
ll_abort(54); |
perror("FFTPlay: Could not create task <equ>\n"); |
sys_end(); |
} |
soft_task_default_model(m6); |
soft_task_def_level(m6,1); |
soft_task_def_period(m6, PERIOD_EQU2D); |
soft_task_def_met(m6, WCET_EQU2D); |
soft_task_def_group(m6, 1); |
soft_task_def_stack(m6,32*1024); |
soft_task_def_usemath(m6); |
soft_task_def_ctrl_jet(m6); |
p6 = task_create("equ2D", equ2d_task, &m6, NULL); |
if (p6 == -1) { |
grx_close(); |
perror("FFTPlay: Could not create task <equ2d>\n"); |
ll_abort(54); |
perror("FFTPlay: Could not create task <equ2D>\n"); |
sys_end(); |
} |
} |
void scenario_fftplay(int f) |
{ |
int i,y; |
char s[50]; |
grx_line(0,WAVE_Y-WAVE_HEIGHT-1,383,WAVE_Y-WAVE_HEIGHT-1,red); |
grx_line(0,WAVE_Y+WAVE_HEIGHT+1,383,WAVE_Y+WAVE_HEIGHT+1,red); |
grx_line(0,EQU_Y-11 ,383,EQU_Y-11 ,red); |
/* lines near the frequencies */ |
grx_line(EQU_X +1,EQU_Y,EQU_X +1,EQU_Y+EQU_NSAMPLES-1,red); |
grx_line(EQU2D_X-1,EQU_Y,EQU2D_X-1,EQU_Y+EQU_NSAMPLES-1,red); |
for (i=0; i<SCENARIO_NLABEL; i++) |
{ |
y = (i*EQU_NSAMPLES)/(SCENARIO_NLABEL-1); |
if (i == SCENARIO_NLABEL-1) y--; |
grx_line(EQU_X +1,EQU_Y+y,EQU_X +4,EQU_Y+y,red); |
grx_line(EQU2D_X-1,EQU_Y+y,EQU2D_X-4,EQU_Y+y,red); |
itoa((i*f)/(SCENARIO_NLABEL-1),s); |
grx_text(s,EQU_X+20,EQU_Y+y-8,white,black); |
} |
grx_text("Power Spectrum" , 0, EQU_Y-21, rgb16(0,0,255), black); |
grx_text("Power Spectrum Story", EQU2D_X+EQU2D_WIDTH-160, EQU_Y-21, rgb16(0,0,255), black); |
grx_text("Waveform" , 0, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black); |
} |
void compute_params(int *freq,WORD *nsamp) |
{ |
if (*freq< 2000) |
{ |
cprintf("WARNING: frequency less than 2000Hz\n ---> frequency set to 2000Hz\n"); |
*freq = 2000; |
} |
if (*freq<= 8000) { *nsamp = 64; return; } //128 |
if (*freq<=16000) { *nsamp = 64; return; } //256 |
if (*freq<=24000) { *nsamp = 64; return; } //512 |
if (*freq>48000) |
{ |
cprintf("WARNING: frequency greather than 48000Hz\n ---> frequency set to 48000Hz\n"); |
*freq = 48000; |
} |
if (*freq<=48000) { *nsamp = 64; return; } //1024 |
} |
/demos/branches/pj/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/branches/pj/thdemo/demo.c |
---|
0,0 → 1,294 |
/* 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 demo_exc_handler(int signo, siginfo_t *info, void *extra) |
{ |
struct timespec t; |
grx_close(); |
/* Default action for an kern exception is */ |
kern_cli(); |
ll_gettime(TIME_EXACT, &t), |
kern_printf("\nHartik Exception raised!!!" |
"\nTime (s:ns) :%ld:%ld" |
"\nException number:%d" |
"\nPID :%d\n", |
t.tv_sec, t.tv_nsec, info->si_value.sival_int, |
info->si_task); |
sys_end(); |
// ll_abort(5); |
} |
void my_close(void *arg) |
{ |
grx_close(); |
// sys_status(3); |
} |
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; |
struct sigaction action; |
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); |
/* Init the standard Hartik exception handler */ |
/* Set the signal action */ |
action.sa_flags = SA_SIGINFO; |
action.sa_sigaction = demo_exc_handler; |
action.sa_handler = 0; |
sigfillset(&action.sa_mask); /* we block all the other signals... */ |
if (sigaction(SIGHEXC, &action, NULL) == -1) { |
perror("Error initializing signals..."); |
sys_end(); |
} |
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/branches/pj/thdemo/fftplay.c |
---|
0,0 → 1,770 |
/* Project: HARTIK 3.0 */ |
/* Description: Hard Real TIme Kernel for 386 & higher machines */ |
/* Author: Paolo Gai <pgai@rsk.it> */ |
/* Advanced Linux Sound Architecture (ALSA) */ |
/* Copyright (c) by Jaroslav Kysela <perex@jcu.cz> */ |
/* Luca Abeni */ |
/* FFTW by M. Frigo and S. G. Johnson */ |
/* Date: 08/09/1999 */ |
/* File: fftplay.c */ |
/* Revision: 1.00 (Kernel 0.1.4; Library 0.0.9; Util 0.0.4) */ |
/* |
FFTPlay 1.0 |
----------- |
This application reads data from the audio microphone and then put it |
to the screen in a graphical oscilloscope-like form (both standard Hartik |
SB driver and Alsa driver can be used). |
The application also calculate a FFT on the latest values, and displays |
the power spectrum in tho ways, one like an equalizator, and the other in a |
2D form. |
A resolution of 1024x768, 64K colors is used. |
The task set is composed by these tasks and functions: |
Self buffering functions (SB only) |
---------------------------------- |
this function takes the samples read by the mic and then makes a window |
with the last WINDATA_NSAMPLES samples. The window is then put in |
the CAB windata. |
Task raw and task mixer (ALSA only) |
----------------------------------- |
These tasks are used with the Alsa driver; because it doesn't support the |
self-buffering mode, we have to do a forever cycle in witch we have to read |
all the data. Then the data are managed like the self-buffering functions of |
the SB driver or sent to the mixer task (with a STREAM mailbox) which makes |
the window. |
(This approach is not good for realtime...) |
Task wave |
--------- |
This task read the last window and then put it on the screen in a wave form |
on the top of the screen. |
The task's period is set to 40 ms (25 fps). |
Task fft |
-------- |
This task read the last window and then it computes the FFT. |
With the FFT data it computes the power spectrum, whitch is sent to the |
CAB pwrdata. |
The task's period is set to 10 ms (good for the 2D story task). |
This task is the only Hard Task. |
Task equ |
-------- |
This task read the last power spectrum and displays it in a graphical |
form, like a hi-fi equalizator. |
The Histograms can be white or coloured like the equ2D task (see EQU_SHADE) |
The task's period is set to 40 ms (25 fps). |
Task equ2D |
---------- |
This task read the lasf power spectrum and displays it in a graphical |
one-line form. Each pixel is a power coefficient, and its colour shade |
from black (no power) to red (high power) passing through green and blue. |
The task display the last EQU2D_WIDTH power lines. |
The task's period is set to 10 ms (good for the 2D story task). |
**************************************************************************** |
TASK LOAD |
**************************************************************************** |
period wcet |
task tick (ms) us % |
-------------------------------------------------------- |
sound driver 24->3 12->1.5 200 0.016->0.133 |
wave 80 40 11500 0.2875 |
fft 20 10 3000 0.3000 |
equ 80 40 7000 0.1750 |
equ2D 20 10 500 0.0500 |
------------- |
0.812 (last 4) |
*/ |
#include <ll/ll.h> |
#include <kernel/types.h> |
#include <kernel/model.h> |
#include <kernel/func.h> |
#include <modules/cabs.h> |
#include <string.h> |
#include <stdlib.h> |
#include <semaphore.h> |
#include <drivers/keyb.h> |
#include <drivers/crtwin.h> |
#include <drivers/glib.h> |
#include <drivers/sound.h> |
#include <ports/rfftw.h> |
/* now the load constants... */ |
#define WCET_WAVE 11500 |
#define WCET_FFT 3000 |
#define WCET_EQU 500 |
#define WCET_EQU2D 500 |
#define PERIOD_WAVE 40000 |
#define PERIOD_FFT 10000 |
#define PERIOD_EQU 40000 |
#define PERIOD_EQU2D 10000 |
/* define if shorts critical sections wanted */ |
#define SHORT_CRITICAL_SECTIONS(x) \ |
if (!((x)%64)) \ |
{ \ |
sem_post(&mutex); \ |
sem_wait(&mutex); \ |
} |
/* define if you want NRT or SOFT... */ |
#define TASK_TYPE SOFT |
//#define TASK_TYPE NRT |
/* Only 4 Debug... */ |
/*#define NO_GRX */ |
/* Samples are 16-bit signed integers */ |
typedef short SAMPLE; |
#define MAX_SAMPLE 32768 |
/* Informations about the sampling rate and buffers */ |
WORD rawdata_nsamples; |
WORD rawdata_buffer_size; |
WORD rawdata_freq; |
/* Numbers of samples of the sample window */ |
#define WINDATA_NSAMPLES 1024 |
/* task WAVE */ |
/* the point (wave_x,wave_y) is on the center left of the area... */ |
#define WAVE_NSAMPLES 1024 |
#define WAVE_X 0 |
#define WAVE_Y 130 |
#define WAVE_HEIGHT 80 |
/* task FFT */ |
#define FFT_NSAMPLES WINDATA_NSAMPLES |
#define PWR_NSAMPLES (FFT_NSAMPLES/2+1) |
/* task EQU */ |
/* the point (equ_x, equ_y) is the top right corner */ |
#define EQU_NSAMPLES PWR_NSAMPLES |
#define EQU_X 170 |
#define EQU_Y 255 |
#define EQU_HEIGHT 170 |
#define EQU_SHADE |
/* task EQU2D */ |
/* the point (equ2d_x, equ2d_y) is the top left corner */ |
#define EQU2D_NSAMPLES EQU_NSAMPLES |
#define EQU2D_X 255 |
#define EQU2D_Y EQU_Y |
#define EQU2D_WIDTH 768 |
#define EQU2D_CLIP 255 |
/* scenario */ |
#define SCENARIO_NLABEL 16 |
/* Scale factors */ |
#define FFT_SCALE (16384.0) |
#define EQU_SCALE (32.0) |
#define EQU2D_SCALE (8.0) |
//#define EQU_SCALE (64.0) |
//#define EQU2D_SCALE (16.0) |
/* CAB ports... */ |
CAB cab_windata; /* a window on the last WINDATA_DIM samples */ |
CAB cab_pwrdata; /* the last power spectrum */ |
/* for the cab_windata */ |
typedef struct { |
int start; |
SAMPLE sample[WINDATA_NSAMPLES]; |
} window; |
/* for the cab_pwrdata */ |
typedef struct { |
fftw_real p[PWR_NSAMPLES]; |
} power; |
/* graphic mutex... */ |
sem_t mutex; |
// win is global... because is used by raw_infun... |
window win; |
/* useful colors... */ |
int white; |
int black; |
int red; |
static void version( void ) |
{ |
cprintf( "Hartik FFT Play 1.0\n" ); |
cprintf( "-----------------------\n" ); |
cprintf( "by Paolo Gai 1999\n" ); |
cprintf( " <pj@hartik.sssup.it>\n" ); |
cprintf( "-----------------------\n" ); |
} |
void reverse(char s[]) |
{ |
int c, i, j; |
for (i = 0, j = strlen(s)-1; i<j; i++, j--) |
{ |
c = s[i]; |
s[i] = s[j]; |
s[j] = c; |
} |
} |
char * itoa(int n, char *s) |
{ |
int i, sign; |
if ((sign = n) < 0) |
n = -n; |
i = 0; |
do |
{ |
s[i++] = n % 10 + '0'; |
} while ((n /= 10) > 0); |
if (sign < 0) |
s[i++] = '-'; |
s[i] = 0; |
reverse(s); |
return s; |
} |
/* |
This is the self-buffering function: read the samples and put their mean |
value in a CAB |
*/ |
int raw_infun(void *b) |
{ |
int i; |
char *w; |
SAMPLE *audiobuf = (SAMPLE *)b; |
for (i=0; i<rawdata_nsamples/2; i++) { |
win.sample[win.start] = audiobuf[i]; |
win.start = (win.start+1) % WINDATA_NSAMPLES; |
} |
w = cab_reserve(cab_windata); |
memcpy(w, &win, sizeof(window)); |
cab_putmes(cab_windata,w); |
#if defined(NO_GRX) |
cprintf("X"); //"XXX%d\n",win.sample[win.start]); |
#endif |
return 0; |
} |
void init_rawdata() |
{ |
int i; |
char *w; |
win.start = 0; |
for (i=0; i<WINDATA_NSAMPLES; i++) |
win.sample[i] = 0; |
w = cab_reserve(cab_windata); |
memcpy(w, &win, sizeof(window)); |
cab_putmes(cab_windata,w); |
} |
TASK wave_task() |
{ |
window *p; |
int x,y; |
int s; |
while(1) |
{ |
p = (window *)cab_getmes(cab_windata); |
/* let's print the wave */ |
sem_wait(&mutex); |
for(x = WAVE_X, s = p->start; |
x < WAVE_X+WAVE_NSAMPLES; |
x++, s = (s+1)%WINDATA_NSAMPLES ) |
{ |
y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE; |
SHORT_CRITICAL_SECTIONS(x); |
grx_plot(x,y,white); |
} |
sem_post(&mutex); |
task_endcycle(); |
/* let's erase the wave */ |
sem_wait(&mutex); |
for(x = WAVE_X, s = p->start; |
x < WAVE_X+WAVE_NSAMPLES; |
x++, s = (s+1)%WINDATA_NSAMPLES ) |
{ |
y = WAVE_Y + (WAVE_HEIGHT * p->sample[s]) / MAX_SAMPLE; |
SHORT_CRITICAL_SECTIONS(x); |
grx_plot(x,y,black); |
} |
sem_post(&mutex); |
cab_unget(cab_windata,(char *)p); |
} |
} |
rfftw_plan plan; |
void fft_close(void *arg) |
{ |
rfftw_destroy_plan(plan); |
} |
TASK fft_task() |
{ |
fftw_real in[FFT_NSAMPLES], out[FFT_NSAMPLES]; |
power power_spectrum; |
#if defined(NO_GRX) |
fftw_real max = 0.0; |
#endif |
char *m; |
int k, i; |
window *p; |
plan = rfftw_create_plan(FFT_NSAMPLES, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE); |
sys_atrunlevel(fft_close, NULL, RUNLEVEL_BEFORE_EXIT); |
while(1) |
{ |
/* Let's prepare the intput FFT data */ |
p = (window *)cab_getmes(cab_windata); |
for (k = 0, i = p->start; |
k < FFT_NSAMPLES; |
k++, i = (i+1)%WINDATA_NSAMPLES) |
in[k] = p->sample[i]/FFT_SCALE; |
cab_unget(cab_windata,(char *)p); |
/* zero-padding if needed */ |
for (k=WINDATA_NSAMPLES; k < FFT_NSAMPLES; k++) |
in[k] = 0.0; |
rfftw_one(plan, in, out); |
/* power spectrum computation */ |
power_spectrum.p[0] = out[0]*out[0]; /* DC component */ |
for (k = 1; k < PWR_NSAMPLES; ++k) /* (k < N/2 rounded up) */ |
power_spectrum.p[k] = out[k]*out[k] + out[FFT_NSAMPLES-k]*out[FFT_NSAMPLES-k]; |
if (FFT_NSAMPLES % 2 == 0) /* N is even */ |
power_spectrum.p[FFT_NSAMPLES/2] = out[FFT_NSAMPLES/2]*out[FFT_NSAMPLES/2]; /* Nyquist freq. */ |
m = cab_reserve(cab_pwrdata); |
memcpy(m, &power_spectrum, sizeof(power)); |
cab_putmes(cab_pwrdata,m); |
#if defined(NO_GRX) |
max = 0.0; |
for (k=0; k<PWR_NSAMPLES; k++) |
if (power_spectrum.p[k] > max) |
max = power_spectrum.p[k]; |
//cprintf("%f %f\n",max,(max / EQU_SCALE) ); |
#endif |
task_endcycle(); |
} |
} |
/* structure is like the wave task... */ |
TASK equ_task() |
{ |
power *p; |
int x[PWR_NSAMPLES]; |
int y; |
int s; |
int r,g,b; |
while(1) |
{ |
p = (power *)cab_getmes(cab_pwrdata); |
/* print the lines */ |
sem_wait(&mutex); |
for(y = EQU_Y, s = 0; |
s < EQU_NSAMPLES; |
y++, s++ ) |
{ |
x[s] = (int)(p->p[s] / EQU_SCALE); |
if (x[s] > EQU_HEIGHT) |
x[s] = EQU_HEIGHT; |
x[s] = EQU_X - x[s]; |
#if defined(EQU_SHADE) |
/* like the task equ2d... */ |
r = (int)(p->p[s] / EQU2D_SCALE); |
if (r > EQU2D_CLIP) |
r = EQU2D_CLIP; |
if (r< 64) g = r * 4; |
else if (r<128) g = (128-r) * 4; |
else g = 0; |
if (r<128) b = 0; |
else if (r<192) b = (r-128) * 4; |
else b = (256-r) * 4; |
SHORT_CRITICAL_SECTIONS(y); |
grx_line(EQU_X,y,x[s],y,rgb16(r,g,b)); |
#else |
SHORT_CRITICAL_SECTIONS(y); |
grx_line(EQU_X,y,x[s],y,white); |
#endif |
} |
sem_post(&mutex); |
task_endcycle(); |
/* erase the lines... */ |
sem_wait(&mutex); |
for(y = EQU_Y, s = 0; |
s < EQU_NSAMPLES; |
y++, s++ ) |
{ |
SHORT_CRITICAL_SECTIONS(y); |
grx_line(EQU_X,y,x[s],y,black); |
} |
sem_post(&mutex); |
cab_unget(cab_pwrdata,(char *)p); |
} |
} |
TASK equ2d_task() |
{ |
power *p; |
int pwrint; |
int x = 0; |
int y,s; |
int r,g,b; |
while(1) |
{ |
p = (power *)cab_getmes(cab_pwrdata); |
/* print the line */ |
sem_wait(&mutex); |
for(y = EQU2D_Y, s = 0; |
s < EQU2D_NSAMPLES; |
y++, s++ ) |
{ |
pwrint = (int)(p->p[s] / EQU2D_SCALE); |
if (pwrint > EQU2D_CLIP) |
pwrint = EQU2D_CLIP; |
r = pwrint; |
if (pwrint< 64) g = pwrint * 4; |
else if (pwrint<128) g = (128-pwrint) * 4; |
else g = 0; |
if (pwrint<128) b = 0; |
else if (pwrint<192) b = (pwrint-128) * 4; |
else b = (256-pwrint) * 4; |
SHORT_CRITICAL_SECTIONS(y); |
grx_plot(EQU2D_X+x,y,rgb16(r,g,b)); |
} |
x = (x+1) % EQU2D_WIDTH; |
grx_line(EQU2D_X+x,EQU2D_Y,EQU2D_X+x,EQU2D_Y+EQU2D_NSAMPLES,white); |
sem_post(&mutex); |
cab_unget(cab_pwrdata,(char *)p); |
task_endcycle(); |
} |
} |
TASK prova_task() |
{ |
window *p; |
while(1) |
{ |
p = (window *)cab_getmes(cab_windata); |
cprintf("%d %d %d\t", p->start /*sample[0]*/,p->sample[1],p->sample[2]); |
cab_unget(cab_windata,(char *)p); |
task_endcycle(); |
} |
} |
void scenario(int f) |
{ |
int i,y; |
char s[6]; |
grx_line(0,WAVE_Y-WAVE_HEIGHT-1,1023,WAVE_Y-WAVE_HEIGHT-1,red); |
grx_line(0,WAVE_Y+WAVE_HEIGHT+1,1023,WAVE_Y+WAVE_HEIGHT+1,red); |
grx_line(0,EQU_Y-11 ,1023,EQU_Y-11 ,red); |
/* lines near the frequencies */ |
grx_line(EQU_X +1,EQU_Y,EQU_X +1,EQU_Y+EQU_NSAMPLES,red); |
grx_line(EQU2D_X-1,EQU_Y,EQU2D_X-1,EQU_Y+EQU_NSAMPLES,red); |
for (i=0; i<SCENARIO_NLABEL; i++) |
{ |
y = (i*EQU_NSAMPLES)/(SCENARIO_NLABEL-1); |
if (i == SCENARIO_NLABEL-1) y--; |
grx_line(EQU_X +1,EQU_Y+y,EQU_X +10,EQU_Y+y,red); |
grx_line(EQU2D_X-1,EQU_Y+y,EQU2D_X-10,EQU_Y+y,red); |
itoa((i*f)/(SCENARIO_NLABEL-1),s); |
grx_text(s,EQU_X+20,EQU_Y+y-8,white,black); |
} |
grx_text("FFTPlay 1.0 - by Paolo Gai 1999 <pj@hartik.sssup.it>", 0,8, rgb16(0,255,0), black ); |
grx_text("...press ENTER key to exit..." , 0,24, rgb16(0,255,0), black ); |
grx_text("FFT Power Spectrum", 0 , EQU_Y-21, rgb16(0,0,255), black); |
grx_text("FFT Power Story", EQU2D_X+16, EQU_Y-21, rgb16(0,0,255), black); |
grx_text("Waveform" , 0, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black); |
} |
void compute_params(int *freq,WORD *nsamp, WORD *per) |
{ |
if (*freq< 2000) |
{ |
cprintf("WARNING: frequency less than 2000Hz\n ---> frequency set to 2000Hz\n"); |
*freq = 2000; |
} |
if (*freq<= 8000) { *nsamp = 128; *per = 10; return; } |
if (*freq<=16000) { *nsamp = 256; *per = 10; return; } |
if (*freq<=24000) { *nsamp = 512; *per = 10; return; } |
if (*freq>48000) |
{ |
cprintf("WARNING: frequency greather than 48000Hz\n ---> frequency set to 48000Hz\n"); |
*freq = 48000; |
} |
if (*freq<=48000) { *nsamp = 1024;*per = 10; return; } |
} |
void my_close(void *arg) |
{ |
grx_close(); |
sys_status(3); |
} |
void endfun(KEY_EVT *k) |
{ |
cprintf("Ctrl-Brk pressed! Ending...\n"); |
sys_end(); |
} |
int main(int argc, char **argv) |
{ |
int modenum; |
int f; |
/* irq period... */ |
WORD period; |
KEY_EVT k; |
SOFT_TASK_MODEL m3, m4, m5, m6; |
PID p3,p4,p5,p6; |
version(); |
if (argc == 1) |
{ |
cprintf("type x fftplay <freq>"); |
return 0; |
} |
f = atoi(argv[1]); |
compute_params(&f,&rawdata_nsamples,&period); |
keyb_set_map(itaMap); |
k.flag = CNTR_BIT; |
k.scan = KEY_C; |
k.ascii = 'c'; |
keyb_hook(k,endfun); |
k.flag = CNTL_BIT; |
k.scan = KEY_C; |
k.ascii = 'c'; |
keyb_hook(k,endfun); |
cab_windata = cab_create("windata", sizeof(window), 4); |
cab_pwrdata = cab_create("pwr", sizeof(power), 4); |
/* Init the sound lib */ |
sound_init((rawdata_nsamples * sizeof(SAMPLE)), NULL); |
sound_info(); |
/* Init the data used by the raw_infun */ |
init_rawdata(); |
/* Start the self-buffering sampling operation */ |
sound_setfun(raw_infun, (int (*)(void *))-1); |
sound_sample(NULL, f, 0, DMA_OP | PCM16 | MYFUN, NULL); |
cprintf("Press Enter..."); |
while (keyb_getchar() != 13); |
sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
#if !defined(NO_GRX) |
grx_init(); |
modenum = grx_getmode(1024, 768, 16); |
grx_setmode(modenum); |
/* init the graphic mutex */ |
sem_init(&mutex, 0, 1); |
/* useful colors ... */ |
white = rgb16(255,255,255); |
black = rgb16(0,0,0); |
red = rgb16(255,0,0); |
scenario(f/2); |
#endif |
#if !defined(NO_GRX) |
soft_task_default_model(m3); |
soft_task_def_period(m3, PERIOD_WAVE); |
soft_task_def_met(m3, WCET_WAVE); |
soft_task_def_group(m3, 1); |
p3 = task_create("wave", wave_task, &m3, NULL); |
if (p3 == -1) { |
perror("FFTPlay: Could not create task <wave>\n"); |
sys_end(); |
} |
#endif |
soft_task_default_model(m4); |
soft_task_def_period(m4, PERIOD_FFT); |
soft_task_def_met(m4, WCET_FFT); |
soft_task_def_group(m4, 1); |
soft_task_def_stack(m4,32*1024); |
soft_task_def_usemath(m4); |
p4 = task_create("fft", fft_task, &m4, NULL); |
if (p4 == -1) { |
perror("FFTPlay: Could not create task <fft>\n"); |
sys_end(); |
} |
#if !defined(NO_GRX) |
soft_task_default_model(m5); |
soft_task_def_period(m5, PERIOD_EQU); |
soft_task_def_met(m5, WCET_EQU); |
soft_task_def_group(m5, 1); |
soft_task_def_stack(m5,32*1024); |
soft_task_def_usemath(m5); |
p5 = task_create("equ", equ_task, &m5, NULL); |
if (p5 == -1) { |
perror("FFTPlay: Could not create task <equ>\n"); |
sys_end(); |
} |
#endif |
#if !defined(NO_GRX) |
soft_task_default_model(m6); |
soft_task_def_period(m6, PERIOD_EQU2D); |
soft_task_def_met(m6, WCET_EQU2D); |
soft_task_def_group(m6, 1); |
soft_task_def_stack(m6,32*1024); |
soft_task_def_usemath(m6); |
p6 = task_create("equ2D", equ2d_task, &m5, NULL); |
if (p6 == -1) { |
perror("FFTPlay: Could not create task <equ2D>\n"); |
sys_end(); |
} |
#else |
/* Start the prova task */ |
//task_def_wcet(m6,1000); |
//task_activate(task_create("prova",prova_task,TASK_TYPE,PERIODIC,200,&m6)); |
#endif |
group_activate(1); |
/* Wait until the user get bored */ |
while (keyb_getchar() != 13); |
sys_end(); |
return 0; |
} |
/demos/branches/pj/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/branches/pj/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/branches/pj/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/branches/pj/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/branches/pj/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/branches/pj/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/branches/pj/thdemo/makefile |
---|
0,0 → 1,23 |
# |
# |
# |
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" OTHERINCL= |
/demos/branches/pj/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/branches/pj/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(); |
} |
/demos/branches/pj/simcity/keyboard.c |
---|
0,0 → 1,49 |
/* ------------------ */ |
/* Keyboard handler */ |
/* ------------------ */ |
#include <drivers/keyb.h> |
#include "include/keyfunct.h" |
void keyb_h() { |
KEY_EVT k; |
keyb_set_map(itaMap); |
/* Exit keys: ALT-X, ENTER */ |
k.flag = ALTL_BIT; |
k.scan = KEY_X; |
k.ascii = 'x'; |
keyb_hook(k, endfun); |
k.flag = ALTR_BIT; |
k.scan = KEY_X; |
k.ascii = 'x'; |
keyb_hook(k, endfun); |
k.flag = 0; |
k.scan = KEY_ENT; |
k.ascii = 13; |
keyb_hook(k, endfun); |
/* Create HARD cars */ |
k.flag = 0; |
k.scan = KEY_C; |
k.ascii = 'c'; |
keyb_hook(k, h_car_create); |
/* kill hard cars*/ |
k.flag = 0; |
k.scan = KEY_K; |
k.ascii = 'k'; |
keyb_hook(k, h_car_kill); |
//refresh map |
k.flag=0; |
k.scan = KEY_R; |
k.ascii = 'r'; |
keyb_hook(k, refresh); |
//create soft cars |
k.flag=0; |
k.scan = KEY_S; |
k.ascii = 's'; |
keyb_hook(k, s_car_create); |
//toggle sensors |
k.flag=0; |
k.scan = KEY_D; |
k.ascii = 'd'; |
keyb_hook(k, sensor_switch); |
} |
/demos/branches/pj/simcity/include/constant.h |
---|
0,0 → 1,99 |
/*************************const.h*******************************/ |
// constants and new types declaration |
#include <kernel/func.h> |
#define CST |
#define NO_SPRITE 8 |
#define GRAPH |
#define SENSOR |
#define MAX_CAR 10 |
#define MAX_TL 3 |
#define S_POINT 4 |
#define PERIOD_CAR 135000 //period for K62 350MHZ |
//#define PERIOD_CAR 87000 //period for PIII 733MHZ |
#define SPERIOD_CAR 13000 |
#define SCAR_WCET 4300 |
#define SECOND 1000000 |
#define TL_WCET 200 |
#define CAM_WCET 200 |
#define GAUGE_WCET 200 |
#define ARROW_WCET 100 |
//#define CAR_WCET 7700 //wcet for PIII 733MHZ |
#define CAR_WCET 12400 //wcet for K62 350MHZ |
#define ROW 30 |
#define COL 30 |
#define NCAR 24 |
#define W 640 |
#define H 480 |
#define SH 12 |
#define SW 12 |
#define MAX_DIST 50.0 |
#define NORM_DIST 15.0 |
#define COLL_DIST 25.0 |
#define MIN_SPEED 0.0 |
#define NORM_SPEED 4.0 |
#define MAX_SPEED 6.0 |
#define MAX_LOOK 50 |
#define LOOK_ANGLE 70 |
#define DIST_ANGLE 90 |
#define CORR_FACT 0.7 |
#define FRANTIC 0.0 |
#define POS_RAND |
#define OBL_START 1 |
#define DEG_TO_RAD(x) ((float)x)/180.0*3.14 |
#define RAD_TO_DEG(x) ((float)x)/3.14*180.0 |
#define RANDDIR (rand()%3-1) |
#define L 0 |
#define R 1 |
#define U 2 |
#define D 3 |
#define DRAW 1 |
#define CANCEL 0 |
#define VERTICAL 1 |
#define HORIZONTAL 0 |
#define STEER_LEFT 1 |
#define STEER_RIGHT -1 |
#define STRAIGHT 0 |
#define CAMX 670 |
#define CAMY 120 |
#define MAPX 8 |
#define MAPY 113 |
#define FREE 0 |
#define STOP 1 |
#define CAR 2 |
/*data types*/ |
typedef struct car_d { |
int number; |
float xpos,ypos; |
int xp,yp; |
float dist_obs,dist_sem; |
float speed,middle,front,rear,somma; |
int angle,sat; |
char collision,boom,correggi,incrocio; |
char dir,running; |
struct car_d *next; |
} car_data; |
typedef struct { |
int x,y; |
} point; |
typedef struct { |
char vpos,hpos,tl_name[8]; |
point l,r,u,d; |
PID pid; |
DWORD period; |
} tl_data; |
typedef struct { |
int xpos,ypos,angles; |
} starting_set; |
/demos/branches/pj/simcity/include/misc.h |
---|
0,0 → 1,18 |
//*****************************misc.h************************************ |
// miscellaneous useful functions |
#ifndef CST |
#include "constant.h" |
#endif |
int my_rint(float); |
void fill_table(); |
void tl_init(); |
void set_start_point(); |
int returnCarIndex(int); |
int find_col(int,int); |
int find_tl(int,int,int); |
int allinea(int); |
int module(int,int,int,int); |
int normalize(int); |
/demos/branches/pj/simcity/include/draw.h |
---|
0,0 → 1,14 |
//****************************draw.h******************************* |
// drawing functions |
#ifndef CST |
#include "constant.h" |
#endif |
void drawSprites(int,int,char,char); |
void drawStreet(int,int); |
void get_images(); |
void drawCar(int,int,int,char); |
void bold_rect(WORD,WORD,WORD,WORD,BYTE,BYTE,BYTE); |
void draw_scenario(); |
void draw_tl(int x,int y,DWORD img[SH][SW]); |
void drawFaces(int,int,short); |
/demos/branches/pj/simcity/include/simcity.h |
---|
0,0 → 1,77 |
/********************************simcity.h***************************/ |
// global variables and tasks bodies declaration |
#include <kernel/func.h> |
#include <semaphor.h> |
#ifndef CST |
#include "constant.h" |
#endif |
/*Global stuff*/ |
extern car_data car_data_array[MAX_CAR]; |
extern tl_data tl_data_array[MAX_TL]; |
//car starting points |
extern starting_set starting_set_array[S_POINT]; |
// PID vectors |
extern PID p_table[MAX_CAR]; |
extern PID c_table[MAX_CAR]; |
extern PID g_table[MAX_CAR]; |
extern PID a_table[MAX_CAR]; |
extern char kill_flag[MAX_CAR]; |
//graphic mutex |
extern sem_t mutex; |
//kill_flag mutexes |
extern sem_t kill_mutex[MAX_CAR]; |
//various sprites |
extern DWORD macchine[ROW][COL][NCAR]; |
extern DWORD strada[H][W]; |
extern BYTE street[H*W*2]; |
extern BYTE vbuf[MAX_CAR][ROW*COL*2]; |
extern BYTE clrcam[(ROW+2)*(COL+2)*2]; |
extern BYTE gauge_img[ROW*COL*2]; |
extern BYTE brk_gauge[ROW*COL*2]; |
extern BYTE arrow[3][ROW*COL*2]; |
extern DWORD sprites[ROW][COL][NO_SPRITE]; |
extern BYTE clrscr[800*600*2]; |
extern DWORD faces[2][74][47]; |
extern char sens; |
//sine & cosine look-up tables |
extern float cosine[360],sine[360]; |
//traffic light sprites |
extern DWORD y_sem[4][SH][SW]; |
extern DWORD r_sem[4][SH][SW]; |
extern DWORD g_sem[4][SH][SW]; |
//useful colors |
extern DWORD white; |
extern DWORD black; |
extern DWORD red; |
extern DWORD gray; |
extern DWORD blue; |
extern DWORD green; |
extern DWORD border; |
extern DWORD tl_bg; |
//task chain pointers |
extern car_data *free_n,*busy_n,*free_o,*busy_o; |
// car counter |
extern short maxc; |
/*function declaration */ |
/* task bodies*/ |
TASK car(car_data *); |
TASK traffic_light(tl_data *); |
TASK camera(int); |
TASK gauge_c(int); |
TASK blink_arrow(int); |
TASK refresher(void *); |
TASK killer(void *); |
/demos/branches/pj/simcity/include/states.h |
---|
0,0 → 1,21 |
enum stati { |
NORM, |
INC1, |
DX1, |
STR1, |
STR2, |
SX1, |
SX2, |
SX3 |
} ; |
enum sprite_type { |
EXPL, |
GAUGE, |
ARR_OFF, |
ARR_SX, |
ARR_DX, |
VECCHINA, |
V_SPLAT, |
GAUGE_ROTTO |
}; |
/demos/branches/pj/simcity/include/proc.h |
---|
0,0 → 1,23 |
//***************************proc.h****************************** |
// task management functions |
#ifndef CST |
#include "constant.h" |
#endif |
int add(); |
void del(int); |
void init_struct(); |
int del_o(); |
void gauge_create(char *,int); |
void arrow_create(char *,int); |
void killer_create(); |
void ref_create(); |
void cam_create(char *,int); |
void stl_create(char *,int); |
void h_create(char *,int); |
void s_create(char *,int); |
void killing(int); |
/demos/branches/pj/simcity/include/car.h |
---|
0,0 → 1,10 |
//*****************************car.h******************************** |
// car management functions |
#ifndef CST |
#include "constant.h" |
#endif |
char collision_sensor(car_data *); |
int sensore(car_data *); |
void ch_spd(car_data *,char); |
/demos/branches/pj/simcity/include/keyfunct.h |
---|
0,0 → 1,10 |
/*******************************keyfunct.h****************************/ |
// keyboard handler functions |
#include <drivers/keyb.h> |
void h_car_create(KEY_EVT *); |
void s_car_create(KEY_EVT *); |
void h_car_kill(KEY_EVT *); |
void endfun(KEY_EVT *); |
void refresh(KEY_EVT *); |
void sensor_switch(KEY_EVT *); |
/demos/branches/pj/simcity/sem/green_l.raw |
---|
0,0 → 1,0 |