source: trunk/MagicSoft/Cosy/videodev/Camera.cc@ 854

#include "Camera.h"

#include <iostream.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <signal.h>
#include <endian.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/ipc.h>
#include <sys/shm.h>

#include "MStopwatch.h"

#include "videodev.h"

#include <sys/time.h>
#include <sys/resource.h>

inline int Camera::Ioctl(int req, void *opt, const char *str)
{
    int rc = ioctl(fd, req, opt);

    if (rc==-1)
    {
        cout << "Error! Ioctl " << req << ": ";
        cout << (str?str:strerror(errno)) << " (rc=" << rc << ")" << endl;
    }

    return rc;
}

void Camera::Error(const char *str, int fatal)
{
    cout << endl
        << (fatal?"Fatal ":"") << "Error! " << str << ": " << strerror(errno)
        << endl;

    if (fatal)
        exit(1);
}
void Camera::SigInit()
{
    struct sigaction act, old;

    memset(&act, 0, sizeof(act));

    act.sa_handler = SigAlarm;

    sigemptyset(&act. sa_mask);
    sigaction(SIGALRM, &act, &old);

    // signal(SIGINT, ctrlc);
}

void Camera::SigAlarm(int signal)
{
    cout << "Camera: oops: got sigalarm" << endl;
    exit(1);
}

char *Camera::GetImg(unsigned int frame)
{
    // wait until grabbing is finished??
    //
    // give signal SIGALARM
    const int SYNC_TIMEOUT = 1;

    alarm(SYNC_TIMEOUT);
    Ioctl(VIDIOCSYNC, &frame); // sync with mmap grabbing
    alarm(0);

    return pMapBuffer+iOffsets[frame];
}

int Camera::StartGrab(unsigned int frame)
{
    static struct video_mmap gb =
    {
        0,                     // frame
        rows, cols,            // height, width
        VIDEO_PALETTE_RGB24    // palette
    };

    gb.frame = frame&1;

    //
    // capture frame
    //
    if (Ioctl(VIDIOCMCAPTURE, &gb) != -1)
        return true;

    if (errno == EAGAIN)
        cout << "Grabber chip can't sync" << endl;

    return false;
}

Camera::Camera() : fd(-1), iBufferSize(0)
{
    cout << "Starting thread..." << flush;
    pthread_cond_init(&fCond, NULL);
    pthread_mutex_init(&fMux, NULL);
    pthread_mutex_lock(&fMux);
    pthread_create(&fThread, NULL, MapThread, this);
    cout << "done." << endl;

    cout << "/dev/video: opening..." << flush;

    //
    // ------ Open device /dev/video ------
    //
    do
    {
        fd = open("/dev/video", O_RDWR);
        usleep(1);
    }
    while (errno==19 && fd==-1);

    if (fd == -1)
        Error("open /dev/video");

    fcntl(fd, F_SETFD, FD_CLOEXEC);  // Close device on exit
    SigInit();

    //
    // get input channel 0 information
    //
    struct video_channel ch;
    ch.channel = 0;
    Ioctl(VIDIOCGCHAN, &ch);

    //
    // ioctl probe, switch to input 0
    //
    Ioctl(VIDIOCSCHAN, &ch, "You need a bttv version > 0.5.13");

    //
    // map grab buffer, get size and offset
    //
    struct video_mbuf buffers;
    Ioctl(VIDIOCGMBUF, &buffers);

    iBufferSize = buffers.size;
    iOffsets[0] = buffers.offsets[0];
    iOffsets[1] = buffers.offsets[1];

    //
    // map file (device) into memory
    //
    pMapBuffer = mmap(0, iBufferSize, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);

    if ((int)pMapBuffer == -1)
        Error("mmap");

    cout << "OK." << endl;

    cout << "Buffer Offset " << buffers.offsets[1] << endl;
    cout << "grab: use: 768x576 24 bit TrueColor (LE: bgr)" << endl;
}

Camera::~Camera()
{
    cout << "Stopping thread..." << flush;
    pthread_cancel(fThread);
    pthread_mutex_destroy(&fMux);
    pthread_cond_destroy(&fCond);
    cout << "done." << endl;

    cout << "/dev/video: closing... " << flush;

    // unmap device memory
    if ((int)pMapBuffer != -1)
        munmap(pMapBuffer, iBufferSize);

    if (fd != -1)
    {
        close(fd);
        fd = -1;
    }

    cout << " Done." << endl;
}


void Camera::Execute(const unsigned long n,
                     byte *img,
                     struct timeval *tm)
{
    cout << "Img: " << n << "  " << (void*)img << endl;
}

void *Camera::MapThread(void *arg)
{
    Camera *cam = (Camera*)arg;

    // setpriority(PRIO_PROCESS, 0, 10);
    pthread_detach(pthread_self());

    cam->Thread();

    return 0;
}


void Camera::LoopStep(const unsigned long n)
{
    char *img = GetImg(n&1);

    gettimeofday(&fTime, NULL);

    const char *end = img + cols*rows*depth;
    char *beg = fImg;

    while (img < end)
    {
        *beg = *img;

        img += depth;
        beg++;
    }
}

void Camera::Thread()
{
#define IsOdd(i) (2*(i/2)!=i)
    while (1)
    {
        pthread_cond_wait(&fCond, &fMux);
        MStopwatch t;
        t.Start();

        unsigned long i=0;

        if (!StartGrab(0))
            continue;

        if (!StartGrab(1))
            continue;

        while (!(fStop || fNum && i==fNum-2))
        {
            LoopStep(i);
            if (!StartGrab(i&1))
                break;
            Execute(i, (byte*)fImg, &fTime);
            i++;
        }

        if (!IsOdd(i))
        {
            LoopStep(i);
            Execute(i, (byte*)fImg, &fTime);
            i++;
        }

        LoopStep(i);
        Execute(i, (byte*)fImg, &fTime);
        i++;

        //
        // Wait until processing of frame 1 finished.
        //
        t.Stop();
        t.Print(i);

        fRunning = 0;
    }
}

void Camera::Loop(unsigned long nof)
{
    if (2*(nof/2) != nof)
    {
        cout << "Sorry, only even values are allowed!" << endl;
        return;
    }

    //
    // Stop running loop
    //
    fStop = 1;

    //
    // Wait until loop is stopped (pthread_cond_wait is executing)
    // set new number of frames to process
    //
    pthread_mutex_lock(&fMux);
    fNum     = nof;
    fStop    = 0;
    fRunning = 1;
    pthread_mutex_unlock(&fMux);

    //
    // Start execution
    //
    pthread_cond_signal(&fCond);
}

void Camera::SetPicPar(int bright, int hue, int contrast)
{
    struct video_picture pict;

    Ioctl(VIDIOCGPICT, &pict);  // get

    if (contrast != -1)
        pict.contrast = contrast;

    if (bright != -1)
        pict.brightness = bright;

    if (hue != -1)
        pict.hue = hue;

    Ioctl(VIDIOCSPICT, &pict);  //set
}

void Camera::GetPicPar(int *bright, int *hue, int *contrast)
{
    struct video_picture pict;

    Ioctl(VIDIOCGPICT, &pict);   // get

    *contrast = pict.contrast;
    *bright   = pict.brightness;
    *hue      = pict.hue;
}