/*
Copyright (c) 2010 Mosalam Ebrahimi
Based on V4L2 video capture example
Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use,
copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following
conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
*/
#include
#include
#include
#include /* low-level i/o */
#include
#include
#include
#include
#include
#include
#include
#include
#include
/* for videodev2.h */#include
#include "framegrabber.h"
#define CLEAR(x) memset (&(x), 0, sizeof (x))
using namespace std;
FrameGrabber::FrameGrabber(string device_name, int w, int h, int buffers):
fd(-1), failed(0)
{
dev_name.assign(device_name);
width = w;
height = h;
n_buffers = buffers;
pixel_size = 2;
}
FrameGrabber::~FrameGrabber()
{
}
int FrameGrabber::Init()
{
// open device
struct stat st;
if (-1 == stat (dev_name.c_str(), &st)) {
fprintf (stderr, "Cannot identify '%s': %d, %s\n",
dev_name.c_str(), errno, strerror (errno));
return 0;
}
if (!S_ISCHR (st.st_mode)) {
fprintf (stderr, "%s is no device\n", dev_name.c_str());
return 0;
}
fd = open (dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
if (-1 == fd) {
fprintf (stderr, "Cannot open '%s': %d, %s\n",
dev_name.c_str(), errno, strerror (errno));
return 0;
}
// init
struct v4l2_capability cap;
struct v4l2_cropcap cropcap;
struct v4l2_crop crop;
struct v4l2_format fmt;
unsigned int min;
if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
if (EINVAL == errno) {
fprintf(stderr, "%s is no V4L2 device\n",
dev_name.c_str());
return 0;
} else {
fprintf(stderr, "VIDIOC_QUERYCAP\n");
return 0;
}
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
fprintf(stderr, "%s is no video capture device\n",
dev_name.c_str());
return 0;
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
fprintf(stderr, "%s does not support streaming i/o\n",
dev_name.c_str());
return 0;
}
// Select video input, video standard and tune here
CLEAR (cropcap);
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
crop.c = cropcap.defrect; // reset to default
if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop)) {
switch (errno) {
case EINVAL:
// Cropping not supported
break;
default:
// Errors ignored
break;
}
}
} else {
// Errors ignored
}
CLEAR(fmt);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = width;
fmt.fmt.pix.height = height;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt)) {
fprintf(stderr, "VIDIOC_S_FMT\n");
return 0;
}
// Note VIDIOC_S_FMT may change width and height
width = fmt.fmt.pix.width;
height = fmt.fmt.pix.height;
// Buggy driver paranoia
min = fmt.fmt.pix.width * 2;
if (fmt.fmt.pix.bytesperline < min)
fmt.fmt.pix.bytesperline = min;
min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
if (fmt.fmt.pix.sizeimage < min)
fmt.fmt.pix.sizeimage = min;
// init mmap
struct v4l2_requestbuffers req;
CLEAR(req);
req.count = n_buffers;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
fprintf(stderr, "%s does not support "
"memory mapping\n", dev_name.c_str());
return 0;
} else {
fprintf(stderr, "VIDIOC_REQBUFS");
return 0;
}
}
if (req.count < 2) {
fprintf(stderr, "Insufficient buffer memory on %s\n",
dev_name.c_str());
return 0;
}
// number of allocated buffers
n_buffers = req.count;
buffers = static_cast(calloc(req.count, sizeof (*buffers)));
if (!buffers) {
fprintf(stderr, "Out of memory\n");
return 0;
}
for (unsigned int i_buffer = 0; i_buffer < req.count; ++i_buffer) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i_buffer;
if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf)) {
fprintf(stderr, "VIDIOC_QUERYBUF");
return 0;
}
buffers[i_buffer].length = buf.length;
buffers[i_buffer].start =
mmap(NULL /* start anywhere */,
buf.length,
PROT_READ | PROT_WRITE /* required */,
MAP_SHARED /* recommended */,
fd, buf.m.offset);
if (MAP_FAILED == buffers[i_buffer].start) {
fprintf(stderr, "VIDIOC_QUERYBUF");
return 0;
}
}
return 1;
}
int FrameGrabber::StartCapturing()
{
unsigned int i;
enum v4l2_buf_type type;
for (i = 0; i < n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) {
fprintf(stderr, "VIDIOC_QBUF");
return 0;
}
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl (fd, VIDIOC_STREAMON, &type)) {
fprintf(stderr, "VIDIOC_STREAMON");
return 0;
}
return 1;
}
int FrameGrabber::StopCapturing()
{
enum v4l2_buf_type type;
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type)) {
fprintf(stderr, "VIDIOC_STREAMOFF");
return 0;
}
return 1;
}
int FrameGrabber::Uninit()
{
unsigned int i;
for (i = 0; i < n_buffers; ++i)
if (-1 == munmap (buffers[i].start, buffers[i].length)) {
fprintf(stderr, "munmap!!!\n");
return 0;
}
free (buffers);
return 1;
}
int FrameGrabber::GrabFrame(unsigned char* img)
{
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (fd, &fds);
// timeout
tv.tv_sec = 0;
tv.tv_usec = 10;
// check if the next frame is ready
r = select (fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
// next frame is not ready
if (EINTR == errno)
return 2;
// select error
return 3;
}
if (0 == r) {
// select timeout
return 4;
}
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
return 0;
case EIO:
// Could ignore EIO, see spec
// fall through
default:
// VIDIOC_DQBUF
return 5;
}
}
assert(buf.index < n_buffers);
unsigned char* ptr = static_cast(buffers[buf.index].start);
memcpy(img, ptr, height*width*pixel_size);
if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
// VIDIOC_QBUF
return 6;
return 1;
}
int FrameGrabber::xioctl(int fd, int request, void* arg)
{
int r;
do r = ioctl (fd, request, arg);
while (-1 == r && EINTR == errno);
return r;
}
/*
Copyright (c) 2010 Mosalam Ebrahimi
Based on V4L2 video capture example
Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use,
copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following
conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef FRAMEGRABBER_H
#define FRAMEGRABBER_H
#include
struct buffer {
void* start;
size_t length;
};
class FrameGrabber
{
public:
FrameGrabber(std::string device_name, int width, int height,
int num_buffers);
int Init();
int StartCapturing();
int StopCapturing();
int Uninit();
int GrabFrame(unsigned char* img);
~FrameGrabber();
private:
int xioctl(int fd, int request, void* arg);
std::string dev_name;
int fd;
unsigned int n_buffers;
int failed;
int width;
int height;
int pixel_size;
struct buffer* buffers;
};
#endif // FRAMEGRABBER_H
/*
g++ -O2 -Wall -DNO_FREETYPE geotag.c framegrabber.cpp -o geotag -lgps
-lpngwriter -lpng -lsqlite3
Copyright (c) 2010 Mosalam Ebrahimi
Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use,
copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following
conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
*/
#include
#include
#include
#include
#include
#include
#include "framegrabber.h"
bool isConn2GPSD = false;
struct gps_data_t *gpsdata = 0;
bool run = true;
void SigHandler(int sig)
{
run = false;
printf("\nExit\n");
}
void ConnectToGPSD()
{
if (gpsdata == NULL) {
char server[] = "localhost";
char port[] = "2947";
gpsdata = gps_open(server, port);
if (gpsdata != NULL) {
isConn2GPSD = true;
gps_stream(gpsdata,
WATCH_ENABLE|WATCH_NEWSTYLE|POLL_NONBLOCK,
NULL);
}
}
}
int main()
{
signal(SIGINT, &SigHandler);
ConnectToGPSD();
sqlite3 *db;
sqlite3_open("images.db", &db);
// vga
const size_t width = 640;
const size_t height = 480;
// yuyv
const size_t depth = 2;
const int image_size = width * height * depth;
// buffer for grabbed frame
unsigned char* tmpBuff = (unsigned char*) calloc(image_size,
sizeof(unsigned char));
// number of buffers for v4l mmap streaming
const size_t n_buffers = 6;
FrameGrabber cam("/dev/video1", width, height, n_buffers);
cam.Init();
cam.StartCapturing();
// image number
int idx = 0;
// main loop
for (;run;) {
// busy loop: wait for a new frame
while (cam.GrabFrame(tmpBuff) != 1);
// check if gpsd has new data
if (!gps_waiting(gpsdata))
continue;
// request data from gpsd
if (gps_poll(gpsdata) != 0) {
fprintf( stderr, "socket error\n");
exit(2);
}
// hack?, skip SKY, pass TPV
if (!(gpsdata->set & LATLON_SET))
continue;
gpsdata->set = 0;
printf("time: %f Lat: %f Lon: %f Image: %d\n",
gpsdata->fix.time,
gpsdata->fix.latitude,
gpsdata->fix.longitude,
idx);
// save png
char filename[15] = "";
sprintf(filename, "%d.png", idx);
pngwriter png(width, height, 0, filename);
for (size_t y=0; yfix.latitude,
gpsdata->fix.longitude,
idx);
sqlite3_exec(db, cmnd, NULL, 0, NULL);
idx++;
}
gps_close(gpsdata);
cam.StopCapturing();
cam.Uninit();
free(tmpBuff);
sqlite3_close(db);
return 0;
}
