Revision: 8847
http://playerstage.svn.sourceforge.net/playerstage/?rev=8847&view=rev
Author: jpgr87
Date: 2010-08-07 22:41:05 +0000 (Sat, 07 Aug 2010)
Log Message:
-----------
Applied patch #3040823: huge improvements in camerav4l2 driver
Modified Paths:
--------------
code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt
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
Added Paths:
-----------
code/player/trunk/server/drivers/camera/v4l2/geode.c
code/player/trunk/server/drivers/camera/v4l2/geode.h
Modified: code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt 2010-08-07
22:22:20 UTC (rev 8846)
+++ code/player/trunk/server/drivers/camera/v4l2/CMakeLists.txt 2010-08-07
22:41:05 UTC (rev 8847)
@@ -1,4 +1,6 @@
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)
+PLAYERDRIVER_REQUIRE_HEADER (camerav4l2 build_camerav4l2 linux/i2c-dev.h
sys/types.h)
+PLAYERDRIVER_REQUIRE_HEADER (camerav4l2 build_camerav4l2 linux/i2c.h
sys/types.h)
+PLAYERDRIVER_ADD_DRIVER (camerav4l2 build_camerav4l2 SOURCES bayer.c geode.c
v4l2.c camerav4l2.cc)
Modified: code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc 2010-08-07
22:22:20 UTC (rev 8846)
+++ code/player/trunk/server/drivers/camera/v4l2/camerav4l2.cc 2010-08-07
22:41:05 UTC (rev 8847)
@@ -53,13 +53,22 @@
- Default: "/dev/video0"
- Device to read video data from.
+- geode (integer)
+ - Default: 0
+ - Set to 1 for SoC camera on AMD Geode.
+
+- i2c (string)
+ - Default: "/dev/i2c-0"
+ - i2C device for changing sources on AMD Geode (see below).
+
- 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 default.
+ interfaces as the number of sources is given for this option. Source
+ channel numbers are used as keys ('ch' prefixed) in 'provides' tuple.
+ - If not given, channel 0 alone will be used by default
+ (channel 1 alone will be used by default for AMD Geode).
- Note that switching between channels takes time. Framerate drops
dramatically whenever more than one channel is used.
@@ -77,17 +86,18 @@
not support the requested size).
- mode (string)
- - Default: "BGR3"
+ - Default: "BGR3" ("YUYV" for AMD Geode)
- Desired capture mode. Can be one of:
- GREY (8-bit monochrome)
- RGBP (16-bit packed; will produce 24-bit color images)
+ - YUYV (16-bit packed, will produce 24-bit color images)
- BGR3, RGB3 (24-bit RGB)
- BGR4, RGB4 (32-bit RGB)
- BA81 (for sn9c1xx-based USB webcams; will produce 24-bit color images)
- MJPG (for webcams producing MJPEG streams not decompressed by V4L2
driver)
- buffers (integer)
- - Default: 2
+ - Default: 2 (3 for AMD Geode)
- 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.
@@ -194,6 +204,7 @@
/** @} */
#include "v4l2.h"
+#include "geode.h"
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
@@ -228,11 +239,12 @@
// Main function for device thread.
virtual void Main();
int useSource();
- int setSource();
+ int setSource(int wait);
int prepareData(player_camera_data_t * data, int sw);
int started;
const char * port;
+ const char * i2c;
const char * mode;
int buffers;
int sources_count;
@@ -252,6 +264,7 @@
int request_only;
int failsafe;
int jpeg;
+ int geode;
};
CameraV4L2::CameraV4L2(ConfigFile * cf, int section)
@@ -263,6 +276,7 @@
this->started = 0;
this->port = NULL;
+ this->i2c = NULL;
this->mode = NULL;
this->buffers = 0;
this->sources_count = 0;
@@ -280,14 +294,16 @@
this->request_only = 0;
this->failsafe = 0;
this->jpeg = 0;
+ this->geode = 0;
memset(this->sources, 0, sizeof this->sources);
memset(this->camera_addrs, 0, sizeof this->camera_addrs);
+ this->geode = cf->ReadInt(section, "geode", 0);
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;
+ this->sources[0] = (this->geode) ? 1 : 0;
+ PLAYER_WARN1("Implicitly using channel %d", this->sources[0]);
if (cf->ReadDeviceAddr(&(this->camera_addrs[0]), section, "provides",
PLAYER_CAMERA_CODE, -1, NULL))
{
@@ -336,7 +352,13 @@
this->SetError(-1);
return;
}
- this->mode = cf->ReadString(section, "mode", "BGR3");
+ this->i2c = cf->ReadString(section, "i2c", "/dev/i2c-0");
+ if (!(this->i2c))
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->mode = cf->ReadString(section, "mode", (this->geode) ? "YUYV" :
"BGR3");
if (!(this->mode))
{
this->SetError(-1);
@@ -350,6 +372,10 @@
{
this->format = PLAYER_CAMERA_FORMAT_RGB888;
this->bpp = 24;
+ } else if (!(strcmp(this->mode, "YUYV")))
+ {
+ this->format = PLAYER_CAMERA_FORMAT_RGB888;
+ this->bpp = 24;
} else if (!(strcmp(this->mode, "BGR3")))
{
this->format = PLAYER_CAMERA_FORMAT_RGB888;
@@ -408,6 +434,15 @@
this->width = 320;
this->height = 240;
}
+ if (this->geode)
+ {
+ if (strcmp(this->norm, "NTSC"))
+ {
+ PLAYER_ERROR("Set NTSC for AMD Geode");
+ this->SetError(-1);
+ return;
+ }
+ }
if (cf->GetTupleCount(section, "size") == 2)
{
this->width = cf->ReadTupleInt(section, "size", 0, this->width);
@@ -418,7 +453,7 @@
this->SetError(-1);
return;
}
- this->buffers = cf->ReadInt(section, "buffers", 2);
+ this->buffers = cf->ReadInt(section, "buffers", (this->geode) ? 3 : 2);
if ((this->buffers) <= 0)
{
this->SetError(-1);
@@ -447,33 +482,43 @@
this->fg = NULL;
}
-int CameraV4L2::setSource()
+int CameraV4L2::setSource(int wait)
{
int dropped = 0;
double start_time, t;
struct timespec tspec;
+ int selected;
- if (set_channel(this->fg, this->sources[this->current_source], this->norm) <
0)
+ if (this->started) return -1;
+ if (this->geode) selected = geode_select_cam(this->i2c,
this->sources[this->current_source]);
+ else selected = set_channel(this->fg, this->sources[this->current_source],
this->norm);
+ if (selected < 0)
{
PLAYER_ERROR1("Cannot set channel %d",
this->sources[this->current_source]);
return -1;
}
- if (start_grab(this->fg) < 0)
+ if (wait)
{
- 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++;
+ 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;
}
@@ -491,7 +536,7 @@
if ((this->next_source) >= (this->sources_count)) this->next_source = 0;
}
if (((this->current_source) < 0) || ((this->current_source) >=
(this->sources_count))) return -1;
- if (this->setSource() < 0) return -1;
+ if (this->setSource(!0) < 0) return -1;
return 0;
}
@@ -500,7 +545,8 @@
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->geode) this->fg = geode_open_fg(this->port, this->mode,
this->width, this->height, (this->bpp) / 8, this->buffers);
+ else this->fg = open_fg(this->port, this->mode, this->width, this->height,
(this->bpp) / 8, this->buffers);
if (!(this->fg)) return -1;
return this->useSource();
}
@@ -536,7 +582,7 @@
pthread_testcancel();
// Process any pending requests.
- ProcessMessages();
+ this->ProcessMessages();
data = reinterpret_cast<player_camera_data_t
*>(malloc(sizeof(player_camera_data_t)));
if (!data)
@@ -678,6 +724,7 @@
char previousNorm[MAX_NORM_LEN + 1];
int previousSource;
int i;
+ struct timespec tspec;
assert(hdr);
if ((this->sources_count) == 1)
@@ -712,28 +759,34 @@
this->sources[this->current_source] = src->source;
if (this->started) stop_grab(this->fg);
this->started = 0;
- if (this->setSource())
+ if (this->setSource(0))
{
this->Publish(this->camera_addrs[0],
resp_queue,
PLAYER_MSGTYPE_RESP_NACK,
PLAYER_CAMERA_REQ_SET_SOURCE,
data);
- if (this->started) stop_grab(this->fg);
- this->started = 0;
+ assert(!(this->started));
} else
{
this->Publish(this->camera_addrs[0],
resp_queue,
- (this->started) ? PLAYER_MSGTYPE_RESP_ACK :
PLAYER_MSGTYPE_RESP_NACK,
+ PLAYER_MSGTYPE_RESP_ACK,
PLAYER_CAMERA_REQ_SET_SOURCE,
data);
+ assert(!(this->started));
+ // this takes too long time, so let's call it after request response:
+ tspec.tv_sec = 0;
+ tspec.tv_nsec = this->sleep_nsec;
+ nanosleep(&tspec, NULL);
+ if (start_grab(this->fg) < 0) PLAYER_ERROR1("Cannot start grab on
channel %d", this->sources[this->current_source]);
+ else this->started = !0;
}
if (!(this->started))
{
snprintf(this->norm, sizeof this->norm, "%s", previousNorm);
this->sources[this->current_source] = previousSource;
- if (this->setSource()) PLAYER_ERROR("Cannot switch back to previous
channel!");
+ if (this->setSource(!0)) PLAYER_ERROR("Cannot switch back to previous
channel!");
}
return 0;
}
Added: code/player/trunk/server/drivers/camera/v4l2/geode.c
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/geode.c
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/geode.c 2010-08-07
22:41:05 UTC (rev 8847)
@@ -0,0 +1,347 @@
+/*
+ * 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
+ *
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: AMD Geode SoC camera support for camerav4l2 driver
+// Author: Paul Osmialowski
+//
+///////////////////////////////////////////////////////////////////////////
+
+#include "geode.h"
+#include "v4l2.h"
+#include <unistd.h>
+#include <sys/types.h>
+#include <linux/videodev2.h>
+#include <linux/i2c-dev.h>
+#include <linux/i2c.h>
+#include <fcntl.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+#ifndef V4L2_CID_CAM_INIT
+#define V4L2_CID_CAM_INIT (V4L2_CID_BASE+33)
+#endif
+
+int geode_select_cam(const char * dev, int cam)
+{
+ int i;
+ struct i2c_smbus_ioctl_data args;
+ union i2c_smbus_data data;
+
+ memset(&args, 0, sizeof args);
+ memset(&data, 0, sizeof data);
+ if (!((cam == 1) || (cam == 2))) return -1;
+ i = open(dev, O_RDWR);
+ if (i == -1) return -1;
+ if (ioctl(i, 0x703, 8) == -1)
+ {
+ close(i);
+ return -1;
+ }
+ args.read_write = I2C_SMBUS_READ;
+ args.command = 170;
+ args.size = I2C_SMBUS_BYTE_DATA;
+ args.data = &data;
+ if (ioctl(i, I2C_SMBUS, &args) == -1)
+ {
+ close(i);
+ return -1;
+ }
+ args.read_write = I2C_SMBUS_WRITE;
+ args.command = 220;
+ args.size = I2C_SMBUS_BLOCK_DATA;
+ args.data = &data;
+ data.block[0] = 1;
+ data.block[1] = cam;
+ if (ioctl(i, I2C_SMBUS, &args) == -1)
+ {
+ close(i);
+ return -1;
+ }
+ close(i);
+ return 0;
+}
+
+void * geode_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 v4l2_control control;
+ struct v4l2_streamparm fps;
+ struct fg_struct * fg;
+ v4l2_std_id esid0;
+
+ 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;
+ switch (width)
+ {
+ case 320:
+ if (height != 240)
+ {
+ fprintf(stderr, "invalid image size\n");
+ free(fg);
+ return NULL;
+ }
+ break;
+ case 640:
+ if (height != 480)
+ {
+ fprintf(stderr, "invalid image size\n");
+ free(fg);
+ return NULL;
+ }
+ break;
+ default:
+ 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("YUYV"))
+ {
+ fg->depth = 2;
+ 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(&control, 0, sizeof control);
+ control.id = V4L2_CID_CAM_INIT;
+ control.value = 0;
+ if (ioctl(fg->dev_fd, VIDIOC_S_CTRL, &control) < 0)
+ {
+ fprintf(stderr, "ioctl error (V4L2_CID_CAM_INIT)\n");
+ free(fg->image);
+ fg->image = NULL;
+ if (fg->bayerbuf) free(fg->bayerbuf);
+ fg->bayerbuf = NULL;
+ fg->bayerbuf_size = 0;
+ free(fg);
+ return NULL;
+ }
+ esid0 = (width == 320) ? 0x04000000UL : 0x08000000UL;
+ if (ioctl(fg->dev_fd, VIDIOC_S_STD, &esid0))
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_S_STD)\n");
+ 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_NONE;
+ 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;
+ }
+ if (format.fmt.pix.sizeimage != ((fg->pixels) * (fg->depth)))
+ {
+ fprintf(stderr, "unexpected size change\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(&fps, 0, sizeof fps);
+ fps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (ioctl(fg->dev_fd, VIDIOC_G_PARM, &fps))
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_G_PARM)\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;
+ }
+ fps.parm.capture.timeperframe.numerator = 1;
+ fps.parm.capture.timeperframe.denominator = 30;
+ if (ioctl(fg->dev_fd, VIDIOC_S_PARM, &fps) == -1)
+ {
+ fprintf(stderr, "ioctl error (VIDIOC_S_PARM)\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;
+}
Added: code/player/trunk/server/drivers/camera/v4l2/geode.h
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/geode.h
(rev 0)
+++ code/player/trunk/server/drivers/camera/v4l2/geode.h 2010-08-07
22:41:05 UTC (rev 8847)
@@ -0,0 +1,41 @@
+/*
+ * 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
+ *
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: AMD Geode SoC camera support for camerav4l2 driver
+// Author: Paul Osmialowski
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef _GEODE_H
+#define _GEODE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern int geode_select_cam(const char * dev, int cam);
+extern void * geode_open_fg(const char * dev, const char * pixformat, int
width, int height, int imgdepth, int buffers);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
Modified: code/player/trunk/server/drivers/camera/v4l2/v4l2.c
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/v4l2.c 2010-08-07 22:22:20 UTC
(rev 8846)
+++ code/player/trunk/server/drivers/camera/v4l2/v4l2.c 2010-08-07 22:41:05 UTC
(rev 8847)
@@ -17,6 +17,12 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: Video4Linux2 routines for camerav4l2 driver
+// Author: Paul Osmialowski
+//
+///////////////////////////////////////////////////////////////////////////
#include "v4l2.h"
#include "bayer.h"
@@ -31,37 +37,8 @@
#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;
@@ -158,17 +135,30 @@
}
}
+#define CLIP(c) ((unsigned char)(((c) > 0xff) ? 0xff : (((c) < 0) ? 0 : (c))))
+
unsigned char * get_image(void * fg)
{
enum v4l2_buf_type type;
int i, grabdepth;
+ int u, v, u1, rg, v1;
const unsigned char * buf;
unsigned char * img;
int count, insize;
+ int fit;
unsigned char table5[] = { 0, 8, 16, 25, 33, 41, 49, 58, 66, 74, 82, 90, 99,
107, 115, 123, 132, 140, 148, 156, 165, 173, 181, 189, 197, 206, 214, 222,
230, 239, 247, 255 };
unsigned char table6[] = { 0, 4, 8, 12, 16, 20, 24, 28, 32, 36, 40, 45, 49,
53, 57, 61, 65, 69, 73, 77, 81, 85, 89, 93, 97, 101, 105, 109, 113, 117, 121,
125, 130, 134, 138, 142, 146, 150, 154, 158, 162, 166, 170, 174, 178, 182, 186,
190, 194, 198, 202, 206, 210, 215, 219, 223, 227, 231, 235, 239, 243, 247, 251,
255 };
- if ((!(FG(fg)->grabbing)) || (!(FG(fg)->image))) return NULL;
+ if (!(FG(fg)->grabbing))
+ {
+ fprintf(stderr, "grabbing not started\n");
+ return NULL;
+ }
+ if (!(FG(fg)->image))
+ {
+ fprintf(stderr, "image not allocated\n");
+ 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;
@@ -184,25 +174,62 @@
return NULL;
}
grabdepth = FG(fg)->depth;
+ fit = 0;
if ((FG(fg)->pixformat) == v4l2_fmtbyname("BA81"))
{
- if (!(FG(fg)->bayerbuf)) return NULL;
+ if (!(FG(fg)->bayerbuf))
+ {
+ fprintf(stderr, "BA81: no buffer allocated\n");
+ 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;
+ if (grabdepth == (FG(fg)->imgdepth)) fit = !0;
} else if ((FG(fg)->pixformat) == v4l2_fmtbyname("RGBP"))
{
- if (!(FG(fg)->bayerbuf)) return NULL;
+ if (!(FG(fg)->bayerbuf))
+ {
+ fprintf(stderr, "RGBP: no buffer allocated\n");
+ return NULL;
+ }
img = FG(fg)->bayerbuf;
for (i = 0; i < (FG(fg)->pixels); i++)
{
- img[0] = table5[(buf[1]) >> 3];
- img[1] = table6[(((buf[1]) & 7) << 3) | ((buf[0]) >> 5)];
- img[2] = table5[(buf[0]) & 0xe0];
- img += 3; buf += 2;
+ img[0] = table5[(buf[1]) >> 3];
+ img[1] = table6[(((buf[1]) & 7) << 3) | ((buf[0]) >> 5)];
+ img[2] = table5[(buf[0]) & 0xe0];
+ img += 3; buf += 2;
}
buf = FG(fg)->bayerbuf;
grabdepth = 3;
+ if (grabdepth == (FG(fg)->imgdepth)) fit = !0;
+ } else if ((FG(fg)->pixformat) == v4l2_fmtbyname("YUYV"))
+ {
+ if (!(FG(fg)->bayerbuf))
+ {
+ fprintf(stderr, "YUYV: no buffer allocated\n");
+ return NULL;
+ }
+ img = FG(fg)->bayerbuf;
+ for (i = 0; i < (FG(fg)->pixels); i += 2)
+ {
+ u = buf[1];
+ v = buf[3];
+ u1 = (((u - 128) << 7) + (u - 128)) >> 6;
+ rg = (((u - 128) << 1) + (u - 128) + ((v - 128) << 2) + ((v - 128) << 1))
>> 3;
+ v1 = (((v - 128) << 1) + (v - 128)) >> 1;
+ img[0] = CLIP(buf[0] + v1);
+ img[1] = CLIP(buf[0] - rg);
+ img[2] = CLIP(buf[0] + u1);
+ img[3] = CLIP(buf[2] + v1);
+ img[4] = CLIP(buf[2] - rg);
+ img[5] = CLIP(buf[2] + u1);
+ img += 6; buf += 4;
+ }
+ buf = FG(fg)->bayerbuf;
+ grabdepth = 3;
+ if (grabdepth == (FG(fg)->imgdepth)) fit = !0;
}
img = FG(fg)->image;
if ((FG(fg)->pixformat) == v4l2_fmtbyname("MJPG"))
@@ -219,19 +246,19 @@
}
if (insize > 1)
{
- memcpy(img, &insize, sizeof(int));
- memcpy(img + sizeof(int), buf, insize);
+ memcpy(img, &insize, sizeof(int));
+ memcpy(img + sizeof(int), buf, insize);
} else
{
- fprintf(stderr, "Internal error\n");
- return NULL;
+ fprintf(stderr, "Internal error\n");
+ return NULL;
}
- } else for (i = 0; i < (FG(fg)->pixels); i++)
+ } else if (!fit) for (i = 0; i < (FG(fg)->pixels); i++)
{
- switch(FG(fg)->imgdepth)
+ switch (FG(fg)->imgdepth)
{
case 1:
- switch(grabdepth)
+ switch (grabdepth)
{
case 1:
img[0] = buf[0];
@@ -248,7 +275,7 @@
}
break;
case 3:
- switch(grabdepth)
+ switch (grabdepth)
{
case 1:
img[0] = buf[0];
@@ -271,7 +298,7 @@
}
break;
case 4:
- switch(grabdepth)
+ switch (grabdepth)
{
case 1:
img[0] = buf[0];
@@ -307,7 +334,9 @@
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;
+ img = fit ? (FG(fg)->bayerbuf) : (FG(fg)->image);
+ if (!img) fprintf(stderr, "Internal error: NULL\n");
+ return img;
}
void * open_fg(const char * dev, const char * pixformat, int width, int
height, int imgdepth, int buffers)
Modified: code/player/trunk/server/drivers/camera/v4l2/v4l2.h
===================================================================
--- code/player/trunk/server/drivers/camera/v4l2/v4l2.h 2010-08-07 22:22:20 UTC
(rev 8846)
+++ code/player/trunk/server/drivers/camera/v4l2/v4l2.h 2010-08-07 22:41:05 UTC
(rev 8847)
@@ -17,14 +17,52 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: Video4Linux2 routines for camerav4l2 driver
+// Author: Paul Osmialowski
+//
+///////////////////////////////////////////////////////////////////////////
#ifndef _V4L2_H
#define _V4L2_H
+#include <sys/types.h>
+#include <linux/videodev2.h>
+
#ifdef __cplusplus
extern "C" {
#endif
+#define v4l2_fmtbyname(name) v4l2_fourcc((name)[0], (name)[1], (name)[2],
(name)[3])
+
+#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))
+
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);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.net email is sponsored by
Make an app they can't live without
Enter the BlackBerry Developer Challenge
http://p.sf.net/sfu/RIM-dev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
