Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1085 | pj | 1 | // framegrabber stuffs |
2 | |||
3 | /* File name ......... : ELABOR.C |
||
4 | * Project............ : |
||
5 | * Object ............ : |
||
6 | * Author ............ : Facchinetti Tullio |
||
7 | * Language .......... : C |
||
8 | * Compiler .......... : GNU C |
||
9 | * Operative system .. : MS-DOS/HARTIK |
||
10 | * Creation data ..... : 04/03/2000 |
||
11 | * Last modify ....... : 19/11/99 |
||
12 | */ |
||
13 | |||
14 | |||
15 | |||
16 | |||
17 | #include <kernel/func.h> |
||
18 | #include <modules/cabs.h> |
||
19 | #include <stdio.h> |
||
20 | #include <drivers/pxc.h> |
||
21 | #include "demo.h" |
||
22 | |||
23 | PID camera_PID; |
||
24 | PID tracking_PID; |
||
25 | |||
26 | static CAB frameCAB; // CAB di deposito delle immagini |
||
27 | static CAB trackingCAB; // CAB di deposito delle info di tracking |
||
28 | |||
29 | |||
30 | int img_border = 10; |
||
31 | int window_width = 40; |
||
32 | int window_height = 40; |
||
33 | TPixel pix_threshold = 128; |
||
34 | |||
35 | // a 256 grayscale palette |
||
36 | WORD gray_palette[256]; |
||
37 | |||
38 | // the image to be putted on the screen |
||
39 | WORD converted_image[IMG_COL*IMG_ROW]; |
||
40 | |||
41 | TDataObj sequence[N_FRAMES]; |
||
42 | |||
43 | |||
44 | |||
45 | void border_up_function(KEY_EVT key) |
||
46 | { |
||
47 | img_border++; |
||
48 | } |
||
49 | |||
50 | void border_down_function(KEY_EVT key) |
||
51 | { |
||
52 | img_border--; |
||
53 | } |
||
54 | |||
55 | void threshold_up_function(KEY_EVT key) |
||
56 | { |
||
57 | char st[50]; |
||
58 | pix_threshold++; |
||
59 | sprintf(st, "threshold %4d", pix_threshold); |
||
60 | mutex_lock(&mutex); |
||
61 | grx_text(st, 400, 100, 255, 0); |
||
62 | mutex_unlock(&mutex); |
||
63 | } |
||
64 | |||
65 | void threshold_down_function(KEY_EVT key) |
||
66 | { |
||
67 | char st[50]; |
||
68 | pix_threshold--; |
||
69 | sprintf(st, "threshold %4d", pix_threshold); |
||
70 | mutex_lock(&mutex); |
||
71 | grx_text(st, 400, 100, 255, 0); |
||
72 | mutex_unlock(&mutex); |
||
73 | } |
||
74 | |||
75 | float distance(unsigned int x1, unsigned int y1, |
||
76 | unsigned int x2, unsigned int y2) |
||
77 | { |
||
78 | return(sqrt(((y2 - y1) * (y2 - y1)) + ((x2 - x1) * (x2 - x1)))); |
||
79 | } |
||
80 | |||
81 | char scan_window_frame(TDataObj *data, TPixel *in_frame, |
||
82 | unsigned int xc, unsigned int yc, int border) |
||
83 | { |
||
84 | unsigned long int offset; |
||
85 | unsigned int i, j; |
||
86 | TPixel pix; |
||
87 | double sum_x = 0.0, sum_y = 0.0; |
||
88 | unsigned int n_pix = 0; |
||
89 | int x1, y1, x2, y2; // Must be int!!! |
||
90 | char found; |
||
91 | |||
92 | data->x1 = N_COL; |
||
93 | data->y1 = N_ROW; |
||
94 | data->x2 = data->y2 = 0; |
||
95 | data->xb = data->yb = -1; |
||
96 | data->time_stamp = -1; |
||
97 | |||
98 | found = 0; |
||
99 | |||
100 | x1 = MAX_NUM((xc - window_width / 2), (border)); |
||
101 | y1 = MAX_NUM((yc - window_height / 2), (border)); |
||
102 | x2 = MIN_NUM((xc + window_width / 2), (N_COL - border)); |
||
103 | y2 = MIN_NUM((yc + window_height / 2), (N_ROW - border)); |
||
104 | |||
105 | for (i = y1; i < y2; i++) { |
||
106 | for (j = x1; j < x2; j++) { |
||
107 | offset = i * N_COL + j; |
||
108 | pix = *(in_frame + offset); |
||
109 | |||
110 | #ifdef __BLACK_ON_WHITE |
||
111 | // Pixel found (object is black, background is white) |
||
112 | if (pix < pix_threshold) { |
||
113 | #else |
||
114 | // Pixel found (object is white, background is black) |
||
115 | if (pix > pix_threshold) { |
||
116 | #endif |
||
117 | data->time_stamp = sys_gettime(NULL); |
||
118 | found = 1; |
||
119 | n_pix++; |
||
120 | sum_x += j; |
||
121 | sum_y += i; |
||
122 | // *(in_frame + offset) = 0; |
||
123 | if (i < data->y1) |
||
124 | data->y1 = i; |
||
125 | if (i > data->y2) |
||
126 | data->y2 = i; |
||
127 | if (j < data->x1) |
||
128 | data->x1 = j; |
||
129 | if (j > data->x2) |
||
130 | data->x2 = j; |
||
131 | |||
132 | } else { |
||
133 | // *(in_frame + offset) = 255; |
||
134 | } |
||
135 | } |
||
136 | } |
||
137 | data->xb = sum_x / n_pix; |
||
138 | data->yb = sum_y / n_pix; |
||
139 | return(found); |
||
140 | } |
||
141 | |||
142 | char scan_all_frame(TDataObj *data, TPixel *in_frame) |
||
143 | { |
||
144 | unsigned long int offset; |
||
145 | unsigned int i, j; |
||
146 | TPixel pix; |
||
147 | double sum_x = 0.0, sum_y = 0.0; |
||
148 | unsigned int n_pix = 0; |
||
149 | char found; |
||
150 | |||
151 | data->x1 = N_COL; |
||
152 | data->y1 = N_ROW; |
||
153 | data->x2 = data->y2 = 0; |
||
154 | data->xb = data->yb = -1; |
||
155 | data->time_stamp = -1; |
||
156 | |||
157 | found = 0; |
||
158 | |||
159 | // In a single image scanning it performs thresholding and computation |
||
160 | for (i = img_border; i < N_ROW - img_border; i++) { |
||
161 | for (j = img_border; j < N_COL - img_border; j++) { |
||
162 | offset = i * N_COL + j; |
||
163 | pix = *(in_frame + offset); |
||
164 | |||
165 | #ifdef __BLACK_ON_WHITE |
||
166 | // Pixel found (object is black, background is white) |
||
167 | if (pix < pix_threshold) { |
||
168 | #else |
||
169 | // Pixel found (object is white, background is black) |
||
170 | if (pix > pix_threshold) { |
||
171 | #endif |
||
172 | data->time_stamp = sys_gettime(NULL); |
||
173 | found = 1; |
||
174 | n_pix++; |
||
175 | sum_x += j; |
||
176 | sum_y += i; |
||
177 | // *(in_frame + offset) = 0; |
||
178 | if (i < data->y1) |
||
179 | data->y1 = i; |
||
180 | if (i > data->y2) |
||
181 | data->y2 = i; |
||
182 | if (j < data->x1) |
||
183 | data->x1 = j; |
||
184 | if (j > data->x2) |
||
185 | data->x2 = j; |
||
186 | |||
187 | } else { |
||
188 | // *(in_frame + offset) = 255; |
||
189 | } |
||
190 | } |
||
191 | } |
||
192 | data->xb = sum_x / n_pix; |
||
193 | data->yb = sum_y / n_pix; |
||
194 | return(found); |
||
195 | } |
||
196 | |||
197 | void tracking(int top_frame, int *track_x, int *track_y, int *int_vx, int *int_vy, int time_to) |
||
198 | { |
||
199 | float vx, vy; |
||
200 | |||
201 | vx = (float)(sequence[top_frame - 1].xb - sequence[top_frame - 2].xb) / |
||
202 | (float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp); |
||
203 | vx *= 1000; |
||
204 | |||
205 | vy = (float)(sequence[top_frame - 1].yb - sequence[top_frame - 2].yb) / |
||
206 | (float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp); |
||
207 | vy *= 1000; |
||
208 | |||
209 | *track_x = sequence[top_frame - 1].xb + vx * time_to; |
||
210 | *track_y = sequence[top_frame - 1].yb + vy * time_to; |
||
211 | |||
212 | *int_vx = vx * 1000; |
||
213 | *int_vy = vy * 1000; |
||
214 | } |
||
215 | |||
216 | TASK tracking_task(void *arg) |
||
217 | { |
||
218 | // static unsigned int n_frame = 0; |
||
219 | char found; |
||
220 | TPixel *grabber_frame; |
||
221 | int top_frame = 0; |
||
222 | TDataObj current; |
||
223 | TTracking *track; |
||
224 | |||
225 | frameCAB = PXC_GetCab(); |
||
226 | |||
227 | grabber_frame = cab_getmes(frameCAB); |
||
228 | |||
229 | // Executes first time |
||
230 | found = scan_all_frame(¤t, grabber_frame); |
||
231 | if (found) { |
||
232 | memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
||
233 | top_frame++; |
||
234 | } |
||
235 | |||
236 | cab_unget(frameCAB, grabber_frame); |
||
237 | |||
238 | task_endcycle(); |
||
239 | |||
240 | while (1) { |
||
241 | // Acquisizione immagine corrente |
||
242 | grabber_frame = (TPixel *)cab_getmes(frameCAB); |
||
243 | track = (TTracking *)cab_reserve(trackingCAB); |
||
244 | |||
245 | // Estrazione della nuova trasformata sul frame corrente |
||
246 | if (found) { |
||
247 | found = scan_window_frame(¤t, grabber_frame, current.xb, current.yb, img_border); |
||
248 | } else { |
||
249 | found = scan_all_frame(¤t, grabber_frame); |
||
250 | } |
||
251 | |||
252 | track->found = found; |
||
253 | |||
254 | if (found) { |
||
255 | if (top_frame < N_FRAMES) { |
||
256 | memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
||
257 | top_frame++; |
||
258 | } else { |
||
259 | top_frame = 0; |
||
260 | memcpy(&sequence[top_frame], ¤t, sizeof(TDataObj)); |
||
261 | } |
||
262 | |||
263 | track->top_frame = top_frame; |
||
264 | memcpy(&track->current, ¤t, sizeof(TDataObj)); |
||
265 | |||
266 | if (top_frame > 1) { |
||
267 | tracking(top_frame, &track->predx, &track->predy, |
||
268 | &track->vx, &track->vy, 100); |
||
269 | } |
||
270 | } else { |
||
271 | track->top_frame = top_frame = 0; |
||
272 | } |
||
273 | |||
274 | // Release CABs |
||
275 | cab_putmes(trackingCAB, (char *)track); |
||
276 | cab_unget(frameCAB, grabber_frame); |
||
277 | |||
278 | task_endcycle(); |
||
279 | } |
||
280 | } |
||
281 | |||
282 | |||
283 | |||
284 | |||
285 | |||
286 | |||
287 | |||
288 | /* |
||
289 | * |
||
290 | * |
||
291 | * |
||
292 | * Camera task |
||
293 | * |
||
294 | * |
||
295 | * |
||
296 | * |
||
297 | */ |
||
298 | |||
299 | |||
300 | TASK camera_task(void *arg) |
||
301 | { |
||
302 | register int i,j,col,row; |
||
303 | static unsigned int n_frame = 0; |
||
304 | TPixel *grabber_frame; |
||
305 | TTracking *track; |
||
306 | char st[50]; |
||
307 | |||
308 | // Inizializzazione del task |
||
309 | frameCAB = PXC_GetCab(); |
||
310 | |||
311 | while (1) { |
||
312 | n_frame++; |
||
313 | sprintf(st, "frame n. %5d", n_frame); |
||
314 | |||
315 | grx_text(st, 400, 224, white, 0); |
||
316 | |||
317 | // Acquisizione immagine corrente |
||
318 | grabber_frame = cab_getmes(frameCAB); |
||
319 | |||
320 | for (i=1; i<IMG_ROW-1; i++) |
||
321 | for (j=0; j<IMG_COL; j++) { |
||
322 | col = (j*(N_COL-1))/(IMG_COL-1); |
||
323 | row = (i*(N_ROW-1))/(IMG_ROW-1); |
||
324 | converted_image[i*IMG_COL+j] = gray_palette[*(grabber_frame+row*N_COL+col)]; |
||
325 | } |
||
326 | |||
327 | // Release CAB |
||
328 | cab_unget(frameCAB, grabber_frame); |
||
329 | |||
330 | for (j=0; j<IMG_COL; j++) { |
||
331 | converted_image[j] = gray_palette[0]; |
||
332 | converted_image[(IMG_ROW-1)*IMG_COL+j] = gray_palette[0]; |
||
333 | } |
||
334 | |||
335 | mutex_lock(&mutex); |
||
336 | grx_putimage(IMG_X, IMG_Y, IMG_X+IMG_COL-1, IMG_Y+IMG_ROW-1, |
||
337 | (BYTE *)converted_image); |
||
338 | mutex_unlock(&mutex); |
||
339 | |||
340 | track = (TTracking *)cab_getmes(trackingCAB); |
||
341 | |||
342 | if (track->found) { |
||
343 | mutex_lock(&mutex); |
||
344 | // sprintf(st, "found: %d", track->found); |
||
345 | // grx_text(st, 400, 280, 255, 0); |
||
346 | |||
347 | if (track->top_frame > 1) { |
||
348 | int px, py; |
||
349 | |||
350 | // sprintf(st, "top_frame %5d", track->top_frame); |
||
351 | // grx_text(st, 400, 270, 255, 0); |
||
352 | |||
353 | if (track->predx < img_border) |
||
354 | px = img_border; |
||
355 | else if (track->predx > N_COL-img_border) |
||
356 | px = N_COL-img_border; |
||
357 | else |
||
358 | px = track->predx; |
||
359 | |||
360 | if (track->predy < img_border) |
||
361 | py = img_border; |
||
362 | else if (track->predy > N_ROW-img_border) |
||
363 | py = N_ROW-img_border; |
||
364 | else |
||
365 | py = track->predy; |
||
366 | |||
367 | grx_disc(IMG_X+(px*2)/3, IMG_Y+(py*2)/3, 3, 127); |
||
368 | |||
369 | // grx_disc(IMG_X+(current.xb*2)/3, IMG_Y+(current.yb*2)/3, 3, 127); |
||
370 | grx_rect(IMG_X+(track->current.x1*2)/3, IMG_Y+(track->current.y1*2)/3, |
||
371 | IMG_X+(track->current.x2*2)/3, IMG_Y+(track->current.y2*2)/3, 127); |
||
372 | |||
373 | sprintf(st, "speed = (%5d, %5d) pix/s", track->vx, track->vy); |
||
374 | grx_text(st, 400, 232, white, 0); |
||
375 | } |
||
376 | mutex_unlock(&mutex); |
||
377 | } |
||
378 | cab_unget(trackingCAB, (char *)track); |
||
379 | |||
380 | task_endcycle(); |
||
381 | } |
||
382 | } |
||
383 | |||
384 | |||
385 | |||
386 | |||
387 | |||
388 | /* |
||
389 | * |
||
390 | * |
||
391 | * |
||
392 | * Framegrabber Initialization |
||
393 | * |
||
394 | * |
||
395 | * |
||
396 | * |
||
397 | */ |
||
398 | |||
399 | void start_listener(TIME p); |
||
400 | |||
401 | void framegrabber_close(void *arg) |
||
402 | { |
||
403 | PXC_Close(); |
||
404 | } |
||
405 | |||
406 | void scenario_framegrabber() |
||
407 | { |
||
408 | grx_text("Camera" , 384, WAVE_Y-WAVE_HEIGHT-10, rgb16(0,0,255), black); |
||
409 | grx_line(384,WAVE_Y-WAVE_HEIGHT-1,639,WAVE_Y-WAVE_HEIGHT-1,red); |
||
410 | } |
||
411 | |||
412 | void init_framegrabber(void) |
||
413 | { |
||
414 | register int i; |
||
415 | KEY_EVT my_key; |
||
416 | TIME period; |
||
417 | |||
418 | my_key.ascii = 'a'; |
||
419 | my_key.scan = KEY_A; |
||
420 | keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_up_function); |
||
421 | |||
422 | my_key.ascii = 'z'; |
||
423 | my_key.scan = KEY_Z; |
||
424 | keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_down_function); |
||
425 | |||
426 | my_key.ascii = 's'; |
||
427 | my_key.scan = KEY_S; |
||
428 | keyb_hook(my_key, (void (*)(KEY_EVT *))border_up_function); |
||
429 | |||
430 | my_key.ascii = 'x'; |
||
431 | my_key.scan = KEY_X; |
||
432 | keyb_hook(my_key, (void (*)(KEY_EVT *))border_down_function); |
||
433 | |||
434 | // Aggiusta la palette |
||
435 | for (i = 0; i < 256; i++) |
||
436 | gray_palette[i] = rgb16(i,i,i); |
||
437 | //for (i = 0; i < 256; i++) |
||
438 | // grx_setcolor(i, i/4, i/4, i/4); |
||
439 | |||
440 | mutex_lock(&mutex); |
||
441 | // grx_text("Grabber enabled: no test!", 10, 10, 255, 0); |
||
442 | |||
443 | // Some messages on screen |
||
444 | // grx_text("A-Z change threshold", 400, 240, 255, 0); |
||
445 | // grx_text("S-X change window borders", 400, 250, 255, 0); |
||
446 | mutex_unlock(&mutex); |
||
447 | |||
448 | period = PXC_Initiate(4); |
||
449 | |||
450 | if (!period) { |
||
451 | grx_close(); |
||
452 | cprintf("Problemi nell'inizializzazione del framegrabber\n"); |
||
453 | halt(); |
||
454 | sys_end(); |
||
455 | } else { |
||
456 | TTracking *trdata; |
||
457 | // tracking CAB init |
||
458 | trackingCAB = cab_create("trackingCAB", sizeof(TTracking), 3); |
||
459 | trdata = (TTracking *)cab_reserve(trackingCAB); |
||
460 | trdata->found = 0; |
||
461 | cab_putmes(trackingCAB, (char *)trdata); |
||
462 | |||
463 | start_listener(period); |
||
464 | } |
||
465 | |||
466 | sys_atrunlevel(framegrabber_close, NULL, RUNLEVEL_BEFORE_EXIT); |
||
467 | } |
||
468 | |||
469 | |||
470 | void start_listener(TIME period) |
||
471 | { |
||
472 | SOFT_TASK_MODEL m1, m2; |
||
473 | |||
474 | soft_task_default_model(m1); |
||
475 | soft_task_def_level(m1,1); |
||
476 | soft_task_def_met(m1,WCET_TRACKING); |
||
477 | soft_task_def_usemath(m1); |
||
478 | // soft_task_def_aperiodic(m1); |
||
479 | soft_task_def_period(m1,(PERIOD_TRACKING)); |
||
480 | soft_task_def_group(m1,1); |
||
481 | soft_task_def_ctrl_jet(m1); |
||
482 | soft_task_def_skip_arrivals(m1); |
||
483 | tracking_PID = task_create("track", tracking_task, &m1, NULL); |
||
484 | if (tracking_PID == -1) { |
||
485 | grx_close(); |
||
486 | perror("FFTPlay: Could not create task <tra>\n"); |
||
487 | ll_abort(54); |
||
488 | perror("FFTPlay: Could not create task <tracking>\n"); |
||
489 | sys_end(); |
||
490 | } |
||
491 | |||
492 | soft_task_default_model(m2); |
||
493 | soft_task_def_level(m2,1); |
||
494 | soft_task_def_met(m2,WCET_CAMERA); |
||
495 | soft_task_def_usemath(m2); |
||
496 | // soft_task_def_aperiodic(m2); |
||
497 | soft_task_def_period(m2,PERIOD_CAMERA); |
||
498 | soft_task_def_group(m2,1); |
||
499 | soft_task_def_ctrl_jet(m2); |
||
500 | // soft_task_def_skip_arrivals(m2); |
||
501 | camera_PID = task_create("cam", camera_task, &m2, NULL); |
||
502 | if (camera_PID == -1) { |
||
503 | grx_close(); |
||
504 | perror("FFTPlay: Could not create task <came>\n"); |
||
505 | ll_abort(54); |
||
506 | perror("FFTPlay: Could not create task <camera>\n"); |
||
507 | sys_end(); |
||
508 | } |
||
509 | |||
510 | } |
||
511 | |||
512 | void start_framegrabber() |
||
513 | { |
||
514 | // PXC_Push_Listener(tracking_PID,2); |
||
515 | PXC_Start(); |
||
516 | } |