Revision: 7966
http://playerstage.svn.sourceforge.net/playerstage/?rev=7966&view=rev
Author: gbiggs
Date: 2009-07-10 00:27:48 +0000 (Fri, 10 Jul 2009)
Log Message:
-----------
Applied patch #2816705
Modified Paths:
--------------
code/player/trunk/server/drivers/camera/CMakeLists.txt
Added Paths:
-----------
code/player/trunk/server/drivers/camera/v4l2/
code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt
code/player/trunk/server/drivers/camera/v4l2/bayer.c
code/player/trunk/server/drivers/camera/v4l2/bayer.h
code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc
code/player/trunk/server/drivers/camera/v4l2/v4l2.c
code/player/trunk/server/drivers/camera/v4l2/v4l2.h
Modified: code/player/trunk/server/drivers/camera/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/camera/CMakeLists.txt 2009-07-10
00:21:47 UTC (rev 7965)
+++ code/player/trunk/server/drivers/camera/CMakeLists.txt 2009-07-10
00:27:48 UTC (rev 7966)
@@ -1,4 +1,5 @@
ADD_SUBDIRECTORY (v4l)
+ADD_SUBDIRECTORY (v4l2)
ADD_SUBDIRECTORY (1394)
ADD_SUBDIRECTORY (camfilter)
ADD_SUBDIRECTORY (compress)
Added: code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt 2009-07-10
00:27:48 UTC (rev 7966)
@@ -0,0 +1,4 @@
+PLAYERDRIVER_OPTION (camerav4l2 build_camerav4l2 ON)
+PLAYERDRIVER_REJECT_OS (camerav4l2 build_camerav4l2 PLAYER_OS_WIN)
+PLAYERDRIVER_REQUIRE_HEADER (camerav4l2 build_camerav4l2 linux/videodev2.h
sys/types.h)
+PLAYERDRIVER_ADD_DRIVER (camerav4l2 build_camerav4l2 SOURCES bayer.c v4l2.c
camerav4l2.cc)
Added: code/player/trunk/server/drivers/camera/v4l2/bayer.c
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/bayer.c
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/bayer.c 2009-07-10
00:27:48 UTC (rev 7966)
@@ -0,0 +1,101 @@
+/*
+ * BAYER2RGB24 ROUTINE TAKEN FROM:
+ *
+ * Sonix SN9C101 based webcam basic I/F routines
+ * Copyright (C) 2004 Takafumi Mizuno <[email protected]>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+void bayer2rgb24(unsigned char *dst, unsigned char *src, long int WIDTH, long
int HEIGHT)
+{
+ long int i;
+ unsigned char *rawpt, *scanpt;
+ long int size;
+
+ rawpt = src;
+ scanpt = dst;
+ size = WIDTH*HEIGHT;
+
+ for ( i = 0; i < size; i++ ) {
+ if ( (i/WIDTH) % 2 == 0 ) {
+ if ( (i % 2) == 0 ) {
+ /* B */
+ if ( (i > WIDTH) && ((i % WIDTH) > 0) ) {
+ *scanpt++ = (*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+
+ *(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4; /* R */
+ *scanpt++ = (*(rawpt-1)+*(rawpt+1)+
+ *(rawpt+WIDTH)+*(rawpt-WIDTH))/4; /* G */
+ *scanpt++ = *rawpt; /* B */
+ } else {
+ /* first line or left column */
+ *scanpt++ = *(rawpt+WIDTH+1); /* R */
+ *scanpt++ = (*(rawpt+1)+*(rawpt+WIDTH))/2; /* G */
+ *scanpt++ = *rawpt; /* B */
+ }
+ } else {
+ /* (B)G */
+ if ( (i > WIDTH) && ((i % WIDTH) < (WIDTH-1)) ) {
+ *scanpt++ = (*(rawpt+WIDTH)+*(rawpt-WIDTH))/2; /* R */
+ *scanpt++ = *rawpt; /* G */
+ *scanpt++ = (*(rawpt-1)+*(rawpt+1))/2; /* B */
+ } else {
+ /* first line or right column */
+ *scanpt++ = *(rawpt+WIDTH); /* R */
+ *scanpt++ = *rawpt; /* G */
+ *scanpt++ = *(rawpt-1); /* B */
+ }
+ }
+ } else {
+ if ( (i % 2) == 0 ) {
+ /* G(R) */
+ if ( (i < (WIDTH*(HEIGHT-1))) && ((i % WIDTH) > 0) ) {
+ *scanpt++ = (*(rawpt-1)+*(rawpt+1))/2; /* R */
+ *scanpt++ = *rawpt; /* G */
+ *scanpt++ = (*(rawpt+WIDTH)+*(rawpt-WIDTH))/2; /* B */
+ } else {
+ /* bottom line or left column */
+ *scanpt++ = *(rawpt+1); /* R */
+ *scanpt++ = *rawpt; /* G */
+ *scanpt++ = *(rawpt-WIDTH); /* B */
+ }
+ } else {
+ /* R */
+ if ( i < (WIDTH*(HEIGHT-1)) && ((i % WIDTH) < (WIDTH-1)) ) {
+ *scanpt++ = *rawpt; /* R */
+ *scanpt++ = (*(rawpt-1)+*(rawpt+1)+
+ *(rawpt-WIDTH)+*(rawpt+WIDTH))/4; /* G */
+ *scanpt++ = (*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+
+ *(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4; /* B */
+ } else {
+ /* bottom line or right column */
+ *scanpt++ = *rawpt; /* R */
+ *scanpt++ = (*(rawpt-1)+*(rawpt-WIDTH))/2; /* G */
+ *scanpt++ = *(rawpt-WIDTH-1); /* B */
+ }
+ }
+ }
+ rawpt++;
+ }
+
+}
+
Added: code/player/trunk/server/drivers/camera/v4l2/bayer.h
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/bayer.h
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/bayer.h 2009-07-10
00:27:48 UTC (rev 7966)
@@ -0,0 +1,6 @@
+#ifndef __BAYER_H
+#define __BAYER_H
+
+extern void bayer2rgb24(unsigned char *dst, unsigned char *src, long int
WIDTH, long int HEIGHT);
+
+#endif
Added: code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc 2009-07-10
00:27:48 UTC (rev 7966)
@@ -0,0 +1,602 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2000 Brian Gerkey et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/*
+ *
+ * This driver captures images from V4L2-compatible cameras.
+ */
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_camerav4l2 camerav4l2
+ * @brief Video4Linux2 camera capture
+
+The camerav4l2 driver captures images from V4L2-compatible cameras.
+
+...@par Compile-time dependencies
+
+- <sys/types.h>
+- <linux/videodev2.h>
+
+...@par Provides
+
+- @ref interface_camera
+
+...@par Requires
+
+- none
+
+...@par Configuration requests
+
+- none
+
+...@par Configuration file options
+
+- port (string)
+ - Default: "/dev/video0"
+ - Device to read video data from.
+
+- sources (integer tuple)
+ - Default: NONE
+ - Some capture cards have few multiple input sources; use this field to
+ select which ones are used. You should define as many provided camera
+ interfaces as the number of sources is given here. Source channel
+ numbers are used as keys ('ch' prefixed) in 'provides' tuple.
+ - If not given, channel 0 alone will be used by defauilt.
+ - Note that switching between channels takes time. Framerate drops
+ dramatically whenever more than one channel is used.
+
+- norm (string)
+ - Default: "NTSC"
+ - Capture format; "NTSC" or "PAL"
+ - Case sensitive!
+
+- size (integer tuple)
+ - Default: varies with norm
+ - PAL: [768 576]
+ - NTSC: [640 480]
+ - other: [320 240]
+ - Desired image size. This may not be honoured if the driver does
+ not support the requested size).
+
+- mode (string)
+ - Default: "BGR3"
+ - Desired capture mode. Can be one of:
+ - GREY (8-bit monochrome)
+ - BGR3, RGB3 (24-bit RGB)
+ - BGR4, RGB4 (32-bit RGB; will produce 24-bit color images)
+ - BA81 (for sn9c1xx-based USB webcams)
+ - Note that not all capture modes are supported by Player's internal image
+ format; in these modes, images will be translated to the closest matching
+ internal format (e.g., BGR4 -> RGB888).
+
+- buffers (integer)
+ - Default: 2
+ - Number of buffers to use for grabbing. This reduces latency, but also
+ potentially reduces throughput. Use this if you are reading slowly
+ from the player driver and do not want to get stale frames.
+
+- sleep_nsec (integer)
+ - Default: 10000000 (=10ms which gives max 100 fps)
+ - timespec value for nanosleep()
+
+- settle_time (double)
+ - Default: 0.5
+ - Time (in seconds) to wait after switching to next channel; images grabbed
+ right after channel switching are too bright or messy, so it is better
+ to wait a little while before start grabbing.
+
+- skip_frames (integer)
+ - Default: 10
+ - See 'settle_time' - during this settle time frames are grabbed anyway
+ and they are counted. We can decide to stop waiting for stable image
+ after given number of frames was skipped.
+
+- failsafe (integer)
+ - Default: 0
+ - If after few days of grabbing you experience unexpected failures you can
+ try to set this to 1. WARNING! Such failures may (or may not) suggest
+ something wrong is going on with your system kernel. Reboot of whole
+ system is a better solution, although it may not always be desired.
+ This feature is turned off by default - use at your own risk.
+
+Note that some of these options may not be honoured by the underlying
+V4L2 kernel driver (it may not support a given image size, for
+example).
+
+...@par Example
+
+...@verbatim
+driver
+(
+ name "camerav4l2"
+ provides ["camera:0"]
+)
+...@endverbatim
+
+...@par Channel 2 explicitly selected:
+
+...@verbatim
+driver
+(
+ name "camerav4l2"
+ sources [2]
+ norm "PAL"
+ size [384 288]
+ mode "BGR4"
+ buffers 4
+ sleep_nsec 10000
+ provides ["ch2:::camera:1"]
+)
+driver
+(
+ name "cameracompress"
+ requires "camera:1"
+ provides "camera:0"
+)
+...@endverbatim
+
+...@par Two channels at the same time:
+
+...@verbatim
+driver
+(
+ name "camerav4l2"
+ sources [0 2]
+ norm "PAL"
+ provides ["ch2:::camera:0" "ch0:::camera:1"]
+)
+...@endverbatim
+
+...@par sn9c1xx-based USB webcam (it accepts one buffer and only 352x288
size!):
+
+...@verbatim
+driver
+(
+ name "camerav4l2"
+ norm "UNKNOWN"
+ mode "BA81"
+ size [352 288]
+ buffers 1
+ provides ["camera:1"]
+)
+driver
+(
+ name "cameracompress"
+ requires ["camera:1"]
+ provides ["camera:0"]
+)
+...@endverbatim
+
+...@author Paul Osmialowski, Takafumi Mizuno
+
+*/
+/** @} */
+
+#include "v4l2.h"
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <pthread.h>
+#include <assert.h>
+#include <libplayercore/playercore.h>
+
+#define MAX_CHANNELS 10
+
+extern PlayerTime * GlobalTime;
+
+class CameraV4L2: public ThreadedDriver
+{
+ public:
+ // Constructor; need that
+ CameraV4L2(ConfigFile * cf, int section);
+
+ virtual ~CameraV4L2();
+
+ // Must implement the following methods.
+ virtual int MainSetup();
+ virtual void MainQuit();
+
+ // This method will be invoked on each incoming message
+ virtual int ProcessMessage(QueuePointer & resp_queue,
+ player_msghdr * hdr,
+ void * data);
+ private:
+ // Main function for device thread.
+ virtual void Main();
+ int setSource();
+
+ int started;
+ const char * port;
+ const char * mode;
+ int buffers;
+ int sources_count;
+ int sources[MAX_CHANNELS];
+ int current_source;
+ player_devaddr_t camera_addrs[MAX_CHANNELS];
+ void * fg;
+ const char * norm;
+ int width;
+ int height;
+ int bpp;
+ int sleep_nsec;
+ double settle_time;
+ int skip_frames;
+ uint32_t format;
+ int failsafe;
+};
+
+CameraV4L2::CameraV4L2(ConfigFile * cf, int section)
+ : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
+{
+ int i;
+ char key[24];
+
+ this->started = 0;
+ this->port = NULL;
+ this->mode = NULL;
+ this->buffers = 0;
+ this->sources_count = 0;
+ this->current_source = 0;
+ this->fg = NULL;
+ this->norm = NULL;
+ this->width = 0;
+ this->height = 0;
+ this->bpp = 0;
+ this->sleep_nsec = 0;
+ this->settle_time = 0.0;
+ this->skip_frames = 0;
+ this->format = 0;
+ this->failsafe = 0;
+ memset(this->sources, 0, sizeof this->sources);
+ memset(this->camera_addrs, 0, sizeof this->camera_addrs);
+ this->sources_count = cf->GetTupleCount(section, "sources");
+ if (((this->sources_count) <= 0) || ((this->sources_count) > MAX_CHANNELS))
+ {
+ PLAYER_WARN("Implicitly using channel 0");
+ this->sources_count = 1;
+ this->sources[0] = 0;
+ if (cf->ReadDeviceAddr(&(this->camera_addrs[0]), section, "provides",
+ PLAYER_CAMERA_CODE, -1, NULL))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (this->AddInterface(this->camera_addrs[0]))
+ {
+ this->SetError(-1);
+ return;
+ }
+ } else
+ {
+ for (i = 0; i < (this->sources_count); i++)
+ {
+ this->sources[i] = cf->ReadTupleInt(section, "sources", i, -1);
+ if ((this->sources[i]) < 0)
+ {
+ PLAYER_ERROR2("Invalid channel number %d for source %d",
this->sources[i], i);
+ this->SetError(-1);
+ return;
+ }
+ snprintf(key, sizeof key, "ch%d", this->sources[i]);
+ if (cf->ReadDeviceAddr(&(this->camera_addrs[i]), section, "provides",
+ PLAYER_CAMERA_CODE, -1, key))
+ {
+ PLAYER_ERROR1("Cannot provide device %s", key);
+ this->SetError(-1);
+ return;
+ }
+ if (this->AddInterface(this->camera_addrs[i]))
+ {
+ this->SetError(-1);
+ return;
+ }
+ }
+ }
+ if (((this->sources_count) <= 0) || ((this->sources_count) > MAX_CHANNELS))
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->port = cf->ReadString(section, "port", "/dev/video0");
+ if (!(this->port))
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->mode = cf->ReadString(section, "mode", "BGR3");
+ if (!(this->mode))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (!(strcmp(this->mode, "GREY")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_MONO8;
+ this->bpp = 8;
+ } else if (!(strcmp(this->mode, "BGR3")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->bpp = 24;
+ } else if (!(strcmp(this->mode, "BGR4")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->bpp = 32;
+ } else if (!(strcmp(this->mode, "RGB3")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->bpp = 24;
+ } else if (!(strcmp(this->mode, "RGB4")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->bpp = 32;
+ } else if (!(strcmp(this->mode, "BA81")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->bpp = 24;
+ } else
+ {
+ PLAYER_ERROR("Unknown pixel format");
+ this->SetError(-1);
+ return;
+ }
+ if ((this->bpp) <= 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ // NTSC, PAL or UNKNOWN
+ this->norm = cf->ReadString(section, "norm", "NTSC");
+ if (!(strcmp(this->norm, "NTSC")))
+ {
+ this->width = 640;
+ this->height = 480;
+ } else if (!(strcmp(this->norm, "PAL")))
+ {
+ this->width = 768;
+ this->height = 576;
+ } else
+ {
+ this->width = 320;
+ this->height = 240;
+ }
+ if (cf->GetTupleCount(section, "size") == 2)
+ {
+ this->width = cf->ReadTupleInt(section, "size", 0, this->width);
+ this->height = cf->ReadTupleInt(section, "size", 1, this->height);
+ }
+ if (((this->width) <= 0) || ((this->height) <= 0))
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->buffers = cf->ReadInt(section, "buffers", 2);
+ if ((this->buffers) <= 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->sleep_nsec = cf->ReadInt(section, "sleep_nsec", 10000000);
+ if ((this->sleep_nsec) < 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->settle_time = cf->ReadFloat(section, "settle_time", 0.5);
+ this->skip_frames = cf->ReadInt(section, "skip_frames", 10);
+ this->failsafe = cf->ReadInt(section, "failsafe", 0);
+}
+
+CameraV4L2::~CameraV4L2()
+{
+ if (this->fg)
+ {
+ if (this->started) stop_grab(this->fg);
+ this->started = 0;
+ close_fg(this->fg);
+ }
+ this->fg = NULL;
+}
+
+int CameraV4L2::setSource()
+{
+ int dropped = 0;
+ double start_time, t;
+ struct timespec tspec;
+ static int next_source = 0;
+
+ if (!(this->fg)) return -1;
+ if (this->started) stop_grab(this->fg);
+ this->started = 0;
+ if ((next_source < 0) || (next_source >= (this->sources_count))) return -1;
+ if (set_channel(this->fg, this->sources[next_source], this->norm) < 0)
+ {
+ PLAYER_ERROR1("Cannot set channel %d", this->sources[next_source]);
+ return -1;
+ }
+ this->current_source = next_source;
+ next_source++;
+ if (next_source >= (this->sources_count)) next_source = 0;
+ if (((this->current_source) < 0) || ((this->current_source) >=
(this->sources_count))) return -1;
+ if (start_grab(this->fg) < 0)
+ {
+ PLAYER_ERROR1("Cannot start grab on channel %d",
this->sources[this->current_source]);
+ return -1;
+ }
+ this->started = !0;
+ GlobalTime->GetTimeDouble(&start_time);
+ for (;;)
+ {
+ tspec.tv_sec = 0;
+ tspec.tv_nsec = this->sleep_nsec;
+ nanosleep(&tspec, NULL);
+ GlobalTime->GetTimeDouble(&t);
+ if (((t - start_time) >= (this->settle_time)) && (dropped >=
(this->skip_frames))) break;
+ if (!(get_image(this->fg))) PLAYER_WARN("No frame grabbed");
+ dropped++;
+ }
+ return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set up the device. Return 0 if things go well, and -1 otherwise.
+int CameraV4L2::MainSetup()
+{
+ assert((!(this->fg)) && (!(this->started)));
+ this->fg = open_fg(this->port, this->mode, this->width, this->height,
(this->bpp) / 8, this->buffers);
+ if (!(this->fg)) return -1;
+ return this->setSource();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Shutdown the device
+void CameraV4L2::MainQuit()
+{
+ if (this->fg)
+ {
+ if (this->started) stop_grab(this->fg);
+ this->started = 0;
+ close_fg(this->fg);
+ }
+ this->fg = NULL;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Main function for device thread
+void CameraV4L2::Main()
+{
+ struct timespec tspec;
+ const unsigned char * img;
+ player_camera_data_t * data = NULL;
+
+ for (;;)
+ {
+ // Go to sleep for a while (this is a polling loop).
+ tspec.tv_sec = 0;
+ tspec.tv_nsec = this->sleep_nsec;
+ nanosleep(&tspec, NULL);
+
+ // Test if we are supposed to cancel this thread.
+ pthread_testcancel();
+
+ // Process any pending requests.
+ ProcessMessages();
+
+ assert(this->fg);
+ assert(this->started);
+ // Grab the next frame (blocking)
+ img = get_image(this->fg);
+ if (this->failsafe)
+ {
+ if (!img)
+ {
+ PLAYER_ERROR("Cannot grab frame");
+ pthread_testcancel();
+ if (this->started) stop_grab(this->fg);
+ this->started = 0;
+ close_fg(this->fg);
+ this->fg = NULL;
+ tspec.tv_sec = 1;
+ tspec.tv_nsec = 0;
+ nanosleep(&tspec, NULL);
+ pthread_testcancel();
+ this->fg = open_fg(this->port, this->mode, this->width, this->height,
(this->bpp) / 8, this->buffers);
+ assert(this->fg);
+ this->setSource();
+ assert(this->started);
+ pthread_testcancel();
+ continue;
+ }
+ } else
+ {
+ assert(img);
+ }
+ data = reinterpret_cast<player_camera_data_t
*>(malloc(sizeof(player_camera_data_t)));
+ if (!data)
+ {
+ PLAYER_ERROR("Out of memory");
+ continue;
+ }
+ memset(data, 0, sizeof data);
+ // Set the image properties
+ data->width = this->width;
+ data->height = this->height;
+ data->bpp = this->bpp;
+ data->format = this->format;
+ data->fdiv = 0;
+ data->image_count = 0;
+ data->image = NULL;
+ data->compression = PLAYER_CAMERA_COMPRESS_RAW;
+ data->image_count = this->width * this->height * ((this->bpp) / 8);
+ assert(data->image_count > 0);
+ data->image = reinterpret_cast<uint8_t *>(malloc(data->image_count));
+ if (!(data->image))
+ {
+ PLAYER_ERROR("Out of memory!");
+ free(data);
+ data = NULL;
+ continue;
+ }
+ memcpy(data->image, img, data->image_count);
+ Publish(this->camera_addrs[this->current_source],
+ PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
+ reinterpret_cast<void *>(data), 0, NULL, false);
+ // copy = false, don't dispose anything here!
+ data = NULL;
+
+ if ((this->sources_count) > 1)
+ {
+ pthread_testcancel();
+ if (this->failsafe)
+ {
+ this->setSource();
+ } else
+ {
+ assert(!(this->setSource()));
+ }
+ }
+
+ // Test if we are supposed to cancel this thread.
+ pthread_testcancel();
+ }
+}
+
+int CameraV4L2::ProcessMessage(QueuePointer & resp_queue,
+ player_msghdr * hdr,
+ void * data)
+{
+ assert(hdr);
+ return -1;
+}
+
+Driver * CameraV4L2_Init(ConfigFile * cf, int section)
+{
+ // Create and return a new instance of this driver
+ return reinterpret_cast<Driver *>(new CameraV4L2(cf, section));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// a driver registration function
+void camerav4l2_Register(DriverTable* table)
+{
+ table->AddDriver("camerav4l2", CameraV4L2_Init);
+}
Added: code/player/trunk/server/drivers/camera/v4l2/v4l2.c
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/v4l2.c
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/v4l2.c 2009-07-10 00:27:48 UTC
(rev 7966)
@@ -0,0 +1,474 @@
+#include "v4l2.h"
+#include "bayer.h"
+#include <unistd.h>
+#include <sys/types.h>
+#include <linux/videodev2.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+#define v4l2_fmtbyname(name) v4l2_fourcc((name)[0], (name)[1], (name)[2],
(name)[3])
+
+#define FAIL -1
+
+#define REQUEST_BUFFERS 4
+
+struct fg_struct
+{
+ int dev_fd;
+ int grabbing;
+ int grab_number;
+ int depth;
+ int buffers_num;
+ unsigned int pixformat;
+ int r, g, b;
+ struct buff_struct
+ {
+ struct v4l2_buffer buffer;
+ unsigned char * video_map;
+ } buffers[REQUEST_BUFFERS];
+ int width;
+ int height;
+ int pixels;
+ int imgdepth;
+ unsigned char * bayerbuf;
+ int bayerbuf_size;
+ unsigned char * image;
+};
+
+#define FG(ptr) ((struct fg_struct *)(ptr))
+
+int fg_width(void * fg)
+{
+ return FG(fg)->width;
+}
+
+int fg_height(void * fg)
+{
+ return FG(fg)->height;
+}
+
+int fg_grabdepth(void * fg)
+{
+ return FG(fg)->depth;
+}
+
+int fg_imgdepth(void * fg)
+{
+ return FG(fg)->imgdepth;
+}
+
+int set_channel(void * fg, int channel, const char * mode)
+{
+ int m;
+
+ if (FG(fg)->grabbing) return FAIL;
+ if (channel < 0) return FAIL;
+ if (ioctl (FG(fg)->dev_fd, VIDIOC_S_INPUT, &channel) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_S_INPUT)\n");
+ return FAIL;
+ }
+ if (!strcmp(mode, "UNKNOWN")) m = 0;
+ else if (!strcmp(mode, "PAL")) m = V4L2_STD_PAL;
+ else if (!strcmp(mode, "NTSC")) m = V4L2_STD_NTSC;
+ else
+ {
+ fprintf(stderr, "invalid mode %s\n", mode);
+ return FAIL;
+ }
+ if (m > 0)
+ {
+ if (ioctl (FG(fg)->dev_fd, VIDIOC_S_STD, &m) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_S_STD)\n");
+ return FAIL;
+ }
+ }
+ return 0;
+}
+
+int start_grab(void * fg)
+{
+ int i;
+ enum v4l2_buf_type type;
+
+ if (FG(fg)->grabbing) return 0;
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ ioctl(FG(fg)->dev_fd, VIDIOC_STREAMOFF, &type);
+ for (i = 0; i < (FG(fg)->buffers_num); i++)
+ {
+ if (ioctl(FG(fg)->dev_fd, VIDIOC_QBUF, &(FG(fg)->buffers[i].buffer)) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_QBUF)\n");
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ ioctl(FG(fg)->dev_fd, VIDIOC_STREAMOFF, &type);
+ return FAIL;
+ }
+ }
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (ioctl(FG(fg)->dev_fd, VIDIOC_STREAMON, &type) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_STREAMON)\n");
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ ioctl(FG(fg)->dev_fd, VIDIOC_STREAMOFF, &type);
+ return FAIL;
+ }
+ FG(fg)->grab_number = 0;
+ FG(fg)->grabbing = !0;
+ return 0;
+}
+
+void stop_grab(void * fg)
+{
+ enum v4l2_buf_type type;
+
+ if (FG(fg)->grabbing)
+ {
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (ioctl(FG(fg)->dev_fd, VIDIOC_STREAMOFF, &type) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_STREAMOFF)\n");
+ }
+ FG(fg)->grabbing = 0;
+ }
+}
+
+unsigned char * get_image(void * fg)
+{
+ enum v4l2_buf_type type;
+ int i, grabdepth;
+ const unsigned char * buf;
+ unsigned char * img;
+
+ if ((!(FG(fg)->grabbing)) || (!(FG(fg)->image))) return NULL;
+ memset(&(FG(fg)->buffers[FG(fg)->grab_number].buffer), 0, sizeof
FG(fg)->buffers[FG(fg)->grab_number].buffer);
+ FG(fg)->buffers[FG(fg)->grab_number].buffer.type =
V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ FG(fg)->buffers[FG(fg)->grab_number].buffer.memory = V4L2_MEMORY_MMAP;
+ if (ioctl(FG(fg)->dev_fd, VIDIOC_DQBUF,
&(FG(fg)->buffers[FG(fg)->grab_number].buffer)) == -1)
+ {
+ fprintf(stderr, "get_image: ioctl error (VIDIOC_DQBUF)\n");
+ return NULL;
+ }
+ buf = FG(fg)->buffers[FG(fg)->grab_number].video_map;
+ if (!buf)
+ {
+ fprintf(stderr, "NULL buffer pointer\n");
+ return NULL;
+ }
+ grabdepth = FG(fg)->depth;
+ if ((FG(fg)->pixformat) == v4l2_fmtbyname("BA81"))
+ {
+ if (!(FG(fg)->bayerbuf)) return NULL;
+ bayer2rgb24(FG(fg)->bayerbuf,
FG(fg)->buffers[FG(fg)->grab_number].video_map, FG(fg)->width, FG(fg)->height);
+ buf = FG(fg)->bayerbuf;
+ grabdepth = 3;
+ }
+ img = FG(fg)->image;
+ for (i = 0; i < (FG(fg)->pixels); i++)
+ {
+ switch(FG(fg)->imgdepth)
+ {
+ case 1:
+ switch(grabdepth)
+ {
+ case 1:
+ img[0] = buf[0];
+ img++; buf++;
+ break;
+ case 3:
+ img[0] = (unsigned char)((buf[FG(fg)->r] / 3) + (buf[FG(fg)->g] / 3) +
(buf[FG(fg)->b] / 3));
+ img++; buf += 3;
+ break;
+ case 4:
+ img[0] = (unsigned char)((buf[FG(fg)->r] / 3) + (buf[FG(fg)->g] / 3) +
(buf[FG(fg)->b] / 3));
+ img++; buf += 4;
+ break;
+ }
+ break;
+ case 3:
+ switch(grabdepth)
+ {
+ case 1:
+ img[0] = buf[0];
+ img[1] = buf[0];
+ img[2] = buf[0];
+ img += 3; buf++;
+ break;
+ case 3:
+ img[0] = buf[FG(fg)->r];
+ img[1] = buf[FG(fg)->g];
+ img[2] = buf[FG(fg)->b];
+ img += 3; buf += 3;
+ break;
+ case 4:
+ img[0] = buf[FG(fg)->r];
+ img[1] = buf[FG(fg)->g];
+ img[2] = buf[FG(fg)->b];
+ img += 3; buf += 4;
+ break;
+ }
+ break;
+ case 4:
+ switch(grabdepth)
+ {
+ case 1:
+ img[0] = buf[0];
+ img[1] = buf[0];
+ img[2] = buf[0];
+ img[3] = buf[0];
+ img += 4; buf++;
+ break;
+ case 3:
+ img[0] = buf[FG(fg)->r];
+ img[1] = buf[FG(fg)->g];
+ img[2] = buf[FG(fg)->b];
+ img[3] = 0;
+ img += 4; buf += 3;
+ break;
+ case 4:
+ img[0] = buf[FG(fg)->r];
+ img[1] = buf[FG(fg)->g];
+ img[2] = buf[FG(fg)->b];
+ img[3] = buf[3];
+ img += 4; buf += 4;
+ break;
+ }
+ break;
+ }
+ }
+ if (ioctl(FG(fg)->dev_fd, VIDIOC_QBUF,
&(FG(fg)->buffers[FG(fg)->grab_number].buffer)) == -1)
+ {
+ fprintf(stderr, "get_image: ioctl error (VIDIOC_QBUF)\n");
+ return NULL;
+ }
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ ioctl(FG(fg)->dev_fd, VIDIOC_STREAMON, &type);
+ FG(fg)->grab_number++;
+ if ((FG(fg)->grab_number) >= (FG(fg)->buffers_num)) FG(fg)->grab_number = 0;
+ return FG(fg)->image;
+}
+
+void * open_fg(const char * dev, const char * pixformat, int width, int
height, int imgdepth, int buffers)
+{
+ int i;
+ struct v4l2_requestbuffers reqbuf;
+ struct v4l2_format format;
+ struct fg_struct * fg;
+
+ fg = malloc(sizeof(struct fg_struct));
+ if (!fg)
+ {
+ fprintf(stderr, "out of memory\n");
+ return NULL;
+ }
+ fg->dev_fd = -1;
+ fg->grabbing = 0;
+ fg->buffers_num = 0;
+ for (i = 0; i < REQUEST_BUFFERS; i++) fg->buffers[i].video_map = NULL;
+ fg->image = NULL;
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ if ((width <= 0) || (height <= 0))
+ {
+ fprintf(stderr, "invalid image size\n");
+ free(fg);
+ return NULL;
+ }
+ fg->width = width; fg->height = height; fg->pixels = (width * height);
+ if (imgdepth <= 0)
+ {
+ fprintf(stderr, "invalid depth given\n");
+ free(fg);
+ return NULL;
+ }
+ fg->imgdepth = imgdepth;
+ if (buffers <= 0)
+ {
+ fprintf(stderr, "invalid number of requested buffers\n");
+ free(fg);
+ return NULL;
+ }
+ if (buffers > REQUEST_BUFFERS) buffers = REQUEST_BUFFERS;
+ fg->buffers_num = buffers;
+ fg->pixformat = v4l2_fmtbyname(pixformat);
+ if ((fg->pixformat) == v4l2_fmtbyname("GREY"))
+ {
+ fg->r = 0; fg->g = 0; fg->b = 0;
+ fg->depth = 1;
+ } else if ((fg->pixformat) == v4l2_fmtbyname("BGR3"))
+ {
+ fg->r = 2; fg->g = 1; fg->b = 0;
+ fg->depth = 3;
+ } else if ((fg->pixformat) == v4l2_fmtbyname("BGR4"))
+ {
+ fg->r = 2; fg->g = 1; fg->b = 0;
+ fg->depth = 4;
+ } else if ((fg->pixformat) == v4l2_fmtbyname("RGB3"))
+ {
+ fg->r = 0; fg->g = 1; fg->b = 2;
+ fg->depth = 3;
+ } else if ((fg->pixformat) == v4l2_fmtbyname("RGB4"))
+ {
+ fg->r = 0; fg->g = 1; fg->b = 2;
+ fg->depth = 4;
+ } else if ((fg->pixformat) == v4l2_fmtbyname("BA81"))
+ {
+ fg->depth = 1;
+ fg->r = 0; fg->g = 1; fg->b = 2;
+ fg->bayerbuf_size = width * height * 3;
+ fg->bayerbuf = malloc(fg->bayerbuf_size);
+ if (!(fg->bayerbuf))
+ {
+ fprintf(stderr, "out of memory\n");
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ } else
+ {
+ fprintf(stderr, "unknown pixel format %s\n", pixformat);
+ free(fg);
+ return NULL;
+ }
+ fg->image = malloc(width * height * imgdepth);
+ if (!(fg->image))
+ {
+ fprintf(stderr, "out of memory\n");
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ fg->dev_fd = open(dev, O_RDWR);
+ if (fg->dev_fd == -1)
+ {
+ fprintf(stderr, "cannot open %s\n", dev);
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ memset(&format, 0, sizeof format);
+ format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ format.fmt.pix.width = width;
+ format.fmt.pix.height = height;
+ format.fmt.pix.pixelformat = v4l2_fmtbyname(pixformat);
+ format.fmt.pix.field = V4L2_FIELD_ANY;
+ format.fmt.pix.bytesperline = 0;
+ /* rest of the v4l2_pix_format structure fields are set by driver */
+ if (ioctl(fg->dev_fd, VIDIOC_S_FMT, &format) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_S_FMT)\n");
+ close(fg->dev_fd); fg->dev_fd = -1;
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ memset(&reqbuf, 0, sizeof reqbuf);
+ reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ reqbuf.memory = V4L2_MEMORY_MMAP;
+ reqbuf.count = (fg->buffers_num);
+ if (ioctl(fg->dev_fd, VIDIOC_REQBUFS, &reqbuf) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_REQBUFS)\n");
+ close(fg->dev_fd); fg->dev_fd = -1;
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ if ((reqbuf.count) != (fg->buffers_num))
+ {
+ fprintf(stderr, "reqbuf.count != fg->buffers_num\n");
+ close(fg->dev_fd); fg->dev_fd = -1;
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ for (i = 0; i < (fg->buffers_num); i++)
+ {
+ fg->buffers[i].buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fg->buffers[i].buffer.memory = V4L2_MEMORY_MMAP;
+ fg->buffers[i].buffer.index = i;
+ if (ioctl(fg->dev_fd, VIDIOC_QUERYBUF, &(fg->buffers[i].buffer)) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_QUERYBUF)\n");
+ close(fg->dev_fd); fg->dev_fd = -1;
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ }
+ for (i = 0; i < (fg->buffers_num); i++)
+ {
+ fg->buffers[i].video_map = mmap(NULL, fg->buffers[i].buffer.length,
PROT_READ|PROT_WRITE, MAP_SHARED, fg->dev_fd, fg->buffers[i].buffer.m.offset);
+ if (fg->buffers[i].video_map == MAP_FAILED)
+ {
+ fprintf(stderr, "cannot mmap()\n");
+ fg->buffers[i].video_map = NULL;
+ for (i = 0; i < REQUEST_BUFFERS; i++)
+ {
+ if (fg->buffers[i].video_map)
+ {
+ munmap(fg->buffers[i].video_map, fg->buffers[i].buffer.length);
+ fg->buffers[i].video_map = NULL;
+ }
+ }
+ close(fg->dev_fd); fg->dev_fd = -1;
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ }
+ return fg;
+}
+
+void close_fg(void * fg)
+{
+ int i;
+
+ if (FG(fg)->grabbing) stop_grab(fg);
+ for (i = 0; i < REQUEST_BUFFERS; i++)
+ {
+ if (FG(fg)->buffers[i].video_map)
+ {
+ munmap(FG(fg)->buffers[i].video_map, FG(fg)->buffers[i].buffer.length);
+ FG(fg)->buffers[i].video_map = NULL;
+ }
+ }
+ if (FG(fg)->image) free(FG(fg)->image);
+ FG(fg)->image = NULL;
+ if (FG(fg)->bayerbuf) free(FG(fg)->bayerbuf);
+ FG(fg)->bayerbuf = NULL;
+ FG(fg)->bayerbuf_size = 0;
+ close(FG(fg)->dev_fd); FG(fg)->dev_fd = -1;
+ free(fg);
+}
Added: code/player/trunk/server/drivers/camera/v4l2/v4l2.h
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/v4l2.h
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/v4l2.h 2009-07-10 00:27:48 UTC
(rev 7966)
@@ -0,0 +1,23 @@
+#ifndef _V4L2_H
+#define _V4L2_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern void * open_fg(const char * dev, const char * pixformat, int width, int
height, int imgdepth, int buffers);
+extern void close_fg(void * fg);
+extern int set_channel(void * fg, int channel, const char * mode);
+extern int start_grab (void * fg);
+extern void stop_grab (void * fg);
+extern unsigned char * get_image(void * fg);
+extern int fg_width(void * fg);
+extern int fg_height(void * fg);
+extern int fg_grabdepth(void * fg);
+extern int fg_imgdepth(void * fg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Enter the BlackBerry Developer Challenge
This is your chance to win up to $100,000 in prizes! For a limited time,
vendors submitting new applications to BlackBerry App World(TM) will have
the opportunity to enter the BlackBerry Developer Challenge. See full prize
details at: http://p.sf.net/sfu/Challenge
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
