From 9ee45e8d830755f2ca278e0b738b8914ea1a00f1 Mon Sep 17 00:00:00 2001 From: Steffen Pohle Date: Sat, 2 Apr 2022 16:37:44 +0200 Subject: [PATCH] adding files --- Makefile | 44 +++++++ convert.c | 240 +++++++++++++++++++++++++++++++++++++ getvideo.c | 326 +++++++++++++++++++++++++++++++++++++++++++++++++++ image.c | 206 ++++++++++++++++++++++++++++++++ main.c | 130 ++++++++++++++++++++ vidoiltank.h | 120 +++++++++++++++++++ 6 files changed, 1066 insertions(+) create mode 100644 Makefile create mode 100644 convert.c create mode 100644 getvideo.c create mode 100644 image.c create mode 100644 main.c create mode 100644 vidoiltank.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..837d5d6 --- /dev/null +++ b/Makefile @@ -0,0 +1,44 @@ +VERSION=0.1 +PROGRAM=vidoiltank +CFLAGS=-O2 -ggdb -Wall +OBJS= getvideo.o image.o main.o convert.o +DIST=$(PROGRAM)-$(VERSION) +LIBS=-ljpeg + +all: $(PROGRAM) + +rebuild: clean all + +$(PROGRAM): $(OBJS) + gcc -o $@ $^ $(LIBS) + +%.o: %.c + gcc $(CFLAGS) -c -o $@ $^ $(DFLAGS) + +clean: + rm *.o -rf + rm $(PROGRAM) -rf + rm nohup.out -rf + rm core -rf + +cleanall: clean + rm *~ -rf + +source: cleanall + +dist: + rm -rf $(DIST) + mkdir $(DIST) + cp Makefile* $(DIST)/ + cp *.c $(DIST)/ -L + cp *.h $(DIST)/ -L + tar cvzf ../$(DIST).tgz $(DIST) + rm -rf $(DIST) + + + + + + + + diff --git a/convert.c b/convert.c new file mode 100644 index 0000000..3c8c40e --- /dev/null +++ b/convert.c @@ -0,0 +1,240 @@ + +#include "vidoiltank.h" + +// +// jpeg: replacement for error_exit +// +METHODDEF(void) jpg_error_exit (j_common_ptr cinfo) { + jpg_error_ptr myerr = (jpg_error_ptr) cinfo->err; + (*cinfo->err->output_message) (cinfo); + longjmp(myerr->setjmp_buffer, 1); +} + +// +// clamp and convert2rgb is build on the sample of the v4l2 api documentation +// +inline unsigned char clamp (double x) { + int r = (int)x; + + if (r < 0) return 0; + else if (r > 255) return 255; + else return r; +}; + + +inline void convert2rgb (unsigned char Y1, unsigned char Cb, unsigned char Cr, + unsigned char *ER, unsigned char *EB, unsigned char *EG) { + register int y1, pb, pr; + + y1 = Y1 - 16; + pb = Cb - 128; + pr = Cr - 128; + + *ER = clamp (y1 + 1.402 * pr); + *EB = clamp (y1 - 0.344 * pb - 0.714 * pr); + *EG = clamp (y1 + 1.772 * pb); +}; + + + +int convert (struct jpeg_decompress_struct *cdata, ImageRaw *dest, unsigned char *ptrsrc, int srcsize, uint32_t pixelformat, int srcw, int srch) { + int xs, ys; + int xd, yd; + unsigned char r,g,b; + unsigned char cb, cr, y1; + unsigned char *ptrdst = NULL; + + struct jpg_error_mgr jerr; + if (cdata == NULL) return -1; + + // check if there is a destination and that the destination is large to keep + // the full image + if (dest == NULL || ptrsrc == NULL) + return -1; + + if (dest->data != NULL && dest->w != srcw && dest->h != srch) { + free (dest->data); + dest->data = NULL; + } + + if (dest->data == NULL) { + dest->w = srcw; + dest->h = srch; + dest->size = srcw * srch * 3; + dest->data = (unsigned char*) malloc (dest->size); + } + ptrdst = dest->data; + + switch (pixelformat) { + case (V4L2_PIX_FMT_RGB32): + for (ys = 0, yd = 0; ys < (signed int)srch; ys++) { + for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) { + /* read the pixel */ + + ptrsrc++; + r = *(ptrsrc++); + g = *(ptrsrc++); + b = *(ptrsrc++); + + /* only paint the image if the source is within the destination */ + if (xd < dest->w) { + /* set the pixel */ + *(ptrdst++) = b; + *(ptrdst++) = g; + *(ptrdst++) = r; + xd++; + } + } + + /* if the source image is too small ignore the other places.. */ + if (xd < dest->w) + ptrdst += 3 * (dest->w - xd); + yd++; + } + break; + + case (V4L2_PIX_FMT_BGR32): + for (ys = 0, yd = 0; ys < (signed int)srch; ys++) { + for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) { + /* read the pixel */ + + b = *(ptrsrc++); + g = *(ptrsrc++); + r = *(ptrsrc++); + ptrsrc++; + + /* only paint the image if the source is within the destination */ + if (xd < dest->w) { + /* set the pixel */ + *(ptrdst++) = b; + *(ptrdst++) = g; + *(ptrdst++) = r; + xd++; + } + } + + /* if the source image is too small ignore the other places.. */ + if (xd < dest->w) + ptrdst += 3 * (dest->w - xd); + yd++; + } + break; + + case (V4L2_PIX_FMT_UYVY): + for (ys = 0, yd = 0; ys < (signed int)srch; ys++) { + for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) { + /* read the pixel */ + if (xs & 1) { + y1 = (unsigned char)*(ptrsrc + 1); + cr = (unsigned char)*(ptrsrc); + cb = (unsigned char)*(ptrsrc - 2); + } + else { + y1 = (unsigned char)*(ptrsrc + 1); + cr = (unsigned char)*(ptrsrc + 2); + cb = (unsigned char)*(ptrsrc); + } + + convert2rgb (y1, cr, cb, &r, &g, &b); + ptrsrc += 2; + + /* only paint the image if the source is within the destination */ + if (xd < dest->w) { + /* set the pixel */ + *(ptrdst++) = b; + *(ptrdst++) = g; + *(ptrdst++) = r; + xd++; + } + } + + /* if the source image is too small ignore the other places.. */ + if (xd < dest->w) + ptrdst += 3 * (dest->w - xd); + yd++; + } + break; + + case (V4L2_PIX_FMT_YUYV): + for (ys = 0, yd = 0; ys < (signed int)srch; ys++) { + if (yd < dest->h) { + for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) { + /* read the pixel */ + if (xs & 1) { + y1 = *(ptrsrc); + cb = *(ptrsrc + 1); + cr = *(ptrsrc - 1); + } + else { + y1 = *(ptrsrc); + cb = *(ptrsrc + 3); + cr = *(ptrsrc + 1); + } + convert2rgb (y1, cr, cb, &r, &g, &b); + ptrsrc += 2; + + /* only paint the image if the source is within the destination */ + if (xd < dest->w) { + /* set the pixel */ + *(ptrdst++) = r; + *(ptrdst++) = g; + *(ptrdst++) = b; + xd++; + } + } + + /* if the source image is too small ignore the other places.. */ + if (xd < dest->w) + ptrdst += 3 * (dest->w - xd); + yd++; + } + } + break; + + case (V4L2_PIX_FMT_MJPEG): + cdata->err = jpeg_std_error(&jerr.pub); + jerr.pub.error_exit = jpg_error_exit; + if (setjmp(jerr.setjmp_buffer)) { + // jpeg data and allocations will be destroyed via StopCapture. + printf ("%s:%d %s JPEG Error\n", __FILE__, __LINE__, __FUNCTION__); + return -1; + } + + jpeg_mem_src(cdata, ptrsrc, srcsize); + jpeg_read_header(cdata, TRUE); + jpeg_start_decompress(cdata); + + while (cdata->output_scanline < cdata->output_height) { + unsigned char *temp_array[] = {ptrdst + (cdata->output_scanline) * srcw * 3}; + jpeg_read_scanlines(cdata, temp_array, 1); + } + + jpeg_finish_decompress(cdata); + + break; + default: + break; + } + + return 0; +}; + + +int convertstart(struct jpeg_decompress_struct *cdata, uint32_t pixelformat) { + if (cdata == NULL) return -1; + + if (pixelformat == V4L2_PIX_FMT_MJPEG) { + jpeg_create_decompress(cdata); + } + + return 0; +}; + + +int convertstop(struct jpeg_decompress_struct *cdata, uint32_t pixelformat) { + if (cdata == NULL) return -1; + if (pixelformat == V4L2_PIX_FMT_MJPEG) jpeg_destroy_decompress(cdata); + return 0; +}; + + diff --git a/getvideo.c b/getvideo.c new file mode 100644 index 0000000..36b4e02 --- /dev/null +++ b/getvideo.c @@ -0,0 +1,326 @@ + +#include "vidoiltank.h" +#define CLEAR(_var_) memset (&_var_, 0x0 , sizeof(_var_)); + +int xioctl(int fd, int request, void *arg); +void printcaps(uint32_t caps); +int vid_initmmap(Video* video); + +/* + * helper functions + */ + +int xioctl(int fd, int request, void *arg) { + int r; + int errnoioctl; + struct timeval to1; + struct timeval to2; + float to; + + gettimeofday(&to1, NULL); + do { + r = ioctl(fd, request, arg); + errnoioctl = errno; + gettimeofday(&to2, NULL); + to = (float)(to2.tv_sec - to1.tv_sec) + ((to2.tv_usec - to1.tv_usec) / 1000000.0); + } while (r == -1 && errnoioctl == EINTR && to < 2.0); + + return r; +}; + + +void printcaps(uint32_t caps) { + printf ("%s:%d %s Caps: %x\n", __FILE__, __LINE__, __FUNCTION__, caps); + + if (caps & V4L2_CAP_VIDEO_CAPTURE) printf (" V4L2_CAP_VIDEO_CAPTURE\n"); + if (caps & V4L2_CAP_EXT_PIX_FORMAT) printf (" V4L2_CAP_EXT_PIX_FORMAT\n"); +#ifdef V4L2_CAP_META_CAPTURE + if (caps & V4L2_CAP_META_CAPTURE) printf (" V4L2_CAP_META_CAPTURE\n"); +#endif + if (caps & V4L2_CAP_STREAMING) printf (" V4L2_CAP_STREAMING\n"); + if (caps & V4L2_CAP_DEVICE_CAPS) printf (" V4L2_CAP_DEVICE_CAPS\n"); + if (caps & V4L2_CAP_TUNER) printf (" V4L2_CAP_TUNER\n"); + if (caps & V4L2_CAP_MODULATOR) printf (" V4L2_CAP_MODULATOR\n"); + if (caps & V4L2_CAP_READWRITE) printf (" V4L2_CAP_READWRITE\n"); + if (caps & V4L2_CAP_ASYNCIO) printf (" V4L2_CAP_ASYNCIO\n"); +} + + +/* + * + */ + +Video *vid_open(char *name) { + Video* video = malloc (sizeof(Video)); + strncpy (video->devname, name, LEN_FILENAME-1); + + // + // open device and get device name and capabilities | O_NONBLOCK + if((video->fd = open(video->devname, O_RDWR)) == -1) { + free (video); + return NULL; + } + + if(ioctl(video->fd, VIDIOC_QUERYCAP, &video->vcap) == -1) + strncpy ((char*)&video->vcap.card, "unknown", sizeof(video->vcap.card)); + +// printf ("%s:%d %s Capabilities: %u\n", __FILE__, __LINE__, __FUNCTION__, video->vcap.capabilities); +// printcaps(video->vcap.capabilities); +// printf ("%s:%d %s Device Capabilities: %u\n", __FILE__, __LINE__, __FUNCTION__, video->vcap.device_caps); +// printcaps(video->vcap.device_caps); + + if (!(video->vcap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { + printf ("%s:%d %s device has no video capture capabilities\n", __FILE__, __LINE__, __FUNCTION__); + close(video->fd); + free (video); + return NULL; + } + + // + // check for cropping.. if we have it setup default + CLEAR (video->cropcap); + video->cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (0 == xioctl (video->fd, VIDIOC_CROPCAP, &video->cropcap)) { + video->crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + video->crop.c = video->cropcap.defrect; // reset to default + + if (-1 == xioctl (video->fd, VIDIOC_S_CROP, &video->crop)) { + // errors here can be ignored + printf ("%s:%d %s VIDEOC_S_CROP Errorcode: %s\n", __FILE__, __LINE__, __FUNCTION__, strerror(errno)); + } + } + + return video; +} + + + +void vid_close(Video *video) { + convertstop(&video->jpgcinfo, video->fmt.fmt.pix.pixelformat); + close (video->fd); +} + + +int vid_start(Video* video, int iomode, int width, int height) { + int min; + char txt[16] = ""; + + // + // prepare resolution and pixelformat + CLEAR (video->fmt); + video->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (height != -1 && width != -1) { // resolution + video->fmt.fmt.pix.width = width; + video->fmt.fmt.pix.height = height; + } + else { + video->fmt.fmt.pix.width = 1920; + video->fmt.fmt.pix.height = 1080; + } + + video->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; + video->fmt.fmt.pix.field = V4L2_FIELD_NONE; + if (-1 == xioctl (video->fd, VIDIOC_S_FMT, &video->fmt)) { + fprintf (stderr, "%s:%d VIDIOC_S_FMT : %s\n", __FILE__, __LINE__, strerror (errno)); + return -1; + } + + // Note VIDIOC_S_FMT may change width and height. + // Buggy driver paranoia. - as written in the v4l2 api documentation + min = video->fmt.fmt.pix.width * 2; + if (video->fmt.fmt.pix.bytesperline < min) + video->fmt.fmt.pix.bytesperline = min; + min = video->fmt.fmt.pix.bytesperline * video->fmt.fmt.pix.height; + if (video->fmt.fmt.pix.sizeimage < min) + video->fmt.fmt.pix.sizeimage = min; + video->width = video->fmt.fmt.pix.width; + video->height = video->fmt.fmt.pix.height; + snprintf (txt, 32, "%c%c%c%c", ((char*)&video->fmt.fmt.pix.pixelformat)[0], + ((char*)&video->fmt.fmt.pix.pixelformat)[1], + ((char*)&video->fmt.fmt.pix.pixelformat)[2], + ((char*)&video->fmt.fmt.pix.pixelformat)[3]); + strncpy (video->format, txt, 16); +// printf ("%s:%d Video Format: %s\n", __FILE__, __LINE__, video->format); + + // init buffers + video->iomode = iomode; + switch (video->iomode) { + case IOM_MMAP: + if (vid_initmmap(video) == -1) + return -1; + break; + case IOM_READ: + default: + break; + } + + // convertstart // for mjpg only + convertstart (&video->jpgcinfo, video->fmt.fmt.pix.pixelformat); + + return 0; +}; + + +int vid_uninitmmap(Video *video) { +// printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + for (int inbuffer_idx = 0; inbuffer_idx < VDEV_INBUFFERS; inbuffer_idx++) { + if (video->inbuffer[inbuffer_idx].data) { + if (-1 == munmap(video->inbuffer[inbuffer_idx].data, video->inbuffer[inbuffer_idx].size)){ + fprintf(stderr, "Fatal Error @ %s:%d munmap Error:%s\n", __FILE__, __LINE__, strerror(errno)); + exit(1); + } + video->inbuffer[inbuffer_idx].data = NULL; + video->inbuffer[inbuffer_idx].size = 0; + } + } + return 0; +} + + +int vid_initmmap(Video *video) { + struct v4l2_requestbuffers bufreq; + struct v4l2_buffer bufinfo; + struct v4l2_buffer buf; + int i, type; + + CLEAR(bufreq); + CLEAR(bufinfo); + + bufreq.count = VDEV_INBUFFERS; + bufreq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + bufreq.memory = V4L2_MEMORY_MMAP; + + if (-1 == xioctl(video->fd, VIDIOC_REQBUFS, &bufreq)) { + if (EINVAL == errno) { + printf("device does not support memory mapping\n"); + return -1; + } else { + printf ("%s:%d %s Error %s\n", __FILE__, __LINE__, __FUNCTION__, strerror(errno)); + return -1; + } + } + + if (bufreq.count < 1) { + printf ( "Insufficient buffer memory\n"); + return -1; + } + + for (i = 0; i < VDEV_INBUFFERS; i++) { + CLEAR(bufinfo); + bufinfo.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + bufinfo.memory = V4L2_MEMORY_MMAP; + bufinfo.index = i; + + if(ioctl(video->fd, VIDIOC_QUERYBUF, &bufinfo) < 0){ + perror("VIDIOC_QUERYBUF"); + exit(1); + } + + video->inbuffer[i].size = bufinfo.length; + video->inbuffer[i].data = (unsigned char*)mmap(NULL, bufinfo.length, PROT_READ | PROT_WRITE, + MAP_SHARED, video->fd, bufinfo.m.offset); + if (video->inbuffer[i].data == MAP_FAILED) { + printf ( "%s:%d error on mmap %s\n", __FILE__, __LINE__, strerror(errno)); + exit (1); + } + } + + for (video->inbuffer_idx = 0; video->inbuffer_idx < VDEV_INBUFFERS; video->inbuffer_idx++) { + CLEAR(buf); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + buf.index = video->inbuffer_idx; + + if (-1 == xioctl(video->fd, VIDIOC_QBUF, &buf)) { + printf ( "%s:%d error on VIDIOC_QBUF %s\n", __FILE__, __LINE__, strerror(errno)); + return -1; + } + } + + video->inbuffer_idx = 0; + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (-1 == xioctl(video->fd, VIDIOC_STREAMON, &type)) { + printf ( "%s:%d VIDIOC_STREAMON %s\n", __FILE__, __LINE__, strerror(errno)); + return -1; + } + + return 0; +}; + + +ImageRaw *vid_grabimage(Video* video, ImageRaw *img) { + struct v4l2_buffer buf; + int len; + ImageRaw *in = img; + + if (in == NULL) in = image_alloc (video->width, video->height); + else if (img->w != video->width || img->h != video->height) { + image_resize(in, video->width, video->height); + } + + switch (video->iomode) { + case IOM_READ: + if ((len = read (video->fd, video->inbuffer[0].data, video->fmt.fmt.pix.sizeimage)) == -1) { + return NULL; + } + else { + convert(&video->jpgcinfo, in, video->inbuffer[0].data, len, video->fmt.fmt.pix.pixelformat, video->fmt.fmt.pix.width, video->fmt.fmt.pix.height); + } + break; + + case IOM_MMAP: + CLEAR(buf); + + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + + if (-1 == xioctl(video->fd, VIDIOC_DQBUF, &buf)) { + return NULL; + } + + if (buf.index >= 0 && buf.index < VDEV_INBUFFERS) { + convert(&video->jpgcinfo, in, video->inbuffer[buf.index].data, buf.bytesused, video->fmt.fmt.pix.pixelformat, video->fmt.fmt.pix.width, video->fmt.fmt.pix.height); } + + if (-1 == xioctl(video->fd, VIDIOC_QBUF, &buf)) { + printf ( "%s:%d error on VIDIOC_QBUF %s\n", __FILE__, __LINE__, strerror(errno)); + return NULL; + } + + if (++(video->inbuffer_idx) >= VDEV_INBUFFERS) video->inbuffer_idx = 0; + break; + + default: + printf ("%s:%d %s wrong IOMODE?\n", __FILE__, __LINE__, __FUNCTION__); + errno = EINVAL; + return NULL; + } + + return in; +}; + + +int vid_stop(Video* video) { + enum v4l2_buf_type type; +// printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + + convertstop(&video->jpgcinfo, video->fmt.fmt.pix.pixelformat); + + switch (video->iomode) { + case IOM_READ: + /* Nothing to do. */ + break; + case IOM_MMAP: + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (-1 == xioctl(video->fd, VIDIOC_STREAMOFF, &type)) { + fprintf(stderr, "%s:%d VIDIOC_STREAMOFF Error:%s\n", __FILE__, __LINE__, strerror(errno)); + return -1; + } + break; + default: + printf ("%s:%d %s wrong IOMODE?\n", __FILE__, __LINE__, __FUNCTION__); + return -1; + } + return 0; +}; + diff --git a/image.c b/image.c new file mode 100644 index 0000000..21478f1 --- /dev/null +++ b/image.c @@ -0,0 +1,206 @@ + +#include "vidoiltank.h" + +#define copypixel(_DEST_, _SRC_) *(_DEST_ + 0) = *(_SRC_ + 0); \ + *(_DEST_ + 1) = *(_SRC_ + 1); \ + *(_DEST_ + 2) = *(_SRC_ + 2); + +ImageRaw *image_alloc (int w, int h) { + ImageRaw *i = NULL; + + i = (ImageRaw *) malloc (sizeof (ImageRaw)); + i->size = w*h*3; + i->data = malloc(i->size); + i->w = w; + i->h = h; + + return i; +}; + + +void image_resize(ImageRaw *i, int w, int h) { + if (i == NULL) { + printf ("error: image_resize .. image points to NULL\n"); + exit (1); + } + + free (i->data); + i->size = w*h*3; + i->data = malloc(i->size); + i->w = w; + i->h = h; +}; + + +int image_save(ImageRaw *i, char *fn, int quality) { + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + FILE * outfile; /* target file */ + JSAMPROW row_pointer[1]; /* pointer to JSAMPLE row[s] */ + int row_stride; /* physical row width in image buffer */ + + if (i == NULL) { + printf ("image_save no image i == NULL\n"); + exit (1); + } + + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + + if ((outfile = fopen(fn, "wb")) == NULL) { + fprintf(stderr, "can't open %s\n", fn); + exit(1); + } + jpeg_stdio_dest(&cinfo, outfile); + + cinfo.image_width = i->w; /* image width and height, in pixels */ + cinfo.image_height = i->h; + cinfo.input_components = 3; /* # of color components per pixel */ + cinfo.in_color_space = JCS_RGB; /* colorspace of input image */ + jpeg_set_defaults(&cinfo); + jpeg_set_quality(&cinfo, quality, TRUE /* limit to baseline-JPEG values */); + jpeg_start_compress(&cinfo, TRUE); + row_stride = i->w * 3; /* JSAMPLEs per row in image_buffer */ + + while (cinfo.next_scanline < cinfo.image_height) { + row_pointer[0] = & i->data[cinfo.next_scanline * row_stride]; + (void) jpeg_write_scanlines(&cinfo, row_pointer, 1); + } + + jpeg_finish_compress(&cinfo); + fclose(outfile); + jpeg_destroy_compress(&cinfo); + + return 0; +}; + +void image_rotate (ImageRaw *img, int deg) { + if (img == NULL || img->data == NULL) return; + if (deg == 0) return; + + if (deg == 90 || deg == 270) { + int nx, ny, x, y; + int nw = img->h; + int nh = img->w; + unsigned char *old = img->data; + unsigned char *new = malloc (3 * img->w * img->h); + + for (y = 0; y < img->h; y++) for (x = 0; x < img->w; x++) { + if (deg == 90) { + nx = img->h - y -1; + ny = x; + } + else { + nx = y; + ny = img->w - x - 1; + } + copypixel(new+3*(nx + ny * nw), old+3*(x + y * img->w)); + } + + img->w = nw; + img->h = nh; + img->data = new; + free (old); + } + else if (deg == 180) { + int x, y, nx, ny; + unsigned char pixel[3]; + for (y = 0; y < (img->h/2); y++) for (x = 0; x < img->w; x++) { + nx = img->w - x - 1; + ny = img->h - y - 1; + copypixel(pixel, img->data+3*(nx + ny * img->w)); + copypixel(img->data+3*(nx + ny * img->w), img->data+3*(x + y * img->w)); + copypixel(img->data+3*(x + y * img->w), pixel); + } + } +}; + + +void image_draw_rect (ImageRaw *img, Rect *r, int col) { + if (r == NULL || img == NULL || img->data == NULL) return; + if (r->x + r->w > img->w || r->y + r->h > img->h) return; + int x, y; + unsigned char pixel[3]; + + memcpy (pixel, &col, 3); + + for (x = 0; x < r->w; x++) { + copypixel (img->data+3*(r->x+x+(r->y)*img->w), pixel); + copypixel (img->data+3*(r->x+x+(r->y+r->h)*img->w), pixel); + } + for (y = 0; y < r->h; y++) { + copypixel (img->data+3*(r->x+(r->y+y)*img->w), pixel); + copypixel (img->data+3*(r->x+r->w+(r->y+y)*img->w), pixel); + } +} + + + +ImageFloat *imageF_alloc (int w, int h) { + ImageFloat *i = NULL; + + i = (ImageFloat *) malloc (sizeof (ImageFloat)); + i->size = w*h*sizeof(float); + i->data = (float *)malloc(i->size); + i->w = w; + i->h = h; + + return i; +}; + + +ImageFloat *imageF_copy(ImageRaw *iraw, Rect *r) { + ImageFloat *ifloat = imageF_alloc(r->w, r->h); + int x, y; + float f; + + for (y = 0; y < r->h; y++) for (x = 0; x < r->w; x++) { + f = *(iraw->data+0+3*(r->x + x + (r->y + y)*iraw->w)) + + *(iraw->data+1+3*(r->x + x + (r->y + y)*iraw->w)) + + *(iraw->data+2+3*(r->x + x + (r->y + y)*iraw->w)); + f = f/3.0; + *(ifloat->data+(x+y*ifloat->w)) = f; + } + + return ifloat; +}; + +void imageF_add(ImageFloat *ifloat, ImageRaw *iraw, Rect *r) { + int x, y; + float f; + + for (y = 0; y < r->h; y++) for (x = 0; x < r->w; x++) { + f = *(iraw->data+0+3*(r->x + x + (r->y + y)*iraw->w)) + + *(iraw->data+1+3*(r->x + x + (r->y + y)*iraw->w)) + + *(iraw->data+2+3*(r->x + x + (r->y + y)*iraw->w)); + f = f/3.0; + *(ifloat->data+(x+y*ifloat->w)) += f; + } +}; + + + +ImageRaw *image_copyF(ImageFloat *ifloat) { + ImageRaw *iraw = image_alloc(ifloat->w, ifloat->h); + int x, y; + int min, max; + unsigned char c; + + // get limit + min = max = *(ifloat->data); + for (y = 0; y < ifloat->h; y++) for (x = 0; x < ifloat->w; x++) { + if (*(ifloat->data+(x+y*ifloat->w)) < min) + min = *(ifloat->data+(x+y*ifloat->w)); + if (*(ifloat->data+(x+y*ifloat->w)) > max) + max = *(ifloat->data+(x+y*ifloat->w)); + } + + for (y = 0; y < ifloat->h; y++) for (x = 0; x < ifloat->w; x++) { + c = (255.0 * (*(ifloat->data+(x+y*ifloat->w)) - min) / (max - min)); + *(iraw->data+0+3*(x + y * iraw->w)) = c; + *(iraw->data+1+3*(x + y * iraw->w)) = c; + *(iraw->data+2+3*(x + y * iraw->w)) = c; + } + + return iraw; +}; diff --git a/main.c b/main.c new file mode 100644 index 0000000..85ad334 --- /dev/null +++ b/main.c @@ -0,0 +1,130 @@ + +#include +#include +#include + +#include "vidoiltank.h" + +char conf_devname[LEN_FILENAME] = "/dev/video0"; +char conf_debugout[LEN_FILENAME] = "."; +int conf_width = 1920; +int conf_height = 1080; +int conf_iomode = IOM_MMAP; +int conf_samples = 25; +int conf_rotate = 0; +Rect conf_rect = { .x = 315, .y = 0, .w = 70, .h = 1910 }; + +/* + * open video device + * read multiple images into memory + * close video device + * + * calculate images together + * find position + * + * save all data + */ + +void help() { + printf ("\t-dev DEVICE video device file (default /dev/video0)\n"); + printf ("\t-s w,h define with and height\n"); + printf ("\t-debug OUTDIR debug files output dir\n"); + printf ("\t-ioread use read instead of mmap\n"); + printf ("\t-samples NUM samples to read\n"); + printf ("\t-rect X,Y,W,H part of the screen to search for the level\n"); + printf ("\t-rotate DEG rotate image by 0,90,180,270\n"); + printf ("\t-help display this help\n"); +} + +int main(int argc, char **argv) { + Video *video; + ImageRaw *img = NULL; + ImageRaw *tmpimg = NULL; + ImageFloat *imgf = NULL; + char fn[3*LEN_FILENAME]; + char outdate[LEN_FILENAME]; + time_t curtime = time(NULL); + struct tm *curtime_tm = NULL; + int i; + + // for debugging only + curtime_tm = localtime(&curtime); + strftime(outdate, LEN_FILENAME-1, "%Y%m%d-%H%M%S", curtime_tm); + + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "-dev") == 0) { + i++; + strncpy (conf_devname, argv[i], LEN_FILENAME-1); + } + else if (strcmp(argv[i], "-s") == 0){ + i++; + sscanf (argv[i], "%d,%d", &conf_width, &conf_height); + } + else if (strcmp(argv[i], "-rect") == 0){ + i++; + sscanf (argv[i], "%d,%d,%d,%d", &conf_rect.x, &conf_rect.y, &conf_rect.w, &conf_rect.h); + } + else if (strcmp(argv[i], "-samples") == 0){ + i++; + conf_samples = atoi (argv[i]); + } + else if (strcmp(argv[i], "-debug") == 0){ + i++; + strncpy (conf_debugout, argv[i], LEN_FILENAME-1); + } + else if (strcmp(argv[i], "-ioread") == 0){ + conf_iomode = IOM_READ; + } + else if (strcmp(argv[i], "-rotate") == 0){ + i++; + conf_rotate = atoi (argv[i]); + if (conf_rotate != 0 && conf_rotate != 90 && conf_rotate != 180 && conf_rotate != 270) { + printf ("rotate can only work for 0°, 90°, 180°, 270°. Rotation of %d° is not supported\n", conf_rotate); + return 1; + } + } + else if (strcmp(argv[i], "-help") == 0){ + help(); + return 0; + } + } + +// printf ("vidoiltank\n"); +// printf (" device: %s (%d x %d)\n", conf_devname, conf_width, conf_height); + + if ((video = vid_open("/dev/video0")) == NULL) return -1; + if (vid_start(video, conf_iomode, conf_width, conf_height) == -1) return -1; + + for (i = 0; i < conf_samples; i++) { + do {} while ((tmpimg = vid_grabimage(video, img)) == NULL && errno == EAGAIN); + if (tmpimg == NULL) { + printf ("could not grab image\n"); + exit(1); + } + img = tmpimg; + image_rotate(img, conf_rotate); + + if (i == 0) imgf = imageF_copy(img, &conf_rect); + else { + imageF_add (imgf, img, &conf_rect); + } + + if (i == 0) { + snprintf (fn, 3*LEN_FILENAME, "%s/%s-img-%02d.jpg", conf_debugout, outdate, i); + image_draw_rect(img, &conf_rect, 0x00FFFF); + image_save(img, fn, 98); + } + } + if (vid_stop(video) == -1) return -1; + vid_close(video); + video = NULL; + + tmpimg = image_copyF(imgf); + snprintf (fn, 3*LEN_FILENAME, "%s/%s-imgf-%03d.jpg", conf_debugout, outdate, conf_samples); + image_save(tmpimg, fn, 98); + + return 0; +} + + + diff --git a/vidoiltank.h b/vidoiltank.h new file mode 100644 index 0000000..e6edaee --- /dev/null +++ b/vidoiltank.h @@ -0,0 +1,120 @@ + +#ifndef _VIDOILTANK_H_ +#define _VIDOILTANK_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#define LEN_FILENAME 1024 + +enum { + IOM_READ, + IOM_MMAP +}; + +struct s_rect { + int x; + int y; + int w; + int h; +} typedef Rect; + +struct s_image_raw { + unsigned char *data; + int size; // allocated size + int h; + int w; +} typedef ImageRaw; + +struct s_image_float { + float *data; + int size; // allocated size + int h; + int w; +} typedef ImageFloat; + + +struct s_inbuffer { + unsigned char *data; + int size; +}; + +#define VDEV_INBUFFERS 3 +struct s_video { + char devname[LEN_FILENAME]; + int fd; + int height; // will be set on start + int width; + int iomode; + + struct s_inbuffer inbuffer[VDEV_INBUFFERS]; + int inbuffer_idx; + struct jpeg_decompress_struct jpgcinfo; + + char format[32]; + struct v4l2_capability vcap; + struct v4l2_cropcap cropcap; + struct v4l2_crop crop; + struct v4l2_format fmt; + +} typedef Video; + + +// +// jpeg error handling +// +struct jpg_error_mgr { + struct jpeg_error_mgr pub; /* "public" fields */ + + jmp_buf setjmp_buffer; /* for return to caller */ +}; +typedef struct jpg_error_mgr *jpg_error_ptr; +int convert (struct jpeg_decompress_struct *cdata, ImageRaw *dest, unsigned char *ptrsrc, int srcsize, uint32_t pixelformat, int srcw, int srch); +int convertstart(struct jpeg_decompress_struct *cdata, uint32_t pixelformat); +int convertstop(struct jpeg_decompress_struct *cdata, uint32_t pixelformat); + +/* image.c + * + */ +ImageRaw *image_alloc (int w, int h); +void image_resize(ImageRaw *i, int w, int h); +int image_save(ImageRaw *i, char *fn, int quality); +void image_rotate (ImageRaw *img, int deg); + +void image_draw_rect (ImageRaw *img, Rect *r, int col); + +ImageFloat *imageF_alloc (int w, int h); +ImageFloat *imageF_copy(ImageRaw *iraw, Rect *r); +ImageRaw *image_copyF(ImageFloat *ifloat); +void imageF_add(ImageFloat *ifloat, ImageRaw *iraw, Rect *r); + +/* getvideo.c + * + */ + + +Video *vid_open(char *device); +int vid_start(Video* video, int ionmode, int width, int height); +ImageRaw *vid_grabimage(Video* video, ImageRaw *img); +int vid_stop(Video* video); +void vid_close(Video* video); + + +#endif