source: eviacam/trunk/fuentes/creavision/crvcamera_v4l2.cpp @ 567

Last change on this file since 567 was 567, checked in by mabarracus, 4 years ago

updated sources to version 2.0.3

File size: 58.4 KB
Line 
1/////////////////////////////////////////////////////////////////////////////
2// Name:        crvcamera_v4l2.cpp
3// Purpose:     Provide a camera capture class around v4l2 and libwebcam
4// Author:      Cesar Mauri Loba (cesar at crea-si dot com)
5// Modified by:
6// Created:     17/05/2010
7// Copyright:   (C) 2008-11 Cesar Mauri Loba - CREA Software Systems
8//              Portions of guvcview are (c) of Paulo Assis and others
9//
10//  This program is free software: you can redistribute it and/or modify
11//  it under the terms of the GNU General Public License as published by
12//  the Free Software Foundation, either version 3 of the License, or
13//  (at your option) any later version.
14//
15//  This program is distributed in the hope that it will be useful,
16//  but WITHOUT ANY WARRANTY; without even the implied warranty of
17//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18//  GNU General Public License for more details.
19//
20//  You should have received a copy of the GNU General Public License
21//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
22/////////////////////////////////////////////////////////////////////////////
23#include "crvcamera_v4l2.h"
24#include <string.h>
25#include <stdio.h>
26#include <sys/stat.h>
27#include <errno.h>
28#include <fcntl.h>
29#include <unistd.h>
30#include <limits.h>
31#include "incvideodev.h"
32#include <libv4l2.h>
33#include <sys/ioctl.h>
34#include <sys/mman.h>
35#include <sys/time.h>
36#include <list>
37#include "pwc-ioctl.h"
38#include "colorspaces.h"
39#include "simplelog.h"
40
41// TODO: thread safety
42
43
44// Define possibly missing entries from videodev2.h
45#ifndef V4L2_PIX_FMT_MJPEG
46#define V4L2_PIX_FMT_MJPEG  v4l2_fourcc('M', 'J', 'P', 'G') /*  MJPEG stream     */
47#endif
48
49#ifndef V4L2_PIX_FMT_JPEG
50#define V4L2_PIX_FMT_JPEG  v4l2_fourcc('J', 'P', 'E', 'G')  /*  JPEG stream      */
51#endif
52
53#ifndef V4L2_PIX_FMT_YUYV
54#define V4L2_PIX_FMT_YUYV    v4l2_fourcc('Y','U','Y','V')   /* YUV 4:2:2        */
55#endif
56
57#ifndef V4L2_PIX_FMT_YVYU
58#define V4L2_PIX_FMT_YVYU    v4l2_fourcc('Y','V','Y','U')   /* YUV 4:2:2        */
59#endif
60
61#ifndef V4L2_PIX_FMT_UYVY
62#define V4L2_PIX_FMT_UYVY    v4l2_fourcc('U','Y','V','Y')   /* YUV 4:2:2        */
63#endif
64
65#ifndef V4L2_PIX_FMT_YYUV
66#define V4L2_PIX_FMT_YYUV    v4l2_fourcc('Y','Y','U','V')   /* YUV 4:2:2        */
67#endif
68
69#ifndef V4L2_PIX_FMT_YUV420
70#define V4L2_PIX_FMT_YUV420  v4l2_fourcc('Y','U','1','2')   /* YUV 4:2:0 Planar  */
71#endif
72
73#ifndef V4L2_PIX_FMT_YVU420
74#define V4L2_PIX_FMT_YVU420  v4l2_fourcc('Y','V','1','2')   /* YUV 4:2:0 Planar  */
75#endif
76
77#ifndef V4L2_PIX_FMT_NV12
78#define V4L2_PIX_FMT_NV12  v4l2_fourcc('N','V','1','2')   /* YUV 4:2:0 Planar (u/v) interleaved */
79#endif
80
81#ifndef V4L2_PIX_FMT_NV21
82#define V4L2_PIX_FMT_NV21  v4l2_fourcc('N','V','2','1')   /* YUV 4:2:0 Planar (v/u) interleaved */
83#endif
84
85#ifndef V4L2_PIX_FMT_NV16
86#define V4L2_PIX_FMT_NV16  v4l2_fourcc('N','V','1','6')   /* YUV 4:2:2 Planar (u/v) interleaved */
87#endif
88
89#ifndef V4L2_PIX_FMT_NV61
90#define V4L2_PIX_FMT_NV61  v4l2_fourcc('N','V','6','1')   /* YUV 4:2:2 Planar (v/u) interleaved */
91#endif
92
93#ifndef V4L2_PIX_FMT_Y41P
94#define V4L2_PIX_FMT_Y41P  v4l2_fourcc('Y','4','1','P')    /* YUV 4:1:1          */
95#endif
96
97#ifndef V4L2_PIX_FMT_GREY
98#define V4L2_PIX_FMT_GREY  v4l2_fourcc('G','R','E','Y')    /*      Y only       */
99#endif
100
101#ifndef V4L2_PIX_FMT_SPCA501
102#define V4L2_PIX_FMT_SPCA501 v4l2_fourcc('S','5','0','1')  /* YUYV - by line     */
103#endif
104
105#ifndef V4L2_PIX_FMT_SPCA505
106#define V4L2_PIX_FMT_SPCA505 v4l2_fourcc('S','5','0','5')  /* YYUV - by line     */
107#endif
108
109#ifndef V4L2_PIX_FMT_SPCA508
110#define V4L2_PIX_FMT_SPCA508 v4l2_fourcc('S','5','0','8')  /* YUVY - by line     */
111#endif
112
113#ifndef V4L2_PIX_FMT_SGBRG8
114#define V4L2_PIX_FMT_SGBRG8  v4l2_fourcc('G', 'B', 'R', 'G') /* GBGB.. RGRG..    */
115#endif
116
117#ifndef V4L2_PIX_FMT_SGRBG8
118#define V4L2_PIX_FMT_SGRBG8  v4l2_fourcc('G', 'R', 'B', 'G') /* GRGR.. BGBG..    */
119#endif
120
121#ifndef V4L2_PIX_FMT_SBGGR8
122#define V4L2_PIX_FMT_SBGGR8  v4l2_fourcc('B', 'A', '8', '1') /* BGBG.. GRGR..    */
123#endif
124
125#ifndef V4L2_PIX_FMT_SRGGB8
126#define V4L2_PIX_FMT_SRGGB8  v4l2_fourcc('R', 'G', 'G', 'B') /* RGRG.. GBGB..    */
127#endif
128
129#ifndef V4L2_PIX_FMT_BGR24
130#define V4L2_PIX_FMT_BGR24   v4l2_fourcc('B', 'G', 'R', '3') /* 24  BGR-8-8-8    */
131#endif
132
133#ifndef V4L2_PIX_FMT_RGB24
134#define V4L2_PIX_FMT_RGB24   v4l2_fourcc('R', 'G', 'B', '3') /* 24  RGB-8-8-8    */
135#endif
136
137CCameraV4L2::CCameraV4L2(int cameraId, unsigned int width, unsigned int height, 
138                float fr) throw(camera_exception)
139{
140        InstanceCreated();
141        if (cameraId>= GetNumDevices()) {
142                InstanceDestroyed();
143                throw camera_exception("wrong camera id");
144        }
145        m_Id= cameraId;
146       
147        m_desiredFormat.frame_rate= (unsigned int) fr;
148        m_desiredFormat.width= width;
149        m_desiredFormat.height= height;
150        m_desiredFormat.pixelformat= 0;
151
152        m_libWebcamHandle= 0;
153        m_captureMethod= CAP_NONE;
154        m_isStreaming= false;
155        m_buffersReady= false;
156        memset(&m_captureBuffersInfo, 0, sizeof(struct v4l2_buffer) * STREAMING_CAPTURE_NBUFFERS);
157        memset(&m_captureBuffersPtr, 0, sizeof(void*) * STREAMING_CAPTURE_NBUFFERS);   
158        memset(&m_currentFormat, 0, sizeof(m_currentFormat));
159        AddSupportedPixelFormats ();
160}
161
162CCameraV4L2::~CCameraV4L2(void)
163{
164    Close ();
165        InstanceDestroyed();
166}
167
168bool CCameraV4L2::DoOpen ()
169{
170        slog_write (SLOG_PRIO_DEBUG, "CCameraV4L2::DoOpen: begin\n");
171        if (m_libWebcamHandle!= 0) {
172                slog_write (SLOG_PRIO_NOTICE, "CCameraV4L2::DoOpen: already open\n");
173                return true;    // Already open
174        }
175        if (!InternalOpen()) {
176                slog_write (SLOG_PRIO_ERR, "CCameraV4L2::DoOpen: open failed\n");
177                return false;
178        }
179       
180        assert (m_desiredFormat.width && m_desiredFormat.height && m_desiredFormat.frame_rate);
181        m_currentFormat= m_desiredFormat;
182               
183        if (!DetectBestImageFormat()) {
184                slog_write (SLOG_PRIO_ERR, "Unable to find any suitable image format\n");
185                Close();
186                return false;
187        }
188        if (!SetImageFormat()) {
189                Close();
190                return false;
191        }
192        m_captureMethod= DetectCaptureMethod();
193        if (m_captureMethod== CAP_NONE) {
194                slog_write (SLOG_PRIO_ERR, "Unable to find a suitable capure mode\n");
195                Close();
196                return false;
197        }
198       
199        if (!AllocateBuffers()) {
200                slog_write (SLOG_PRIO_ERR, "Unable to allocate buffers\n");
201                Close();
202                return false;
203        }
204       
205        if (!EnableVideo(true)) {
206                slog_write (SLOG_PRIO_ERR, "Unable to enable video\n");
207                DeallocateBuffers();
208                Close();
209                return false;
210        }
211       
212        // TODO: Awful. This is a provisional solution to avoid broken frames while capturing.
213        // It seems as if the driver/camera needs some time before start grabbing.
214        usleep (2000000);
215        return true;
216}
217
218void CCameraV4L2::DoClose ()
219{
220        if (m_isStreaming) EnableVideo(false); 
221        if (m_buffersReady) DeallocateBuffers();
222        if (m_libWebcamHandle!= 0) {
223                c_close_device (m_libWebcamHandle);
224                m_libWebcamHandle= 0;
225        }
226        m_captureMethod= CAP_NONE;
227        m_cameraControls.clear();
228}
229
230bool CCameraV4L2::InternalOpen()
231{
232        char devName[CAM_DEVICE_SHORT_NAME_LENGHT+5];
233        struct stat st; 
234
235        // Create device name
236        snprintf (devName, CAM_DEVICE_SHORT_NAME_LENGHT+5, "/dev/%s", g_deviceShortNames[m_Id]);
237
238        // Check if exists and if it is a device
239        if (stat (devName, &st)== -1) {
240                slog_write (SLOG_PRIO_ERR, "Cannot identify ’%s’: %d, %s\n", devName, errno, strerror (errno));
241                return false;
242        }
243        if (!S_ISCHR (st.st_mode)) {
244                slog_write (SLOG_PRIO_ERR, "%s is no device\n", devName);
245                return false;
246        }
247       
248        // "Open" device via libwebcam
249        m_libWebcamHandle= c_open_device (g_deviceShortNames[m_Id]); //m_deviceShortName);
250        if (m_libWebcamHandle== 0 || c_get_file_descriptor (m_libWebcamHandle)<= 0) {
251                slog_write (SLOG_PRIO_ERR, "Cannot open ’%s’ via libwebcam\n", devName);
252                Close();
253                return false;
254        }
255       
256        PopulateCameraControls ();
257         
258        return true;
259}
260
261/* ioctl with a number of retries in the case of failure
262* args:
263* fd - device descriptor
264* IOCTL_X - ioctl reference
265* arg - pointer to ioctl data
266* returns - ioctl result
267* Based on xioctl from guvcview
268*/
269// TODO: this is code borrowed from Paulo's guvcview, but here
270// it seems useless because we don't open the device in non-blocking mode
271// Considered either removing or opening device in non-blocking mode
272// See sleep TODO below
273
274#define IOCTL_RETRY 4
275static
276int xioctl(int fd, int IOCTL_X, void *arg)
277{
278        int ret = 0;
279        int tries= IOCTL_RETRY;
280        do {
281                ret = v4l2_ioctl(fd, IOCTL_X, arg);
282        } 
283        while (ret && tries-- && ((errno == EINTR) || (errno == EAGAIN) || (errno == ETIMEDOUT)));
284
285        if (ret && (tries <= 0)) 
286                slog_write (SLOG_PRIO_ERR, "ioctl (%i) retried %i times - giving up: %s)\n", IOCTL_X, IOCTL_RETRY, strerror(errno));
287       
288        return (ret);
289} 
290
291void CCameraV4L2::AddSupportedPixelFormats ()
292{
293        // Adds supported pixel formats in preferred order
294        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_RGB24);
295        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_YUYV);
296        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_YUV420);
297        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SGBRG8);
298        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SGRBG8);
299        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SBGGR8);
300        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SRGGB8);
301        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_UYVY);
302        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_YVYU);
303        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_YYUV);
304        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_YVU420);
305        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_NV12);
306        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_NV21);
307        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_NV16);
308        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_NV61);
309        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_Y41P);
310        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_GREY);
311        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SPCA501);
312        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SPCA505);
313        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_SPCA508);
314        m_supportedPixelFormats.push_back (V4L2_PIX_FMT_BGR24);
315}
316
317bool CCameraV4L2::PopulateCameraControls ()
318{
319        CControl *controls= NULL;
320        unsigned int size= 0, count= 0;
321
322        // First call to discover required buffer size
323        if (c_enum_controls(m_libWebcamHandle, controls, &size, &count)!= C_BUFFER_TOO_SMALL) return false;
324        else {
325                // Ok, allocate buffer and query information
326                assert (size> 0);
327                unsigned char buffer[size];
328                controls= (CControl *) buffer;
329                if (c_enum_controls(m_libWebcamHandle, controls, &size, &count)!= C_SUCCESS) return false;
330               
331                // Populate camera control vector
332                for (unsigned int i= 0; i< count; ++i) {
333                        if (controls[i].type!=  CC_TYPE_RAW)
334                                // Ignore raw controls
335                                m_cameraControls.push_back(CCameraControlV4L2(m_libWebcamHandle, controls[i]));
336                }
337        }
338        return true;
339}
340
341unsigned int CCameraV4L2::GetCameraControlsCount()
342{
343        return static_cast<unsigned int>(m_cameraControls.size());
344}
345
346CCameraControl* CCameraV4L2::GetCameraControl(unsigned int numControl)
347{
348        if (numControl>= GetCameraControlsCount()) return NULL;
349        return &m_cameraControls[numControl];
350}
351
352// Do VIDIOC_REQBUFS   
353bool CCameraV4L2::RequestBuffers(enum v4l2_memory mem)
354{
355        struct v4l2_requestbuffers requestbuffers;
356
357        memset (&requestbuffers, 0, sizeof(requestbuffers));
358        requestbuffers.count= STREAMING_CAPTURE_NBUFFERS;
359        requestbuffers.type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
360        requestbuffers.memory= mem; //V4L2_MEMORY_MMAP;
361        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_REQBUFS, &requestbuffers)== 0) {
362                if (requestbuffers.count== STREAMING_CAPTURE_NBUFFERS) return true;
363                if (requestbuffers.count> 0) UnRequestBuffers(mem);             
364        }
365        slog_write (SLOG_PRIO_ERR, "ERROR: RequestBuffers: failed\n");
366        return false;
367}
368
369// Undo VIDIOC_REQBUFS
370bool CCameraV4L2::UnRequestBuffers(enum v4l2_memory mem)
371{
372        struct v4l2_requestbuffers requestbuffers;
373
374        memset (&requestbuffers, 0, sizeof(requestbuffers));
375        requestbuffers.count= 0;
376        requestbuffers.type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
377        requestbuffers.memory= mem; //V4L2_MEMORY_MMAP;
378        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_REQBUFS, &requestbuffers)== 0) return true;
379        slog_write (SLOG_PRIO_ERR, "ERROR: UnRequestBuffers: failed\n");
380        return false;
381}
382       
383// This method must be called AFTER desired format is set       
384CCameraV4L2::ECaptureMethod CCameraV4L2::DetectCaptureMethod()
385{
386        struct v4l2_capability capability;     
387
388        // Query basic capabilities. This never should fail
389        if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_QUERYCAP, &capability)!= 0) {
390                slog_write (SLOG_PRIO_WARNING, "Cannot query camera capabilities: VIDIOC_QUERYCAP ioctl failed\n");
391                return CAP_NONE;
392        }
393
394        if (!(capability.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
395            slog_write (SLOG_PRIO_WARNING, "This is no video capture device\n");
396            return CAP_NONE;
397        }
398
399        // Driver supports streaming
400        if ((capability.capabilities & V4L2_CAP_STREAMING)) {
401                // Streaming supported. Which kind?
402                // It can be V4L2_MEMORY_MMAP or V4L2_MEMORY_USERPTR
403
404                // Check MMAP first as preferent option
405                if (RequestBuffers(V4L2_MEMORY_MMAP)) {
406                        UnRequestBuffers(V4L2_MEMORY_MMAP);
407                        return CAP_STREAMING_MMAP;
408                }
409
410                // Check using user-allocated memory
411                if (RequestBuffers(V4L2_MEMORY_USERPTR)) {
412                        UnRequestBuffers(V4L2_MEMORY_USERPTR);
413                        return CAP_STREAMING_USR;
414                }
415        }
416        if (capability.capabilities & V4L2_CAP_READWRITE) return CAP_READ;
417
418        return CAP_NONE;
419}
420
421// Only used by DetectBestImageFormat
422typedef struct  {
423        unsigned int min_frame_rate;
424        unsigned int max_frame_rate;
425        unsigned int step_frame_rate;
426        unsigned int min_width;
427        unsigned int min_height;
428        unsigned int max_width;
429        unsigned int max_height;
430        unsigned int step_width;
431        unsigned int step_height;               
432        uint32_t pixelformat;   // Four character code 
433} TImageFormatEx;
434
435static 
436void dump_TImageFormatEx (TImageFormatEx& ife)
437{
438        slog_write (SLOG_PRIO_DEBUG, "dump_TImageFormatEx. fr: %u, %u, %u w: %u, %u, %u h: %u, %u, %u px: %x\n",
439                ife.min_frame_rate, ife.max_frame_rate, ife.step_frame_rate,
440                ife.min_width, ife.max_width, ife.step_width,
441                ife.min_height, ife.max_height, ife.step_height,
442                ife.pixelformat);
443}
444
445// Given a range returns the absolute distance to a value
446template <class T>
447T abs_distance_to_range (T min, T max, T val)
448{
449        if (val< min) return min-val;
450        if (val< max) return 0;
451        return val-max;
452}
453
454// Given a steped range and a desired value, selects a value within
455// the range as near as possible to the desired value
456static 
457uint range_value_fit (uint min, uint max, uint step, uint val)
458{
459        if (val<= min) return min;
460        if (val>= max) return max;
461       
462        // Adjust step
463        if (step== 0) step= 1;
464        val-= (val % step);
465        if (val< min) val= min;
466        return val;     
467}
468
469
470// Look for best matching entries for a desired frame rate and remove all other entries
471// fr= 0 means that nothing is done (all formats are appropriate), fr=UINT_MAX selects highest
472// frame rate
473static void SelectBestFrameRate (unsigned int fr, std::list<TImageFormatEx>& availableFormats)
474{
475        if (fr> 0) {   
476                unsigned int bestdiff= UINT_MAX;
477                // Find closest frame ratio     
478                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end(); ++i) {
479                        unsigned int diff= abs_distance_to_range<uint> (i->min_frame_rate, i->max_frame_rate, fr);
480                        if (diff< bestdiff) bestdiff= diff;
481                }
482                // Remove worse entries
483                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end();)
484                        if (abs_distance_to_range<uint> (i->min_frame_rate, i->max_frame_rate, fr)!= bestdiff) 
485                                i= availableFormats.erase(i);
486                        else
487                                ++i;
488               
489                assert (availableFormats.size()> 0);
490        }
491}
492
493// Sizes. Chooses closest number of pixels (width*height) to the requested
494static void SelectBestFramePixelNumber (unsigned int npixels, std::list<TImageFormatEx>& availableFormats)
495{
496        if (npixels> 0) {
497                unsigned int bestdiff= UINT_MAX;
498
499                // Find closest frame ratio     
500                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end(); ++i) {
501                        unsigned int diff= abs_distance_to_range<uint> (i->min_width * i->min_height, i->max_width * i->max_height, npixels);
502                        if (diff< bestdiff) bestdiff= diff;
503                }
504                // Remove worse entries
505                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end();)
506                        if (abs_distance_to_range<uint> (i->min_width * i->min_height, i->max_width * i->max_height, npixels)!= bestdiff) 
507                                i= availableFormats.erase(i);
508                        else
509                                ++i;
510               
511                assert (availableFormats.size()> 0);           
512        }
513}
514
515
516// Given a set of desired parameters finds the most suitable configuration which the camera supports
517//
518// Initialize the fields of the TImageFormat struct with the desired value for
519// each field or 0 to express that any possibility is fine or UINT_MAX to request
520// the maximum available value.
521
522bool CCameraV4L2::DetectBestImageFormat()
523{
524        std::list<TImageFormatEx> availableFormats;
525
526        //
527        // First build an array containing all possible formats
528        //
529       
530        unsigned int sformats, ssizes, sintervals;
531        unsigned int cformats, csizes, cintervals;
532       
533        // Request needed buffer to store formats
534        sformats= cformats= 0; 
535        if (c_enum_pixel_formats (m_libWebcamHandle, NULL, &sformats, &cformats)!= C_BUFFER_TOO_SMALL) return false;
536        else {
537                unsigned char bufferformats[sformats];
538                CPixelFormat* formats= (CPixelFormat*) bufferformats;
539                // Get formats
540                if (c_enum_pixel_formats (m_libWebcamHandle, formats, &sformats, &cformats)!= C_SUCCESS) return false;
541               
542                for (unsigned int ifo= 0; ifo< cformats; ++ifo) {
543                        TImageFormatEx tmpif;
544                        memset(&tmpif, 0, sizeof(tmpif));
545                       
546                        // Store pixel format
547                        tmpif.pixelformat= v4l2_fourcc (formats[ifo].fourcc[0], formats[ifo].fourcc[1], formats[ifo].fourcc[2], formats[ifo].fourcc[3]);
548                                               
549                        // Request needed buffer to store sizes
550                        ssizes= csizes= 0;
551                        if (c_enum_frame_sizes (m_libWebcamHandle, &formats[ifo], NULL, &ssizes, &csizes)!= C_BUFFER_TOO_SMALL)
552                                // No frame sizes detected, should not happen but we cope with it anyway
553                                availableFormats.push_back(tmpif);                     
554                        else {
555                                unsigned char buffersizes[ssizes];
556                                CFrameSize* sizes= (CFrameSize*) buffersizes;
557                                // Get sizes
558                                if (c_enum_frame_sizes (m_libWebcamHandle, &formats[ifo], sizes, &ssizes, &csizes)!= C_SUCCESS) {
559                                        // Unlikely but we cope with it anyway
560                                        availableFormats.push_back(tmpif);
561                                        continue;
562                                }
563                               
564                                for (unsigned int is= 0; is< csizes; ++is) {
565                                        // Store size
566                                        if (sizes[is].type== CF_SIZE_CONTINUOUS) {
567                                                tmpif.min_width= sizes[is].min_width;
568                                                tmpif.min_height= sizes[is].min_height;
569                                                tmpif.max_width= sizes[is].max_width;
570                                                tmpif.max_height= sizes[is].max_height;
571                                                tmpif.step_width= sizes[is].step_width;
572                                                tmpif.step_height= sizes[is].step_height;
573                                        } 
574                                        else {
575                                                tmpif.min_width= sizes[is].width;
576                                                tmpif.min_height= sizes[is].height;
577                                                tmpif.max_width= sizes[is].width;
578                                                tmpif.max_height= sizes[is].height;
579                                                tmpif.step_width= 1;
580                                                tmpif.step_height= 1;
581                                        }
582                                       
583                                        // Request needed buffer to store intervals
584                                        sintervals= cintervals= 0;
585                                        if (c_enum_frame_intervals (m_libWebcamHandle, &formats[ifo], &sizes[is], NULL, &sintervals, &cintervals)!= C_BUFFER_TOO_SMALL)
586                                                // No intervals detected. Some cameras doesn't provide this information
587                                                availableFormats.push_back(tmpif);
588                                        else {
589                                                unsigned char bufferintervals[sintervals];
590                                                CFrameInterval* intervals= (CFrameInterval*) bufferintervals;
591                                                // Get intervals
592                                                if (c_enum_frame_intervals (m_libWebcamHandle, &formats[ifo], &sizes[is], intervals, &sintervals, &cintervals)!= C_SUCCESS) {
593                                                        // Unlikely but we cope with it anyway
594                                                        availableFormats.push_back(tmpif);
595                                                        continue;
596                                                }
597                                               
598                                                for (unsigned int ii= 0; ii< cintervals; ++ii) {
599                                                        // Store frame rate
600                                                        if (intervals[ii].type== CF_INTERVAL_DISCRETE) {
601                                                                tmpif.max_frame_rate= tmpif.min_frame_rate= (intervals[ii].n? intervals[ii].d / intervals[ii].n : 0);
602                                                                tmpif.step_frame_rate= 1;
603                                                        }
604                                                        else {
605                                                                tmpif.max_frame_rate= (intervals[ii].max_n? intervals[ii].max_d / intervals[ii].max_n : 0);
606                                                                tmpif.min_frame_rate= (intervals[ii].min_n? intervals[ii].min_d / intervals[ii].min_n : 0);
607                                                                tmpif.step_frame_rate= (intervals[ii].step_n? intervals[ii].step_d / intervals[ii].step_n : 0);
608                                                                if (tmpif.step_frame_rate== 0) tmpif.step_frame_rate= 1;
609                                                        }
610                                                       
611                                                        availableFormats.push_back(tmpif);                             
612                                                }
613                                        }
614                                }
615                               
616                        }                       
617                }
618        }
619
620        if (slog_get_priority ()>= SLOG_PRIO_DEBUG) {
621                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end(); ++i) {
622                        dump_TImageFormatEx (*i);
623                }
624        }
625
626        //
627        // Selection process
628        //
629       
630        // Filter by compatible pixel formats. Remove entries which use non supported encodings
631        for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end();) {
632                bool found= false;
633                for (unsigned int ienc= 0; !found && ienc< m_supportedPixelFormats.size(); ++ienc)
634                        if (m_supportedPixelFormats[ienc]== i->pixelformat) found= true;
635               
636                if (!found) {
637                        char* tmp= (char *) &i->pixelformat;
638                        slog_write (SLOG_PRIO_NOTICE, 
639                                "crvcamera_v4l2: discarding unsuported format: %c%c%c%c\n",
640                                tmp[0], tmp[1], tmp[2], tmp[3]);
641                        i= availableFormats.erase (i);
642                }
643                else ++i;
644        }
645
646        // No available formats detected
647        if (availableFormats.size()== 0) return false;
648
649        // As for realtime computer vision frame rate is usually a critical parameter we first choose it.
650        SelectBestFrameRate (m_currentFormat.frame_rate, availableFormats);
651       
652        // Sizes. Chooses closest number of pixel (width*height) to the requested
653        SelectBestFramePixelNumber (m_currentFormat.width * m_currentFormat.height, availableFormats);
654       
655        // Check aspect ratio
656        //TODO: Check weird errors. floating point errors.     
657        if (m_currentFormat.width> 0 && m_currentFormat.height> 0) {
658                float bestdiff= FLT_MAX;
659                float aratio= (float) m_currentFormat.width / (float) m_currentFormat.height;
660                // Find closest frame ratio
661                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end(); ++i) {
662                        float diff= abs_distance_to_range<float> ((float)i->min_width / (float)i->max_height, (float)i->max_width / (float)i->min_height, aratio);
663                        if (diff< bestdiff) bestdiff= diff;
664                }
665                // Remove worst entries
666                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end();)
667                {
668                        if (abs_distance_to_range<float> ((float)i->min_width / (float)i->max_height, (float)i->max_width / (float)i->min_height, aratio)> bestdiff) 
669                                i= availableFormats.erase(i);
670                        else
671                                ++i;
672                }
673                assert (availableFormats.size()> 0);
674        }
675
676       
677        // If frame rate not explicity specified then selects highest fr available
678        if (m_currentFormat.frame_rate== 0) {
679                m_currentFormat.frame_rate= UINT_MAX;
680                SelectBestFrameRate (m_currentFormat.frame_rate, availableFormats);
681        }
682       
683        // If frame size not explicity specified then selects bigger frame size available
684        if (m_currentFormat.width== 0 || m_currentFormat.height== 0) {
685                if (!m_currentFormat.width) m_currentFormat.width= UINT_MAX;
686                if (!m_currentFormat.height) m_currentFormat.height= UINT_MAX;         
687                SelectBestFramePixelNumber (UINT_MAX, availableFormats);
688        }
689       
690        // Finally chooses best available pixelformat
691        for (unsigned int ienc= 0; ienc< m_supportedPixelFormats.size(); ++ienc) {             
692                for (std::list<TImageFormatEx>::iterator i= availableFormats.begin(); i!= availableFormats.end(); ++i) {
693                        if (m_supportedPixelFormats[ienc]== i->pixelformat) {
694                                // Bingo! Store data and finish
695                                m_currentFormat.pixelformat= m_supportedPixelFormats[ienc];
696                                m_currentFormat.frame_rate= range_value_fit (i->min_frame_rate, i->max_frame_rate, i->step_frame_rate, m_currentFormat.frame_rate);
697                                m_currentFormat.width= range_value_fit (i->min_width, i->max_width, i->step_width, m_currentFormat.width);
698                                m_currentFormat.height= range_value_fit (i->min_height, i->max_height, i->step_height, m_currentFormat.height);
699                                slog_write (SLOG_PRIO_DEBUG, "Chosen format\n");
700                                dump_TImageFormatEx (*i);
701                                return true;
702                        }
703                }
704        }
705       
706        // Execution should never reach this point
707        assert (false);
708        return false;
709}
710
711bool CCameraV4L2::SetImageFormat()
712{
713        struct v4l2_format format;
714       
715        //
716        // Set frame format, including width, height and pixelformat. First query current
717        // settings, then modify relevant values and finally query actual values
718        //
719        memset (&format, 0, sizeof (format));
720        format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
721        if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_G_FMT, &format)== -1) {
722                slog_write (SLOG_PRIO_ERR, "Unable to get format.\n");
723                return false;   
724        }
725       
726        format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
727        format.fmt.pix.width = m_currentFormat.width;
728        format.fmt.pix.height = m_currentFormat.height;
729        format.fmt.pix.pixelformat = m_currentFormat.pixelformat;
730        format.fmt.pix.field = V4L2_FIELD_ANY;
731
732        if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_FMT, &format)== -1) {
733                slog_write (SLOG_PRIO_ERR, "Unable to set format.\n");
734                return false;   
735        }
736       
737        // Store currently set format (last VIDIOC_S_FMT may have changed it)
738        //m_currentFormat.frame_rate= imgformat.frame_rate;
739        m_currentFormat.width= format.fmt.pix.width;
740        m_currentFormat.height= format.fmt.pix.height;
741        m_currentFormat.pixelformat= format.fmt.pix.pixelformat;       
742
743        //     
744        // Set framerate and other camera parameters.
745        //
746        // We here try to cope with two common drivers: pwc and uvc. Unfortunately pwc driver versions above
747        // 10.0.12 removed support for many ioctls (including that to set the framerate VIDIOCSWIN).
748        // As of kernel version 3.0.0 (which ships pwc 10.0.14) this issue has not been fixed and the odds are
749        // it won't be as the hardware has been discontinued :-(
750        //
751        if (strcasestr(g_deviceDriverNames[m_Id], "pwc")!= NULL)  {
752                // Using a PWC based camera.
753                bool properlySet= false;
754                struct video_window vwin;               
755                if ((xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOCGWIN, &vwin) == 0) && (vwin.flags & PWC_FPS_FRMASK)) {
756                        vwin.flags &= ~PWC_FPS_FRMASK;
757                        vwin.flags |= (m_currentFormat.frame_rate << PWC_FPS_SHIFT);
758                        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOCSWIN, &vwin) == 0) properlySet= true;
759                }
760
761                if (!properlySet) 
762                        slog_write (SLOG_PRIO_WARNING, 
763                                "Cannot set FPS: %d for PWC camera\n", m_currentFormat.frame_rate);
764        }
765        else {
766                struct v4l2_streamparm parm;
767
768                // An UVC camera is assumed
769                memset(&parm, 0, sizeof (parm));
770                parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
771       
772                // Firstly try to get current v4l2_streamparm parameters
773                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_G_PARM, &parm)== 0) {
774                        parm.parm.capture.timeperframe.numerator = 1;
775                        parm.parm.capture.timeperframe.denominator = m_currentFormat.frame_rate;
776                        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_PARM, &parm)!= 0) {
777                                // Show warning and continue
778                                slog_write (SLOG_PRIO_WARNING, 
779                                        "Cannot set FPS: %d for UVC camera\n", 
780                                        m_currentFormat.frame_rate);
781                        }
782                       
783                        // Read values again and store actual values
784                        if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_G_PARM, &parm)== 0) {
785                                // Set failed, store read values
786                                m_currentFormat.frame_rate= 0;
787                                if (parm.parm.capture.timeperframe.denominator)
788                                        m_currentFormat.frame_rate= parm.parm.capture.timeperframe.denominator / parm.parm.capture.timeperframe.numerator;
789                        }
790                        else
791                                slog_write (SLOG_PRIO_WARNING, "Warning: cannot read again VIDIOC_G_PARM\n");
792                }
793                else {
794                        slog_write (SLOG_PRIO_ERR, "VIDIOC_G_PARM for UVC camera\n");
795                        return false;
796                }
797       
798                //
799                // Control V4L2_CID_EXPOSURE_AUTO is key to get a constant capture rate & good
800                // lightning conditions. In order of preference, the value of this control should be:
801                //
802                //   - V4L2_EXPOSURE_APERTURE_PRIORITY: Auto exposure time, manual iris. In practice,
803                //     when (V4L2_CID_EXPOSURE_AUTO_PRIORITY== false) and the frame rate is previously
804                //     set via VIDIOC_S_PARM, provides a constant frame rate at top speed and changes
805                //     exposure time and gain automatically according to the lightning conditions.
806                //     Tested with the Logitech Webcam 9000 (046d:0990). It seems that uvcvideo drivers
807                //     for newer kernels (3.0 and above) set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_MANUAL
808                //     by default instead of V4L2_EXPOSURE_SHUTTER_PRIORITY (at least for this camera model)
809                //     which came as default option for older kernels.
810                //
811                //   - V4L2_EXPOSURE_SHUTTER_PRIORITY: ??? (not tested)
812                //
813                //   - V4L2_EXPOSURE_MANUAL: Manual exposure time, manual iris. Provides maximum flexibility
814                //     and allows for constant frame but i) does not cope well variable lightnint conditions
815                //     (forces user to manually adjust settings when needed) ii) it seems that default gain
816                //     and exposure values are undefined and thus they need to be set to reasonable
817                //     defaults and also suggest that camera controls should be permanently stored.
818                //
819                //   - V4L2_EXPOSURE_AUTO: Automatic exposure time, automatic iris aperture. Automatically
820                //     reduces the frame rate depending on the lightning conditions and thus should be avoided.
821                //
822                // TODO: now uses the V4L2 api directly instead of libwebcam as some controls are not available
823                // any more.
824                // TODO: store control settings permanently
825                //
826               
827       
828                struct v4l2_control control;
829                memset (&control, 0, sizeof (control));
830
831                /*
832                control.id= V4L2_CID_EXPOSURE_AUTO;
833                control.value= -1;
834                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_G_CTRL, &control))
835                        fprintf (stderr, "Warning: cannot get V4L2_CID_EXPOSURE_AUTO\n");
836                else {
837                        fprintf (stderr, "Info: V4L2_CID_EXPOSURE_AUTO= %d\n", control.value);
838                }
839               
840
841                for (int i= 0; i<= 16; ++i) {
842                        control.id= V4L2_CID_EXPOSURE_AUTO;
843                        control.value= i;
844                        if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_CTRL, &control))
845                                fprintf (stderr, "Warning: cannot set V4L2_CID_EXPOSURE_AUTO to %d\n", i);
846                        else
847                                fprintf (stderr, "Info: set V4L2_CID_EXPOSURE_AUTO to %d\n", i);
848                }
849                */
850
851                //
852                // First set manual mode with reasonable exposure and gain values.
853                //
854                control.id= V4L2_CID_EXPOSURE_AUTO;
855                control.value= V4L2_EXPOSURE_MANUAL;
856                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_CTRL, &control))
857                        slog_write (SLOG_PRIO_WARNING, 
858                                "cannot set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_MANUAL\n");
859                else
860                        slog_write (SLOG_PRIO_INFO, 
861                                "set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_MANUAL\n");
862
863                // Exposure time to match requested FPS
864                control.id= V4L2_CID_EXPOSURE_ABSOLUTE;
865                control.value= 10000 / m_currentFormat.frame_rate;
866                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_CTRL, &control))
867                        slog_write (SLOG_PRIO_WARNING, "cannot set V4L2_CID_EXPOSURE_ABSOLUTE to %d\n", control.value);
868                else
869                        slog_write (SLOG_PRIO_INFO, "set V4L2_CID_EXPOSURE_ABSOLUTE to %d\n", control.value);
870               
871                // Sets gain to the maximum value
872                for (unsigned int i= 0; i< m_cameraControls.size(); ++i) {
873                        if (m_cameraControls[i].GetId()== CCameraControl::CAM_GAIN) {
874                                if (m_cameraControls[i].SetValue(m_cameraControls[i].GetMaximumValue()))
875                                        slog_write (SLOG_PRIO_INFO, "set CAM_GAIN to %d\n", 
876                                                m_cameraControls[i].GetMaximumValue());
877                                else
878                                        slog_write (SLOG_PRIO_WARNING, "cannot set CAM_GAIN to %d\n", 
879                                                m_cameraControls[i].GetMaximumValue());
880                                break;
881                        }
882                }
883                       
884                // Tries to set automatic gain
885                control.id= V4L2_CID_AUTOGAIN;
886                control.value= 1;
887                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_CTRL, &control))
888                        slog_write (SLOG_PRIO_WARNING, "cannot set V4L2_CID_AUTOGAIN\n");
889                else
890                        slog_write (SLOG_PRIO_INFO, "set V4L2_CID_AUTOGAIN\n");
891
892                // Set V4L2_CID_EXPOSURE_AUTO_PRIORITY to false
893                bool hasExposureAutoPriority= false;
894                for (unsigned int i= 0; i< m_cameraControls.size(); ++i) {
895                        if (m_cameraControls[i].GetId()== CCameraControl::CAM_AUTO_EXPOSURE_PRIORITY) {
896                                if (m_cameraControls[i].SetValue(0)) {
897                                        hasExposureAutoPriority= true;
898                                        slog_write (SLOG_PRIO_INFO, "Info: AUTO_EXPOSURE_PRIORITY disabled\n");
899                                }
900                                else
901                                        slog_write (SLOG_PRIO_WARNING, "cannot disable AUTO_EXPOSURE_PRIORITY\n");
902                                break;
903                        }
904                }
905
906                // If EXPOSURE_AUTO_PRIORITY cannot be disabled does not attempt remaing settings
907                if (!hasExposureAutoPriority) return true;     
908       
909                //
910                // Secondly tries to set V4L2_EXPOSURE_SHUTTER_PRIORITY mode
911                //
912                control.id= V4L2_CID_EXPOSURE_AUTO;
913                control.value= V4L2_EXPOSURE_SHUTTER_PRIORITY;
914                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_CTRL, &control))
915                        slog_write (SLOG_PRIO_WARNING, "cannot set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_SHUTTER_PRIORITY\n");
916                else
917                        slog_write (SLOG_PRIO_INFO, "set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_SHUTTER_PRIORITY\n");
918
919
920                // Finally tries to set V4L2_EXPOSURE_APERTURE_PRIORITY
921                control.id= V4L2_CID_EXPOSURE_AUTO;
922                control.value= V4L2_EXPOSURE_APERTURE_PRIORITY;
923                if (xioctl (c_get_file_descriptor (m_libWebcamHandle), VIDIOC_S_CTRL, &control))
924                        slog_write (SLOG_PRIO_WARNING, "cannot set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_APERTURE_PRIORITY\n");
925                else
926                        slog_write (SLOG_PRIO_INFO, "set V4L2_CID_EXPOSURE_AUTO to V4L2_EXPOSURE_APERTURE_PRIORITY\n");
927        }
928        return true;
929}
930
931void CCameraV4L2::UnmapBuffers()
932{
933        for (int i= STREAMING_CAPTURE_NBUFFERS-1; i>= 0; --i) {
934                if (m_captureBuffersPtr[i]!= NULL) {
935                        if (munmap(m_captureBuffersPtr[i], m_captureBuffersInfo[i].length)!= 0) 
936                                slog_write (SLOG_PRIO_ERR, "couldn't unmap buff: %s\n", strerror (errno));
937                        m_captureBuffersPtr[i]= NULL;
938                }
939        }
940}
941
942// Enable/disable video
943bool CCameraV4L2::EnableVideo(bool enable)
944{
945        if (m_libWebcamHandle== 0) return false;        // Camera not open, fail
946        if (enable== m_isStreaming) return true;
947       
948        switch(m_captureMethod) {               
949        case CAP_READ:
950                // capture using read, nothing to enable
951                break;
952        case CAP_STREAMING_MMAP: 
953            {
954                int type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
955                int action= (enable? VIDIOC_STREAMON : VIDIOC_STREAMOFF);
956                if (xioctl(c_get_file_descriptor (m_libWebcamHandle), action, &type)!= 0) {
957                        slog_write (SLOG_PRIO_ERR, "VIDIOC_STREAMON - Unable to start capture: %s\n", strerror (errno));
958                        return false;
959                }
960                break;
961            }   
962        case CAP_STREAMING_USR:
963                // Not implemented
964                slog_write (SLOG_PRIO_CRIT, "CAP_STREAMING_USR: feature not implemented\n");
965                assert (false);
966                return false;
967        default:
968                // Never should happen
969                assert (false);
970                return false;           
971        }
972        m_isStreaming= enable;
973
974        return true;
975}
976
977// Allocate buffers before start capturing
978bool CCameraV4L2::AllocateBuffers()
979{
980        if (m_isStreaming) {
981                slog_write (SLOG_PRIO_ERR, "AllocateBuffers: trying to allocate buffers while streaming\n");
982                return false;
983        }
984        if (m_libWebcamHandle== 0) {
985                slog_write (SLOG_PRIO_ERR, "AllocateBuffers: device not open\n");
986                return false;
987        }
988        if (m_buffersReady) return true; // Already allocated
989       
990        if (m_captureMethod== CAP_READ) {
991                // TODO
992        }
993        else if (m_captureMethod== CAP_STREAMING_MMAP) {
994                // Request buffers
995                if (!RequestBuffers(V4L2_MEMORY_MMAP)) {
996                        slog_write (SLOG_PRIO_ERR, "VIDIOC_REQBUFS - Unable to allocate buffers: %s\n", strerror (errno));
997                        return false;
998                }
999               
1000                //  Buffers details
1001                for (int i= 0; i< STREAMING_CAPTURE_NBUFFERS; ++i) {
1002                        memset(&m_captureBuffersInfo[i], 0, sizeof(struct v4l2_buffer));
1003                        m_captureBuffersInfo[i].index= i;
1004                        m_captureBuffersInfo[i].type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
1005                        m_captureBuffersInfo[i].memory= V4L2_MEMORY_MMAP;                               
1006                        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_QUERYBUF, &m_captureBuffersInfo[i])!= 0) {
1007                                slog_write (SLOG_PRIO_ERR, "VIDIOC_QUERYBUF - Unable to query buffer: %s\n", strerror (errno));
1008                                UnRequestBuffers(V4L2_MEMORY_MMAP);
1009                                return false;
1010                        }
1011                        if (m_captureBuffersInfo[i].length <= 0) 
1012                                slog_write (SLOG_PRIO_WARNING, "WARNING VIDIOC_QUERYBUF - buffer length is %d\n", m_captureBuffersInfo[i].length);
1013                }
1014               
1015                // MMap buffers
1016                memset(&m_captureBuffersPtr, 0, sizeof(void*) * STREAMING_CAPTURE_NBUFFERS);
1017                for (int i= 0; i< STREAMING_CAPTURE_NBUFFERS; ++i) {
1018                        m_captureBuffersPtr[i]= mmap(NULL, m_captureBuffersInfo[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, c_get_file_descriptor (m_libWebcamHandle), m_captureBuffersInfo[i].m.offset);
1019                        if (m_captureBuffersPtr[i]== MAP_FAILED) {
1020                                m_captureBuffersPtr[i]= NULL;
1021                                slog_write (SLOG_PRIO_ERR, "Unable to map buffer: %s\n", strerror (errno));
1022                                UnmapBuffers();
1023                                UnRequestBuffers(V4L2_MEMORY_MMAP);
1024                                return false;
1025                        }
1026                }
1027               
1028                // Queue buffers
1029                for (int i= 0; i< STREAMING_CAPTURE_NBUFFERS; ++i) {
1030                        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_QBUF, &m_captureBuffersInfo[i])!= 0) {
1031                                slog_write (SLOG_PRIO_ERR, "VIDIOC_QBUF - Unable to queue buffer: %s\n", strerror (errno));
1032                                UnmapBuffers();
1033                                UnRequestBuffers(V4L2_MEMORY_MMAP);
1034                                return false;
1035                        }
1036                }               
1037        }
1038        else if (m_captureMethod== CAP_STREAMING_USR) {
1039                slog_write (SLOG_PRIO_CRIT, "AllocateBuffers: CAP_STREAMING_USR not implemented\n");
1040                assert (false);
1041                return false;
1042        }
1043        else {
1044                slog_write (SLOG_PRIO_CRIT, "AllocateBuffers: capture method not set\n");
1045                return false;
1046        }
1047        m_buffersReady= true;
1048        return true;
1049}
1050
1051// Deallocate buffers
1052bool CCameraV4L2::DeallocateBuffers()
1053{
1054        if (m_isStreaming) {
1055                slog_write (SLOG_PRIO_ERR, "trying to deallocate buffers while streaming\n");
1056                return false;
1057        }
1058        if (!m_buffersReady) return true; // Already deallocated
1059
1060        if (m_captureMethod== CAP_READ) {
1061                // TODO
1062        }
1063        else if (m_captureMethod== CAP_STREAMING_MMAP) {
1064                UnmapBuffers();
1065                UnRequestBuffers(V4L2_MEMORY_MMAP);
1066        }
1067        else if (m_captureMethod== CAP_STREAMING_USR) {
1068                slog_write (SLOG_PRIO_CRIT, "DeallocateBuffers: CAP_STREAMING_USR not implemented\n");
1069                assert (false);
1070                return false;
1071        }
1072        else {
1073                slog_write (SLOG_PRIO_CRIT, "DeallocateBuffers: capture method not set\n");
1074                return false;
1075        }
1076        m_buffersReady= false;
1077        return true;
1078}
1079
1080
1081
1082// From opencv (otherlibs/highgui/cvcap_v4l.cpp)
1083/*
1084 * Turn a YUV4:2:0 block into an RGB block
1085 *
1086 * Video4Linux seems to use the blue, green, red channel
1087 * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
1088 *
1089 * Color space conversion coefficients taken from the excellent
1090 * http://www.inforamp.net/~poynton/ColorFAQ.html
1091 * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
1092 * Y values are given for all 4 pixels, but the U (Pb)
1093 * and V (Pr) are assumed constant over the 2x2 block.
1094 *
1095 * To avoid floating point arithmetic, the color conversion
1096 * coefficients are scaled into 16.16 fixed-point integers.
1097 * They were determined as follows:
1098 *
1099 *  double brightness = 1.0;  (0->black; 1->full scale)
1100 *  double saturation = 1.0;  (0->greyscale; 1->full color)
1101 *  double fixScale = brightness * 256 * 256;
1102 *  int rvScale = (int)(1.402 * saturation * fixScale);
1103 *  int guScale = (int)(-0.344136 * saturation * fixScale);
1104 *  int gvScale = (int)(-0.714136 * saturation * fixScale);
1105 *  int buScale = (int)(1.772 * saturation * fixScale);
1106 *  int yScale = (int)(fixScale);
1107 */
1108
1109/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
1110#define LIMIT(x) ((unsigned char)((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16))))
1111
1112static inline void move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v, int rowPixels, unsigned char * rgb)
1113{
1114        const int rvScale = 91881;
1115        const int guScale = -22553;
1116        const int gvScale = -46801;
1117        const int buScale = 116129;
1118        const int yScale  = 65536;
1119        int r, g, b;
1120        g = guScale * u + gvScale * v;
1121    //  if (force_rgb) {
1122    //      r = buScale * u;
1123    //      b = rvScale * v;
1124    //  } else {
1125        r = rvScale * v;
1126        b = buScale * u;
1127    //  }
1128
1129        yTL *= yScale; yTR *= yScale;
1130        yBL *= yScale; yBR *= yScale;
1131
1132        /* Write out top two pixels */
1133        rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
1134        rgb[2] = LIMIT(r+yTL);
1135
1136        rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
1137        rgb[5] = LIMIT(r+yTR);
1138
1139        /* Skip down to next line to write out bottom two pixels */
1140        rgb += 3 * rowPixels;
1141        rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
1142        rgb[2] = LIMIT(r+yBL);
1143
1144        rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
1145        rgb[5] = LIMIT(r+yBR);
1146}
1147
1148
1149// From opencv (otherlibs/highgui/cvcap_v4l.cpp)
1150// Consider a YUV420P image of 8x2 pixels.
1151//
1152// A plane of Y values    A B C D E F G H
1153//                        I J K L M N O P
1154//
1155// A plane of U values    1   2   3   4
1156// A plane of V values    1   2   3   4 ....
1157//
1158// The U1/V1 samples correspond to the ABIJ pixels.
1159//     U2/V2 samples correspond to the CDKL pixels.
1160//
1161/* Converts from planar YUV420P to RGB24. */
1162static void yuv420p_to_rgb24(int width, int height, unsigned char *pIn0, unsigned char *pOut0)
1163{
1164        const int numpix = width * height;
1165        const int bytes = 24 >> 3;
1166        int i, j, y00, y01, y10, y11, u, v;
1167        unsigned char *pY = pIn0;
1168        unsigned char *pU = pY + numpix;
1169        unsigned char *pV = pU + numpix / 4;
1170        unsigned char *pOut = pOut0;
1171
1172        for (j = 0; j <= height - 2; j += 2) {
1173                for (i = 0; i <= width - 2; i += 2) {
1174                        y00 = *pY;
1175                        y01 = *(pY + 1);
1176                        y10 = *(pY + width);
1177                        y11 = *(pY + width + 1);
1178                        u = (*pU++) - 128;
1179                        v = (*pV++) - 128;
1180
1181                        move_420_block(y00, y01, y10, y11, u, v, width, pOut);
1182
1183                        pY += 2;
1184                        pOut += 2 * bytes;
1185
1186                }
1187                pY += width;
1188                pOut += width * bytes;
1189        }
1190}
1191
1192bool CCameraV4L2::DecodeToRGB (void* src, void* dst, int width, int height, uint32_t pixelformat)
1193{
1194        // TODO: check another codecs to allow direct conversions in all cases
1195       
1196        // Direct conversions
1197        switch (pixelformat) 
1198        {
1199                case V4L2_PIX_FMT_YUYV:
1200                        yuyv2rgb ((BYTE*) src, (BYTE*) dst, width, height);
1201                        return true;
1202                case V4L2_PIX_FMT_YUV420:
1203                        yuv420p_to_rgb24(width, height, (BYTE*)src, (BYTE*)dst);
1204                        return true;
1205                case V4L2_PIX_FMT_SGBRG8:
1206                        bayer_to_rgb24 ((BYTE*) src, (BYTE*) dst, width, height, 0);
1207                        return true;
1208                case V4L2_PIX_FMT_SGRBG8: //1
1209                        bayer_to_rgb24 ((BYTE*) src, (BYTE*) dst, width, height, 1);
1210                        return true;
1211                case V4L2_PIX_FMT_SBGGR8: //2
1212                        bayer_to_rgb24 ((BYTE*) src, (BYTE*) dst, width, height, 2);
1213                        return true;
1214                case V4L2_PIX_FMT_SRGGB8: //3
1215                        bayer_to_rgb24 ((BYTE*) src, (BYTE*) dst, width, height, 3);
1216                        return true;
1217                case V4L2_PIX_FMT_RGB24:
1218                        memcpy(dst, src, width*height*3);
1219                        return true;
1220        }
1221       
1222        // Indirect conversions through YUYV
1223        BYTE tmp_buffer[width*height*2];
1224        switch (pixelformat) 
1225        {
1226#if 0
1227                // TODO jpeg format
1228                case V4L2_PIX_FMT_JPEG:
1229                case V4L2_PIX_FMT_MJPEG:
1230                        /*
1231                        if(vd->buf.bytesused <= HEADERFRAME1)
1232                {
1233                                // Prevent crash on empty image
1234                        g_printf("Ignoring empty buffer ...\n");
1235                        return (ret);
1236        }
1237                        memcpy(src, vd->mem[vd->buf.index],vd->buf.bytesused);
1238                        */
1239
1240                        if (jpeg_decode(&dst, src, width, height) < 0) {
1241                                slog_write (SLOG_PRIO_ERROR, "jpeg decode errors\n");
1242                                return false;
1243                        }
1244                        break;
1245#endif
1246                case V4L2_PIX_FMT_UYVY:
1247                        uyvy_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1248                        break;
1249                case V4L2_PIX_FMT_YVYU:                 
1250                        yvyu_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1251                        break;
1252                case V4L2_PIX_FMT_YYUV:
1253                        yyuv_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1254                        break;
1255                case V4L2_PIX_FMT_YVU420:
1256                        yvu420_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1257                        break;
1258                case V4L2_PIX_FMT_NV12:
1259                        nv12_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1260                        break;
1261                case V4L2_PIX_FMT_NV21:
1262                        nv21_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1263                        break;
1264                case V4L2_PIX_FMT_NV16:
1265                        nv16_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1266                        break;
1267                case V4L2_PIX_FMT_NV61:
1268                        nv61_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1269                        break;
1270                case V4L2_PIX_FMT_Y41P: 
1271                        y41p_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1272                        break;
1273                case V4L2_PIX_FMT_GREY:
1274                        grey_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1275                        break;
1276                case V4L2_PIX_FMT_SPCA501:
1277                        s501_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1278                        break;
1279                case V4L2_PIX_FMT_SPCA505:
1280                        s505_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1281                        break;
1282                case V4L2_PIX_FMT_SPCA508:
1283                        s508_to_yuyv(tmp_buffer, (BYTE*) src, width, height);
1284                        break;
1285                case V4L2_PIX_FMT_BGR24:
1286                        bgr2yuyv((BYTE*) src, tmp_buffer, width, height);
1287                        break;
1288                default:
1289                        slog_write (SLOG_PRIO_ERR, "error grabbing (crvcamera_v4l2.cpp) unknown format: %i\n", pixelformat);
1290                        return false;
1291        }
1292        yuyv2rgb ((BYTE*) tmp_buffer, (BYTE*) dst, width, height);
1293       
1294        return true;
1295}
1296
1297IplImage *CCameraV4L2::DoQueryFrame()
1298{
1299        if (!DoQueryFrame(m_resultImage)) return NULL;
1300       
1301        return m_resultImage.ptr();
1302}
1303
1304bool CCameraV4L2::DoQueryFrame(CIplImage& image)
1305{
1306        if (!m_isStreaming) return false;
1307        fd_set rdset;
1308        struct timeval timeout;
1309               
1310        FD_ZERO(&rdset);
1311        FD_SET(c_get_file_descriptor (m_libWebcamHandle), &rdset);
1312        timeout.tv_sec = 1; // 1 sec timeout
1313        timeout.tv_usec = 0;
1314       
1315        // select - wait for data or timeout
1316        int retval = select(c_get_file_descriptor (m_libWebcamHandle) + 1, &rdset, NULL, NULL, &timeout);
1317        if (retval < 0) {
1318                slog_write (SLOG_PRIO_ERR, "Could not grab image (select error): %s\n", strerror (errno));
1319                return false;
1320        } else if (retval == 0) {
1321                slog_write (SLOG_PRIO_ERR, "Could not grab image (select timeout): %s\n", strerror (errno));
1322                return false;
1323        }
1324        else if ((retval > 0) && (FD_ISSET(c_get_file_descriptor (m_libWebcamHandle), &rdset))) {
1325                switch (m_captureMethod) {
1326                case CAP_READ:
1327                        slog_write (SLOG_PRIO_CRIT, "CAP_READ Capture method not implemented yet\n");
1328                        assert (false);
1329                        return false;   
1330                case CAP_STREAMING_MMAP: {
1331                        struct v4l2_buffer buffer;
1332                       
1333                        // Dequeue buffer
1334                        memset(&buffer, 0, sizeof(struct v4l2_buffer));                 
1335                        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1336                        buffer.memory = V4L2_MEMORY_MMAP;
1337                        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_DQBUF, &buffer)!= 0) {
1338                                slog_write (SLOG_PRIO_ERR, "VIDIOC_DQBUF - Unable to dequeue buffer: %s\n", strerror (errno));
1339                                return false;
1340                        }
1341                       
1342                        // Allocate result image when necessary
1343                        bool allocFailed= false;
1344                        if (!image.Initialized() || 
1345                                m_currentFormat.width!= static_cast<unsigned int>(image.Width()) || 
1346                                m_currentFormat.height!= static_cast<unsigned int>(image.Height())) {
1347                               
1348                                // TODO: correct the V4L2_PIX_FMT_YUV420 conversion routine
1349                                const char* planeOrder;
1350                                if (m_currentFormat.pixelformat== V4L2_PIX_FMT_YUV420) planeOrder= "BGR";
1351                                else planeOrder= "RGB";
1352                                // TODO: make sure that image is not initialized with padded rows
1353                                if (!image.Create (m_currentFormat.width, m_currentFormat.height, IPL_DEPTH_8U, planeOrder, IPL_ORIGIN_TL, IPL_ALIGN_DWORD )) {
1354                                        slog_write (SLOG_PRIO_ERR, "Cannot create result image\n");
1355                                        allocFailed= true;                                     
1356                                }
1357                        }
1358                        // Convert to destination format (always RGB 24bit)
1359                        // TODO: check return value
1360                        if (!allocFailed) 
1361                                DecodeToRGB (m_captureBuffersPtr[buffer.index], (BYTE*) image.ptr()->imageData, 
1362                                     image.Width(), image.Height(), m_currentFormat.pixelformat);
1363                       
1364                        // Queue buffer again
1365                        if (xioctl(c_get_file_descriptor (m_libWebcamHandle), VIDIOC_QBUF, &buffer)!= 0) {
1366                                slog_write (SLOG_PRIO_ERR, "VIDIOC_QBUF - Unable to queue buffer: %s\n", strerror (errno));
1367                                return false;
1368                        }
1369                        return (!allocFailed);
1370                }
1371                case CAP_STREAMING_USR:
1372                        slog_write (SLOG_PRIO_CRIT, "CAP_STREAMING_USR Capture method not implemented yet\n");
1373                        assert (false);
1374                        return false;                   
1375                default:
1376                        assert (false);
1377                        return false;
1378                }               
1379        }
1380               
1381        return true;
1382
1383#if 0
1384        if (-1 == read (c_get_file_descriptor (m_libWebcamHandle), m_buffer.start, m_buffer.length)) {
1385        switch (errno) {
1386        case EAGAIN:
1387        return 0;
1388#endif
1389}
1390
1391void CCameraV4L2::Dump()
1392{
1393        if (slog_get_priority ()< SLOG_PRIO_DEBUG) return;
1394        slog_write (SLOG_PRIO_DEBUG, "CCameraV4L2::Dump(). Begin\n");
1395       
1396        slog_write (SLOG_PRIO_DEBUG, "m_libWebcamHandle: %u\n", m_libWebcamHandle);
1397        slog_write (SLOG_PRIO_DEBUG, "m_captureMethod: %d\n", m_captureMethod);
1398        slog_write (SLOG_PRIO_DEBUG, "m_isStreaming: %s\n", (m_isStreaming? "true" : "false"));
1399        slog_write (SLOG_PRIO_DEBUG, "m_buffersReady: %s\n", (m_buffersReady? "true" : "false"));
1400       
1401        slog_write (SLOG_PRIO_DEBUG, "m_currentFormat.frame_rate: %u\n", m_currentFormat.frame_rate);
1402        slog_write (SLOG_PRIO_DEBUG, "m_currentFormat.width .height: %u - %u\n", 
1403                m_currentFormat.width, m_currentFormat.height);
1404        slog_write (SLOG_PRIO_DEBUG, "m_currentFormat.pixelformat: %x\n", m_currentFormat.pixelformat);
1405/*     
1406        for (unsigned int i= 0; i< m_supportedPixelFormats.size(); ++i)
1407                std::cout << "m_supportedPixelFormats[" << i << "]:" << m_supportedPixelFormats[i] << std::endl;
1408        for (unsigned int i= 0; i< m_cameraControls.size(); ++i) {
1409                std::cout << "m_cameraControls[" << i << "]:\n" << std::endl;
1410                 m_cameraControls[i].Dump();
1411        }
1412        */
1413        slog_write (SLOG_PRIO_DEBUG, "CCameraV4L2::Dump(). End\n");
1414}
1415
1416///////////////////////////////////////////////////////////////////////////////
1417//
1418// Implementacion of CCameraControlV4L2 class
1419//
1420
1421CCameraControlV4L2::CCameraControlV4L2 (CHandle handle, const CControl& control) :
1422        m_name(control.name)
1423{
1424        m_handle= handle;       
1425        m_id= control.id;
1426        m_type= control.type;
1427        m_default= control.value.value;
1428               
1429        if (CControlType2ECameraControlType (control.type)!= CCTYPE_CHOICE) {
1430                m_min= control.min.value;
1431                m_max= control.max.value;
1432                // TODO: control.step is really necessary?
1433        }
1434        else {
1435                m_min= 0;
1436                m_max= control.choices.count-1;
1437                for (int i= m_min; i<= m_max; ++i) m_choices.push_back(control.choices.list[i].name);           
1438        }       
1439}
1440
1441// Get/set the current value. For boolean controls 0 and 1 are the only
1442// possible values. For choice controls 0 represents the first option.
1443// Set method returns true on success, false otherwise
1444int CCameraControlV4L2::GetValue() const
1445{
1446        CControlValue value;
1447        value.type= m_type;
1448        value.value= 0;
1449       
1450        if (c_get_control (m_handle, m_id, &value)!= C_SUCCESS) {
1451                slog_write (SLOG_PRIO_WARNING, "CCameraControlV4L2::GetValue() failed to query value\n");
1452                return 0;
1453        }
1454       
1455        return value.value;
1456}
1457
1458bool CCameraControlV4L2::SetValue(int value)
1459{
1460        CControlValue cvalue;
1461        cvalue.type= m_type;
1462        cvalue.value= value;
1463       
1464        if (c_set_control (m_handle, m_id, &cvalue)!= C_SUCCESS) {
1465                slog_write (SLOG_PRIO_WARNING, "CCameraControlV4L2::SetValue() failed to set value\n");
1466                return false;
1467        }
1468        return true;
1469}
1470
1471const char* CCameraControlV4L2::GetChoiceName(unsigned int numOption) const
1472{
1473        if (numOption> (unsigned int) m_max) return NULL;
1474       
1475        return  m_choices[numOption].c_str();
1476}
1477
1478bool CCameraControlV4L2::CheckSupportedLibwebcamId (CControlId id)
1479{
1480        return (LibwebcamId2ECameraControlId(id)!= CAM_ERROR_ENTRY);   
1481}
1482
1483CCameraControl::ECameraControlId CCameraControlV4L2::LibwebcamId2ECameraControlId (CControlId id)
1484{
1485        switch(id) {
1486                case CC_BRIGHTNESS: return CAM_BRIGHTNESS;
1487                case CC_CONTRAST: return CAM_CONTRAST;
1488                case CC_GAIN: return CAM_GAIN;
1489                case CC_SATURATION: return CAM_SATURATION;
1490                case CC_HUE: return CAM_HUE;
1491                case CC_GAMMA: return CAM_GAMMA;
1492                case CC_SHARPNESS: return CAM_SHARPNESS;
1493                case CC_WHITE_BALANCE_TEMPERATURE: return CAM_WHITE_BALANCE_TEMPERATURE;
1494                case CC_AUTO_WHITE_BALANCE_TEMPERATURE: return CAM_AUTO_WHITE_BALANCE_TEMPERATURE;
1495                case CC_WHITE_BALANCE_COMPONENT: return CAM_WHITE_BALANCE_COMPONENT;
1496                case CC_AUTO_WHITE_BALANCE_COMPONENT: return CAM_AUTO_WHITE_BALANCE_COMPONENT;
1497                case CC_BACKLIGHT_COMPENSATION: return CAM_BACKLIGHT_COMPENSATION;
1498                case CC_POWER_LINE_FREQUENCY: return CAM_POWER_LINE_FREQUENCY;
1499                case CC_AUTO_HUE: return CAM_AUTO_HUE;
1500                case CC_AUTO_EXPOSURE_MODE: return CAM_AUTO_EXPOSURE_MODE;
1501                case CC_AUTO_EXPOSURE_PRIORITY: return CAM_AUTO_EXPOSURE_PRIORITY;
1502                case CC_EXPOSURE_TIME_ABSOLUTE: return CAM_EXPOSURE_TIME_ABSOLUTE;
1503                case CC_EXPOSURE_TIME_RELATIVE: return CAM_EXPOSURE_TIME_RELATIVE;
1504                case CC_AUTO_FOCUS: return CAM_AUTO_FOCUS;
1505                case CC_FOCUS_ABSOLUTE: return CAM_FOCUS_ABSOLUTE;
1506                case CC_FOCUS_RELATIVE: return CAM_FOCUS_RELATIVE;
1507                case CC_IRIS_ABSOLUTE: return CAM_IRIS_ABSOLUTE;
1508                case CC_IRIS_RELATIVE: return CAM_IRIS_RELATIVE;
1509                case CC_ZOOM_ABSOLUTE: return CAM_ZOOM_ABSOLUTE;
1510                case CC_ZOOM_RELATIVE: return CAM_ZOOM_RELATIVE;
1511                case CC_DIGITAL_ZOOM: return CAM_DIGITAL_ZOOM;
1512                case CC_PAN_ABSOLUTE: return CAM_PAN_ABSOLUTE;
1513                case CC_PAN_RELATIVE: return CAM_PAN_RELATIVE;
1514                case CC_TILT_ABSOLUTE: return CAM_TILT_ABSOLUTE;
1515                case CC_TILT_RELATIVE: return CAM_TILT_RELATIVE;
1516                case CC_ROLL_ABSOLUTE: return CAM_ROLL_ABSOLUTE;
1517                case CC_ROLL_RELATIVE: return CAM_ROLL_RELATIVE;
1518                case CC_PRIVACY: return CAM_PRIVACY;
1519                case CC_PAN_RESET: return CAM_PAN_RESET;
1520                case CC_TILT_RESET: return CAM_TILT_RESET;
1521                case CC_LOGITECH_PANTILT_RELATIVE: return CAM_LOGITECH_PANTILT_RELATIVE;
1522                case CC_LOGITECH_PANTILT_RESET: return CAM_LOGITECH_PANTILT_RESET;
1523                case CC_LOGITECH_LED1_MODE: return CAM_LOGITECH_LED1_MODE;
1524                case CC_LOGITECH_LED1_FREQUENCY: return CAM_LOGITECH_LED1_FREQUENCY;
1525                case CC_LOGITECH_DISABLE_PROCESSING: return CAM_LOGITECH_DISABLE_PROCESSING;
1526                case CC_LOGITECH_RAW_BITS_PER_PIXEL: return CAM_LOGITECH_RAW_BITS_PER_PIXEL;
1527                default: return CAM_ERROR_ENTRY;
1528        }
1529}
1530
1531CCameraControl::ECameraControlType CCameraControlV4L2::CControlType2ECameraControlType (CControlType type)
1532{
1533        ECameraControlType ownType= CCTYPE_NUMBER;
1534        switch (type) {
1535                case CC_TYPE_BOOLEAN:
1536                        ownType= CCTYPE_BOOLEAN;
1537                        break;
1538                case CC_TYPE_BYTE:
1539                case CC_TYPE_WORD:
1540                case CC_TYPE_DWORD:
1541                        ownType= CCTYPE_NUMBER;
1542                        break;
1543                case CC_TYPE_CHOICE:
1544                        ownType= CCTYPE_CHOICE;
1545                        break;
1546                case CC_TYPE_BUTTON:
1547                        ownType= CCTYPE_BUTTON;
1548                        break;
1549                default:
1550                        // Unsupported control. Execution should never reach this point
1551                        assert (false);
1552        }       
1553        return ownType;
1554}
1555
1556//
1557// Debugging code
1558//
1559
1560static
1561void print_device_info (CHandle handle, char *device_name)
1562{
1563        assert(handle || device_name);
1564        unsigned int size = static_cast<unsigned int>(sizeof(CDevice)) + 
1565                (device_name? static_cast<unsigned int>(strlen(device_name)): 32) + 84;
1566        CDevice *info = (CDevice *)malloc(size);
1567        assert(info);
1568
1569        if(handle)
1570                slog_write (
1571                        SLOG_PRIO_DEBUG, 
1572                        "  Getting device information for handle %d ...\n", 
1573                        handle);
1574        else if(device_name)
1575                slog_write (
1576                        SLOG_PRIO_DEBUG, 
1577                        "  Getting device information for device name '%s' ...\n", 
1578                        device_name);
1579       
1580        int ret = c_get_device_info(handle, device_name, info, &size);
1581        if(ret) {
1582                slog_write (SLOG_PRIO_DEBUG, "  Failed to c_get_device_info (%d).\n", ret);
1583        }
1584        else {
1585                slog_write (
1586                        SLOG_PRIO_DEBUG, 
1587                        "    { shortName = '%s', name = '%s', driver = '%s', location = '%s', vid = %04x, pid = %04x, bcd = %d }\n",
1588                        info->shortName, info->name, info->driver, info->location, 
1589                        info->usb.vendor, info->usb.product, info->usb.release);
1590        }
1591
1592        free(info);
1593}
1594
1595static
1596void print_control_info(CControl *control)
1597{
1598        slog_write (SLOG_PRIO_DEBUG, "      { id = %d, name = '%s', type = %d, flags = %d",
1599                        control->id, control->name, control->type, control->flags);
1600        if(control->type == CC_TYPE_CHOICE) {
1601                slog_write (SLOG_PRIO_DEBUG, ", choice = {");
1602                for(unsigned int index = 0; index < control->choices.count; index++) {
1603                        slog_write (SLOG_PRIO_DEBUG, " '%s'[%d]", control->choices.list[index].name, control->choices.list[index].index);
1604                }
1605                slog_write (SLOG_PRIO_DEBUG, " }");
1606        }
1607        else {
1608                slog_write (SLOG_PRIO_DEBUG, ", min = %d, max = %d, def = %d, step = %d",
1609                                control->min.value, control->max.value, control->def.value, control->step.value);
1610        }
1611        slog_write (SLOG_PRIO_DEBUG, " }\n");
1612}
1613
1614static
1615void print_device_controls(CHandle handle)
1616{
1617        unsigned int size = 0, count = 0;
1618
1619        slog_write (SLOG_PRIO_DEBUG, "  Getting control information for handle %d ...\n", handle);
1620        CResult ret = c_enum_controls(handle, NULL, &size, &count);
1621        if(ret == C_BUFFER_TOO_SMALL) {
1622                CControl *controls = (CControl *)malloc(size);
1623                ret = c_enum_controls(handle, controls, &size, &count);
1624                if(ret) slog_write (SLOG_PRIO_DEBUG, "Unable to c_enum_controls (%d).\n", ret);
1625
1626                for(unsigned int i = 0; i < count; i++) {
1627                        CControl *control = &controls[i];
1628                        slog_write (SLOG_PRIO_DEBUG, "    Control found: %s\n", control->name);
1629                        print_control_info(control);
1630                }
1631
1632                free(controls);
1633        }
1634        else {
1635                slog_write (SLOG_PRIO_DEBUG, "  No controls found (ret = %d).\n", ret);
1636        }
1637}
1638
1639void CCameraControlV4L2::Dump()
1640{
1641        if (slog_get_priority ()< SLOG_PRIO_DEBUG) return;
1642       
1643        slog_write (SLOG_PRIO_DEBUG, " CCameraControlV4L2::Dump(). Begin\n");
1644        slog_write (SLOG_PRIO_DEBUG, "m_handle: %u\n", m_handle);
1645        slog_write (SLOG_PRIO_DEBUG, "m_id: %d\n", m_id);
1646        slog_write (SLOG_PRIO_DEBUG, "m_id (converted): %d\n", LibwebcamId2ECameraControlId (m_id));
1647        slog_write (SLOG_PRIO_DEBUG, "m_name: %s\n", m_name.c_str());
1648        slog_write (SLOG_PRIO_DEBUG, "m_type: %d\n", m_type);
1649        slog_write (SLOG_PRIO_DEBUG, "m_type (converted): %d\n", CControlType2ECameraControlType(m_type));
1650        slog_write (SLOG_PRIO_DEBUG, "m_default: %d\n", m_default);
1651        slog_write (SLOG_PRIO_DEBUG, "m_min: %d\n", m_min);
1652        slog_write (SLOG_PRIO_DEBUG, "m_max: %d\n", m_max);
1653        for (unsigned int i= 0; i< m_choices.size(); ++i)
1654                slog_write (SLOG_PRIO_DEBUG, "m_choices[%u]:%s\n", i, m_choices[i].c_str());
1655        slog_write (SLOG_PRIO_DEBUG, "Value: %d\n", GetValue());
1656        slog_write (SLOG_PRIO_DEBUG, " CCameraControlV4L2::Dump(). End\n");
1657}
1658
1659//
1660// End Debugging code
1661//
1662
1663
1664//
1665// Static attributes
1666//
1667int CCameraV4L2::g_numDevices= -1;
1668char CCameraV4L2::g_deviceNames[MAX_CAM_DEVICES][CAM_DEVICE_NAME_LENGHT];
1669char CCameraV4L2::g_deviceShortNames[MAX_CAM_DEVICES][CAM_DEVICE_SHORT_NAME_LENGHT];
1670char CCameraV4L2::g_deviceDriverNames[MAX_CAM_DEVICES][CAM_DEVICE_DRIVER_NAME_LENGHT];
1671int CCameraV4L2::g_numInstances= 0;
1672
1673void CCameraV4L2::InstanceCreated() throw(camera_exception)
1674{
1675        // TODO: thread safety
1676        if (g_numInstances== 0) {
1677                GetNumDevices();        // Fill structures when needed
1678                CResult retval= c_init();
1679                if(retval) throw camera_exception("cannot initialize libwebcam");               
1680        }
1681        ++g_numInstances;
1682}
1683
1684void CCameraV4L2::InstanceDestroyed() 
1685{
1686        // TODO: thread safety
1687        assert (g_numInstances> 0);
1688
1689        if (--g_numInstances== 0) {
1690                c_cleanup();
1691                g_numDevices= -1;
1692        }
1693}
1694
1695int CCameraV4L2::GetNumDevices()
1696{
1697        // Linux: uses libwebcam
1698        if (g_numDevices== -1) {
1699                CResult retval= c_init();
1700                if(retval) { 
1701                        slog_write (SLOG_PRIO_ERR, "Unable to c_init (%d).\n", retval);
1702                        return 0;
1703                }
1704               
1705                // Enumerate devices
1706                unsigned int size = 0, count = 0;
1707                retval= c_enum_devices(NULL, &size, &count);
1708                if(retval == C_BUFFER_TOO_SMALL) {
1709                        char devices_buffer[size];
1710                        CDevice *devices = (CDevice *) devices_buffer;
1711
1712                        retval = c_enum_devices(devices, &size, &count);
1713                        if(retval) { 
1714                                slog_write (SLOG_PRIO_ERR, "Unable to c_enum_devices (%d).\n", retval);
1715                                return 0;
1716                        }
1717                        // Adjust & store number of maximum devices
1718                        if (count> MAX_CAM_DEVICES) count= MAX_CAM_DEVICES;
1719                        g_numDevices= count;   
1720               
1721                        // Store information about detected devices
1722                        for(unsigned int i = 0; i< count; ++i) {
1723                                CDevice *device = &devices[i];
1724                               
1725                                // Prepend device number and append device name
1726                                unsigned int j= count - 1 - i;
1727                                snprintf (g_deviceNames[j], CAM_DEVICE_NAME_LENGHT, " (Id:%d) %s", j, device->name);           
1728                                snprintf (g_deviceShortNames[j], CAM_DEVICE_SHORT_NAME_LENGHT, "%s", device->shortName);
1729                                snprintf (g_deviceDriverNames[j], CAM_DEVICE_DRIVER_NAME_LENGHT, "%s", device->driver);
1730
1731                                if (slog_get_priority ()>= SLOG_PRIO_DEBUG) {
1732                                        CHandle handle = c_open_device(device->shortName);
1733                                        if(handle == 0) {
1734                                                slog_write (SLOG_PRIO_ERR, "  Failed to open device '%s'.\n", device->shortName);
1735                                                continue;
1736                                        }
1737                                        slog_write (
1738                                                SLOG_PRIO_DEBUG, 
1739                                                "  Opened device '%s' successfully (handle = %d)\n", 
1740                                                device->shortName, handle);
1741                                        print_device_info(handle, NULL);
1742                                        print_device_controls(handle);
1743                                        c_close_device(handle);
1744                                        handle = 0;
1745                                        slog_write (
1746                                                SLOG_PRIO_DEBUG, 
1747                                                "  Closed device '%s' (handle = %d)\n\n", 
1748                                                device->shortName, handle);
1749                                }
1750                        }
1751                }
1752                else {
1753                        // No devices found
1754                        g_numDevices= 0;                       
1755                }
1756               
1757                // Frees libwebcam
1758                c_cleanup();           
1759        }
1760
1761        return g_numDevices;
1762}
1763
1764const char* CCameraV4L2::GetDeviceName (unsigned int id)
1765{
1766        if ((int) id>= GetNumDevices()) return NULL;
1767        return g_deviceNames[id];
1768}
Note: See TracBrowser for help on using the repository browser.