Subversion Repositories shark

/*
 * Project: S.Ha.R.K.
 *
 * Coordinators:
 *   Giorgio Buttazzo    <giorgio@sssup.it>
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *
 * Authors     :
 *   Giacomo Guidi       <giacomo@gandalf.sssup.it>
 *
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
 *
 * http://www.sssup.it
 * http://retis.sssup.it
 * http://shark.sssup.it
 */

/*
 * Copyright (C) 2003 Giacomo Guidi
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "kernel/kern.h"
#include "stdlib.h"

#include "drivers/shark_keyb26.h"
#include "drivers/shark_videodev26.h"
#include "drivers/shark_fb26.h"

#define WIDTH 640
#define HEIGHT 480
#define BYTES_PP 2

unsigned char *video_buf = NULL; //Video Buffer
unsigned long int VMEMLONG = WIDTH * HEIGHT * BYTES_PP / 4; // Used by copy_videomem_16to16
unsigned long int RGB565MEM = WIDTH * HEIGHT * BYTES_PP; // Total video mem

#define FG_PERIOD 40000
#define FG_WCET 30000
#define FG_W 320               
#define FG_H 200

void program_end(void *arg)
{

  sys_end();
 
}

void elaborate_image(void *imageptr);

TASK grab_task(void *arg) {

  struct video_picture vpic;
  struct video_buffer fbuf[2];
  struct video_window win;
  struct video_channel chan;
  int on,display,temp,save;

  task_nopreempt();

  VIDEODEV26_open(0);
                                                                                                                             
  VIDEODEV26_ioctl(0,VIDIOCGCHAN,(unsigned long)&chan);
                                                                                                                             
  chan.channel = (int)(arg);  
                                                                                                                           
  VIDEODEV26_ioctl(0,VIDIOCSCHAN,(unsigned long)&chan);
                                                                                                                             
  VIDEODEV26_ioctl(0,VIDIOCGPICT,(unsigned long)&vpic);
                                                                                                                             
  vpic.palette = VIDEO_PALETTE_GREY;
  vpic.depth = 8;                            
                                                                                                 
  VIDEODEV26_ioctl(0,VIDIOCSPICT,(unsigned long)&vpic);

  fbuf[0].base = malloc(320*200);
  fbuf[0].height = 200;
  fbuf[0].width = 320;
  fbuf[0].bytesperline = 320;
  fbuf[0].depth = 8;
 
  fbuf[1].base = malloc(320*200);
  fbuf[1].height = 200;
  fbuf[1].width = 320;
  fbuf[1].bytesperline = 320;
  fbuf[1].depth = 8;

  VIDEODEV26_ioctl(0,VIDIOCSFBUF,(unsigned long)&(fbuf[0]));
  VIDEODEV26_ioctl(0,VIDIOCSFBUF,(unsigned long)&(fbuf[1]));

  VIDEODEV26_ioctl(0,VIDIOCGWIN,(unsigned long)&win);
                                                                                                                             
  win.x = 0;
  win.y = 0;
  win.width = 320;
  win.height = 200;
                                                                                                                             
  VIDEODEV26_ioctl(0,VIDIOCSWIN,(unsigned long)&win);

  task_preempt();
                               
  display = 1;
  save    = 0;
                                                                           
  while(1) {
                 
    task_nopreempt();
 
    VIDEODEV26_ioctl(0,VIDIOCSFBUF,(unsigned long)&(fbuf[save]));

    on = 1;
    VIDEODEV26_ioctl(0,VIDIOCCAPTURE,(unsigned long)&on);
 
    task_preempt();

    elaborate_image(fbuf[display].base);

    temp = display;
    display = save;
    save = temp;

    task_endcycle();

  }

  return NULL;

}

extern void *video_memory;

void elaborate_image(void *imageptr)
{

  WORD x,y;
  BYTE *col;

    for(y = 0; y < FG_H; y++)
      for(x = 0; x < FG_W; x++) {

        col = (BYTE *)(imageptr + y * FG_W + x);
        *(WORD *)(video_memory + y*(WIDTH*2) + (x*2)) = (WORD)rgb16(*(BYTE *)(col),*(BYTE *)(col),*(BYTE *)(col));

    }

}

int main(int argc, char **argv)
{

  SOFT_TASK_MODEL st;
  PID grab_task_pid;

  soft_task_default_model(st);
  soft_task_def_period(st,40000);
  soft_task_def_met(st,30000);
  soft_task_def_arg(st, (void *)(1));
  soft_task_def_ctrl_jet(st);
 
  grab_task_pid = task_create("GrabTask",grab_task,&st,NULL);
  if (grab_task_pid == NIL) {
        cprintf("ERROR: Cannot create grab task\n");
        sys_end();
  }

  task_activate(grab_task_pid);

  while(keyb_getch(BLOCK) != ESC);

  sys_end();

  return 0;

}