Subversion Repositories shark

Rev

Rev 1085 | Go to most recent revision | Details | Compare with Previous | 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 image_elab_PID;
24
TIME periodo;
25
CAB PXC_CAB;
26
 
27
static CAB frameCAB; // CAB di deposito delle immagini
28
static TDataObj current, older;
29
 
30
// extern in INIT.C
31
int img_border    =  10;
32
int window_width  =  40;
33
int window_height =  40;
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
 
42
#ifdef __BLACK_ON_WHITE
43
TPixel pix_threshold = 64;
44
#else
45
TPixel pix_threshold = 243;
46
#endif
47
 
48
 
49
// Global for testing!!!
50
static char st[50];
51
TIME before;
52
 
53
TDataObj sequence[N_FRAMES];
54
int top_frame = 0;
55
 
56
double dist, speed;
57
 
58
static TPixel *grabber_frame;
59
 
60
void border_up_function(KEY_EVT key)
61
{
62
  img_border++;
63
}
64
 
65
void border_down_function(KEY_EVT key)
66
{
67
  img_border--;
68
}
69
 
70
void threshold_up_function(KEY_EVT key)
71
{
72
  pix_threshold++;
73
  sprintf(st, "threshold %4d", pix_threshold);
74
  mutex_lock(&mutex);
75
  //grx_text(st, 400, 300, 255, 0);
76
  mutex_unlock(&mutex);
77
}
78
 
79
void threshold_down_function(KEY_EVT key)
80
{
81
  pix_threshold--;
82
  sprintf(st, "threshold %4d", pix_threshold);
83
  mutex_lock(&mutex);
84
  //grx_text(st, 400, 300, 255, 0);
85
  mutex_unlock(&mutex);
86
}
87
 
88
float distance(unsigned int x1, unsigned int y1,
89
               unsigned int x2, unsigned int y2)
90
{
91
  return(sqrt(((y2 - y1) * (y2 - y1)) + ((x2 - x1) * (x2 - x1))));
92
}
93
 
94
char scan_window_frame(TDataObj *data, TPixel *in_frame,
95
                       unsigned int xc, unsigned int yc, int border)
