Rev 1085 |
Go to most recent revision |
Blame |
Compare with Previous |
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 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
();
}