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

#include "Camera.h"

#include <iostream>
#include <errno.h>     // errono
#include <pthread.h>
#include <sys/ioctl.h> // ioctl
#include <sys/mman.h>  // PROT_READ

#include "MStopwatch.h"

#include "videodev.h"

ClassImp(Camera);

using namespace std;

inline int Camera::Ioctl(int req, void *opt, const char *str)
{
    while (1)
    {
        const int rc = ioctl(fd, req, opt);
        if (rc>=0)
            return rc;

        // errno== 4: Interrupted system call
        // errno==16: Device or resource busy
        if (errno==4 || errno==16)
        {
            cout << "Camera: ioctl returned rc=" << rc << " '";
            cout << (str?str:strerror(errno)) << "' (errno = " << errno << ")" << endl;
            usleep(10);
            continue;
        }

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

void Camera::Error(const char *str, int fatal)
{
    cout << endl;
    cout << (fatal?"Fatal ":"") << "Error! " << str << ": " << strerror(errno);
    cout << 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)
{
    // We could also get RGB555, RGB565 and RGB32. But we want
    // RGB24 because we have a 8bit DAC which gives us 8bit per
    // color ==> RGB24 which is in the following the most simple
    // to process.
    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(PixClient &client, Int_t nch) : fd(-1), iBufferSize(0), fClient(client), fCond(), fMutex(fCond.GetMutex())
{
    cout << "Camera: " << this << " /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 = nch;
    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 = (char*)mmap(0, iBufferSize, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);

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

    cout << "OK." << endl;

    cout << "Buffer Address: " << (void*)pMapBuffer << endl;
    cout << "Buffer Offset 1: " << (void*)iOffsets[0] << endl;
    cout << "Buffer Offset 2: " << (void*)iOffsets[1] << endl;
    cout << "Buffer Size: " << (void*)iBufferSize << endl;
    cout << "grab: use: 768x576 24 bit TrueColor (LE: bgr) = " << (void*)(768*576*3) << "b" << endl;

//    if (fMutex->UnLock()==13)
//        cout << "Camera::Camera - tried to unlock mutex locked by other thread." << endl;

    cout << "Starting thread..." << flush;
    //pthread_cond_init(&fCond, NULL);
    //pthread_mutex_init(&fMux, NULL);
    //pthread_mutex_lock(&fMux);
//    if (fMutex->Lock()==13)
//        cout << "Camera::Camera - mutex is already locked by this thread" << endl;
    pthread_create(&fThread, NULL, MapThread, this);
    cout << "done." << endl;

}

Camera::~Camera()
{
    pthread_cancel(fThread);

    cout << "/dev/video: closing... " << flush;
    if (fd != -1)
    {
        close(fd);
        fd = -1;
    }
    cout << "done." << endl;

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

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

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

    cam->Thread();

    return 0;
}

void Camera::ExitLoop()
{
    //    cout << "ExitLoop..." << endl;
    fStop = 1;
    while (IsRunning())
        usleep(1);
    //    cout << "Loop exited." << endl;
}

//
// flag if the execution is running or not
//
int Camera::IsRunning() const
{
    const Int_t rc = fMutex->TryLock();
    if (rc==0)
        return false;

    if (rc==13)
        return false;

    fMutex->UnLock();
    return true;
}

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;
    }
}

void Camera::Thread()
{
#define IsOdd(i) (2*(i/2)!=i)
    cout << "Camera::Thread started..." << endl;
    while (1)
    {
        //cout << "Wait..." << flush;
        fCond.Wait();
        //cout << "done." << endl;
        if (fd==-1)
            break;

        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;

            fClient.ProcessFrame(i, (byte*)fImg, &fTime);
            i++;
        }

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

        LoopStep(i);
        fClient.ProcessFrame(i, (byte*)fImg, &fTime);
        i++;

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

    fMutex->UnLock();

    cout << "Camera::Thread.. stopped." << endl;
}

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

    cout << "Loop..." << endl;
    ExitLoop();

    fNum  = nof;
    fStop = 0;

    //
    // Start execution
    //
    fCond.Broadcast();
}

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;
}