Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
// 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
();
}