Subversion Repositories shark

// 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(&current, grabber_frame);

//  found =0;

  if (found) {

    memcpy(&sequence[top_frame], &current, 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, &current, sizeof(TDataObj));

    // Estrazione della nuova trasformata sul frame corrente

    if (found) {

      found = scan_window_frame(&current, grabber_frame, current.xb, current.yb, img_border);

    } else {

      found = scan_all_frame(&current, 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], &current, sizeof(TDataObj));

        top_frame++;

      } else {

        top_frame = 0;

        memcpy(&sequence[top_frame], &current, 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();

}