96
{
97
  unsigned long int offset;
98
  unsigned int i, j;
99
  TPixel pix;
100
  double sum_x = 0.0, sum_y = 0.0;
101
  unsigned int n_pix = 0;
102
  int x1, y1, x2, y2;  // Must be int!!!
103
  char found;
104
 
105
  data->x1 = N_COL;
106
  data->y1 = N_ROW;
107
  data->x2 = data->y2 = 0;
108
  data->xb = data->yb = -1;
109
  data->time_stamp = -1;
110
 
111
  found = 0;
112
 
113
  x1 = MAX_NUM((xc - window_width / 2), (border));
114
  y1 = MAX_NUM((yc - window_height / 2), (border));
115
  x2 = MIN_NUM((xc + window_width / 2), (N_COL - border));
116
  y2 = MIN_NUM((yc + window_height / 2), (N_ROW - border));
117
 
118
  for (i = y1; i < y2; i++) {
119
    for (j = x1; j < x2; j++) {
120
      offset = i * N_COL + j;
121
      pix = *(in_frame + offset);
122
 
123
#ifdef __BLACK_ON_WHITE
124
      // Pixel found (object is black, background is white)
125
      if (pix < pix_threshold) {
126
#else
127
      // Pixel found (object is white, background is black)
128
      if (pix > pix_threshold) {
129
#endif
130
        data->time_stamp = sys_gettime(NULL);
131
        found = 1;
132
        n_pix++;
133
        sum_x += j;
134
        sum_y += i;
135
        *(in_frame + offset) = 0;
136
        if (i < data->y1)
137
          data->y1 = i;
138
        if (i > data->y2)
139
          data->y2 = i;
140
        if (j < data->x1)
141
          data->x1 = j;
142
        if (j > data->x2)
143
          data->x2 = j;
144
 
145
      } else {
146
        *(in_frame + offset) = 255;
147
      }
148
    }
149
  }
150
  data->xb = sum_x / n_pix;
151
  data->yb = sum_y / n_pix;
152
  return(found);
153
}
154
 
155
char scan_all_frame(TDataObj *data, TPixel *in_frame)
156
{
157
  unsigned long int offset;
158
  unsigned int i, j;
159
  TPixel pix;
160
  double sum_x = 0.0, sum_y = 0.0;
161
  unsigned int n_pix = 0;
162
  char found;
163
 
164
  data->x1 = N_COL;
165
  data->y1 = N_ROW;
166
  data->x2 = data->y2 = 0;
167
  data->xb = data->yb = -1;
168
  data->time_stamp = -1;
169
 
170
  found = 0;
171
 
172
  // In a single image scanning it performs thresholding and computation
173
  for (i = img_border; i < N_ROW - img_border; i++) {
174
    for (j = img_border; j < N_COL - img_border; j++) {
175
      offset = i * N_COL + j;
176
      pix = *(in_frame + offset);
177
 
178
#ifdef __BLACK_ON_WHITE
179
      // Pixel found (object is black, background is white)
180
      if (pix < pix_threshold) {
181
#else
182
      // Pixel found (object is white, background is black)
183
      if (pix > pix_threshold) {
184
#endif
185
        data->time_stamp = sys_gettime(NULL);
186
        found = 1;
187
        n_pix++;
188
        sum_x += j;
189
        sum_y += i;
190
        *(in_frame + offset) = 0;
191
        if (i < data->y1)
192
          data->y1 = i;
193
        if (i > data->y2)
194
          data->y2 = i;
195
        if (j < data->x1)
196
          data->x1 = j;
197
        if (j > data->x2)
198
          data->x2 = j;
199
 
200
      } else {
201
        *(in_frame + offset) = 255;
202
      }
203
    }
204
  }
205
  data->xb = sum_x / n_pix;
206
  data->yb = sum_y / n_pix;
207
  return(found);
208
}
209
 
210
void tracking(int *track_x, int *track_y, int time_to)
211
{
212
  float vx, vy;
213
 
214
  vx = (float)(sequence[top_frame - 1].xb - sequence[top_frame - 2].xb) /
215
       (float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp);
216
  vx *= 1000000;
217
 
218
  vy = (float)(sequence[top_frame - 1].yb - sequence[top_frame - 2].yb) /
219
       (float)(sequence[top_frame - 1].time_stamp - sequence[top_frame - 2].time_stamp);
220
  vy *= 1000000;
221
 
222
  (*track_x) = sequence[top_frame - 1].xb + vx * time_to;
223
  (*track_y) = sequence[top_frame - 1].yb + vy * time_to;
224
 
225
  sprintf(st, "speed = (%5d, %5d) pix/s", (int)vx, (int)vy);
226
  mutex_lock(&mutex);
227
  //grx_text(st, 400, 410, 255, 0);
228
  mutex_unlock(&mutex);
229
}
230
 
231
void put_frame(TPixel *frame)
232
{
233
    register int i,j,col,row;
234
 
235
    for (i=0; i<IMG_ROW; i++)
236
      for (j=0; j<IMG_COL; j++) {
237
        col = (j*(N_COL-1))/(IMG_COL-1);
238
        row = (i*(N_ROW-1))/(IMG_ROW-1);
239
        converted_image[i*IMG_COL+j] = gray_palette[*(frame+row*N_COL+col)];
240
      }
241
 
242
    mutex_lock(&mutex);
243
    //grx_putimage(IMG_X, IMG_Y, IMG_X+IMG_COL-1, IMG_Y+IMG_ROW-1,
244
    //             (BYTE *)converted_image);
245
    mutex_unlock(&mutex);
246
}
247
 
248
 
249
TASK elab_image_TASK(void)
250
{
251
//  register int i, j;
252
  static unsigned int n_frame = 0;
253
  char found;
254
  int pred_x, pred_y;
255
 
256
  // Inizializzazione del task
257
  frameCAB = PXC_GetCab();
258
 
259
  grabber_frame = cab_getmes(frameCAB);
260
 
261
  // Executes first time
262
  found = scan_all_frame(&current, grabber_frame);
263
//  found =0;
264
  if (found) {
265
    memcpy(&sequence[top_frame], &current, sizeof(TDataObj));
266
    top_frame++;
267
  }
268
 
269
  cab_unget(frameCAB, grabber_frame);
270
 
271
  task_endcycle();
272
 
273
  while (1) {
274
 
275
//    before = sys_gettime(NULL);
276
 
277
    n_frame++;
278
    sprintf(st, "frame n. %5d", n_frame);
279
 
280
    mutex_lock(&mutex);
281
    //grx_text(st, 400, 290, 255, 0);
282
 
283
    sprintf(st, "top_frame %5d", top_frame);
284
    //grx_text(st, 400, 270, 255, 0);
285
 
286
    sprintf(st, "found: %d!", found);
287
    //grx_text(st, 400, 280, 255, 0);
288
    mutex_unlock(&mutex);
289
 
290
    // Acquisizione immagine corrente
291
    grabber_frame = cab_getmes(frameCAB);
292
 
293
    // copy current in older
294
    memcpy(&older, &current, sizeof(TDataObj));
295
 
296
    // Estrazione della nuova trasformata sul frame corrente
297
    if (found) {
298
      found = scan_window_frame(&current, grabber_frame, current.xb, current.yb, img_border);
299
    } else {
300
      found = scan_all_frame(&current, grabber_frame);
301
    }
302
 
303
//    //grx_putimage(0, 0, N_COL - 1, N_ROW - 1, grabber_frame);
304
    put_frame(grabber_frame);
305
 
306
    if (found) {
307
      if (top_frame < N_FRAMES) {
308
        memcpy(&sequence[top_frame], &current, sizeof(TDataObj));
309
        top_frame++;
310
      } else {
311
        top_frame = 0;
312
        memcpy(&sequence[top_frame], &current, sizeof(TDataObj));
313
      }
314
 
315
      if (top_frame > 1) {
316
        tracking(&pred_x, &pred_y, 100);
317
 
318
        mutex_lock(&mutex);
319
//        //grx_disc(IMG_X+(pred_x*2)/3, IMG_Y+(pred_y*2)/3, 3, 127);
320
 
321
        //grx_disc(IMG_X+(current.xb*2)/3, IMG_Y+(current.yb*2)/3, 3, 127);
322
        //grx_rect(IMG_X+(current.x1*2)/3, IMG_Y+(current.y1*2)/3,
323
        //         IMG_X+(current.x2*2)/3, IMG_Y+(current.y2*2)/3, 127);
324
        mutex_unlock(&mutex);
325
 
326
      }
327
    } else {
328
      top_frame = 0;
329
    }
330
 
331
    // Release CAB
332
    cab_unget(frameCAB, grabber_frame);
333
 
334
//    sprintf(st, "durata = %3d ms", (int)(sys_gettime(NULL) - before)/1000);
335
//    mutex_lock(&mutex);
336
//    //grx_text(st, 400, 400, 255, 0);
337
//    mutex_unlock(&mutex);
338
 
339
    task_endcycle();
340
  }
341
}
342
 
343
 
344
void start_listener(void);
345
 
346
void framegrabber_close(void *arg)
347
{
348
  PXC_Close();
349
}
350
 
351
void init_framegrabber(void)
352
{
353
  register int i;
354
  KEY_EVT my_key;
355
 
356
  my_key.ascii = 'a';
357
  my_key.scan = KEY_A;
358
  keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_up_function);
359
 
360
  my_key.ascii = 'z';
361
  my_key.scan = KEY_Z;
362
  keyb_hook(my_key, (void (*)(KEY_EVT *))threshold_down_function);
363
 
364
  my_key.ascii = 's';
365
  my_key.scan = KEY_S;
366
  keyb_hook(my_key, (void (*)(KEY_EVT *))border_up_function);
367
 
368
  my_key.ascii = 'x';
369
  my_key.scan = KEY_X;
370
  keyb_hook(my_key, (void (*)(KEY_EVT *))border_down_function);
371
 
372
  // Aggiusta la palette
373
    for (i = 0; i < 256; i++)
374
      gray_palette[i] = rgb16(i,i,i);
375
  //for (i = 0; i < 256; i++)
376
  //  grx_setcolor(i, i/4, i/4, i/4);
377
 
378
  mutex_lock(&mutex);
379
//  grx_text("Grabber enabled: no test!", 10, 10, 255, 0);
380
 
381
  // Some messages on screen
382
  //grx_text("A-Z change threshold", 400, 240, 255, 0);
383
  //grx_text("S-X change window borders", 400, 250, 255, 0);
384
  mutex_unlock(&mutex);
385
 
386
  periodo = PXC_Initiate(3);
387
  PXC_CAB = PXC_GetCab();
388
 
389
  if (!periodo) {
390
    //grx_close();
391
    cprintf("Problemi nell'inizializzazione del driver\n");
392
    sys_end();
393
  } else {
394
    start_listener();
395
  }
396
 
397
  sys_atrunlevel(framegrabber_close, NULL, RUNLEVEL_BEFORE_EXIT);
398
}
399
 
400
 
401
void start_listener(void)
402
{
403
  SOFT_TASK_MODEL m_soft;
404
 
405
  soft_task_default_model(m_soft);
406
  soft_task_def_met(m_soft,IMAGING_WCET);
407
  soft_task_def_usemath(m_soft);
408
  soft_task_def_aperiodic(m_soft);
409
  soft_task_def_period(m_soft,(periodo*3));
410
  soft_task_def_group(m_soft,1);
411
  soft_task_def_ctrl_jet(m_soft);
412
 
413
  image_elab_PID = task_create("imaging", elab_image_TASK, &m_soft, NULL);
414
 
415
/*  task_activate(  image_elab_PID);
416
  PXC_Push_Listener(image_elab_PID,2);
417
  PXC_Start();*/
418
}
419
 
420
void start_framegrabber()
421
{
422
  PXC_Push_Listener(image_elab_PID,2);
423
  PXC_Start();
424
}