Subversion Repositories shark

Rev

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(&current, grabber_frame);
231
  if (found) {
232
    memcpy(&sequence[top_frame], &current, 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(&current, grabber_frame, current.xb, current.yb, img_border);
248
    } else {
249
      found = scan_all_frame(&current, grabber_frame);
250
    }
251
 
252
    track->found = found;
253
 
254
    if (found) {
255
      if (top_frame < N_FRAMES) {
256
        memcpy(&sequence[top_frame], &current, sizeof(TDataObj));
257
        top_frame++;
258
      } else {
259
        top_frame = 0;
260
        memcpy(&sequence[top_frame], &current, sizeof(TDataObj));
261
      }
262
 
263
      track->top_frame = top_frame;
264
      memcpy(&track->current, &current, 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
}