source: flair-src/trunk/lib/FlairSensorActuator/src/V4LCamera.cpp@ 341

Last change on this file since 341 was 340, checked in by Sanahuja Guillaume, 5 years ago

add servos
File size: 36.4 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2014/07/17
6// filename: V4LCamera.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: base class for V4l camera
14//
15//
16/*********************************************************************/
17
18#include "V4LCamera.h"
19#include <GroupBox.h>
20#include <DoubleSpinBox.h>
21#include <CheckBox.h>
22#include <Label.h>
23#include <Image.h>
24#include <FrameworkManager.h>
25#include <fcntl.h>
26#include <linux/videodev2.h>
27#include <sys/ioctl.h>
28#include <unistd.h>
29#include <cstring>
30#include <sys/mman.h>
31#include <VisionFilter.h>
32
33#define DEFAULT_V4L_BUFFERS 4
34
35#define CHANNEL_NUMBER 1
36#define CLEAR(x) memset (&(x), 0, sizeof (x))
37#define DEFAULT_V4L_WIDTH 640
38#define DEFAULT_V4L_HEIGHT 480
39#define CV_CAP_PROP_POS_MSEC 0
40#define CV_CAP_PROP_POS_FRAMES 1
41#define CV_CAP_PROP_POS_AVI_RATIO 2
42#define CV_CAP_PROP_FRAME_WIDTH 3
43#define CV_CAP_PROP_FRAME_HEIGHT 4
44#define CV_CAP_PROP_FPS 5
45#define CV_CAP_PROP_FOURCC 6
46#define CV_CAP_PROP_FRAME_COUNT 7
47#define CV_CAP_PROP_FORMAT 8
48#define CV_CAP_PROP_MODE 9
49#define CV_CAP_PROP_BRIGHTNESS 10
50#define CV_CAP_PROP_CONTRAST 11
51#define CV_CAP_PROP_SATURATION 12
52#define CV_CAP_PROP_HUE 13
53#define CV_CAP_PROP_GAIN 14
54#define CV_CAP_PROP_CONVERT_RGB 15
55#define CV_CAP_PROP_SHARPNESS 16
56#define CV_CAP_PROP_EXPOSURE 17
57#define CV_CAP_PROP_AUTOGAIN 18
58#define CV_CAP_PROP_AWB 19
59
60#define HAVE_CAMV4L2
61
62using std::string;
63using namespace flair::core;
64using namespace flair::gui;
65
66namespace flair {
67namespace sensor {
68
69 V4LCamera::V4LCamera(string name,
70 uint8_t camera_index, uint16_t width, uint16_t height,
71 Image::Type::Format format, uint8_t priority)
72 : Thread(getFrameworkManager(), name, priority),
73 Camera(name, width, height, format) {
74
75 string deviceName="/dev/video"+std::to_string(camera_index);
76 device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
77 if (device == -1) {
78 Thread::Err("Cannot open %s\n",deviceName.c_str());
79 } else {
80 Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
81 }
82
83 /* w/o memset some parts arent initialized - AKA: Fill it with zeros so it is clean */
84 memset(&capture,0,sizeof(CvCaptureCAM_V4L));
85 /* Present the routines needed for V4L funtionality. They are inserted as part of
86 the standard set of cv calls promoting transparency. "Vector Table" insertion. */
87 capture.FirstCapture = 1;
88 capture.deviceHandle = device;
89
90 PALETTE_YUYV = 0;
91 PALETTE_UYVY= 0;
92
93 n_buffers = 0;
94 if (_capture_V4L2 (&capture) == -1) {
95 Thread::Err("_capture_V4L2 failed\n");
96 }
97
98if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FRAME_WIDTH, width)<0)
99 Thread::Err("cvSetCaptureProperty error\n");
100 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FRAME_HEIGHT, height)<0)
101 Thread::Err("cvSetCaptureProperty error\n");
102
103 if (format == Image::Type::Format::UYVY) {
104 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_UYVY)<0)
105 Thread::Err("cvSetCaptureProperty error\n");
106 } else if (format == Image::Type::Format::YUYV) {
107 if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_YUYV) <
108 0)
109 Thread::Err("cvSetCaptureProperty error\n");
110 } else {
111 Thread::Err("format not supported\n");
112 }
113 // ground station
114 gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
115 exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
116 bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
117 contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
118 hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
119 sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
120 sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
121 autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
122 autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
123 awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
124 fps = new Label(GetGroupBox()->NewRow(), "fps");
125
126 hasProblems=false;
127}
128
129V4LCamera::~V4LCamera() {
130 for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
131 //FreeFunction((char*)buffers[n_buffers].start);
132 }
133 SafeStop();
134 Join();
135}
136int V4LCamera::icvSetPropertyCAM_V4L( CvCaptureCAM_V4L* capture,
137 int property_id, double value ){
138 static int width = 0, height = 0;
139 int retval;
140
141 /* initialization */
142 retval = 0;
143
144 /* two subsequent calls setting WIDTH and HEIGHT will change
145 the video size */
146 /* the first one will return an error, though. */
147
148 switch (property_id) {
149 case CV_CAP_PROP_FRAME_WIDTH:
150 width = value;
151 if(width !=0 && height != 0) {
152 retval = icvSetVideoSize( capture, width, height);
153 width = height = 0;
154 }
155 break;
156 case CV_CAP_PROP_FRAME_HEIGHT:
157 height = value;
158 if(width !=0 && height != 0) {
159 retval = icvSetVideoSize( capture, width, height);
160 width = height = 0;
161 }
162 break;
163 case CV_CAP_PROP_FORMAT:
164
165 capture->form.fmt.pix.pixelformat = (unsigned long)value;
166 retval == xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form);
167 if ((unsigned long)value != capture->form.fmt.pix.pixelformat)
168 retval=-1;
169
170
171 PALETTE_YUYV = 0;
172 PALETTE_UYVY= 0;
173
174
175 if ((unsigned long)value== V4L2_PIX_FMT_YUYV)
176 {
177 PALETTE_YUYV = 1;
178 }
179 if ((unsigned long)value== V4L2_PIX_FMT_UYVY)
180 {
181 PALETTE_UYVY = 1;
182 }
183
184
185 break;
186 case CV_CAP_PROP_BRIGHTNESS:
187 case CV_CAP_PROP_CONTRAST:
188 case CV_CAP_PROP_SATURATION:
189 case CV_CAP_PROP_HUE:
190 case CV_CAP_PROP_GAIN:
191 case CV_CAP_PROP_SHARPNESS:
192 case CV_CAP_PROP_EXPOSURE:
193 case CV_CAP_PROP_AUTOGAIN:
194 case CV_CAP_PROP_AWB:
195 retval = icvSetControl(capture, property_id, value);
196 break;
197 default:
198 fprintf(stderr,
199 "HIGHGUI ERROR: V4L: setting property #%d is not supported\n",
200 property_id);
201 }
202
203 /* return the the status */
204 return retval;
205}
206int V4LCamera::icvSetControl (CvCaptureCAM_V4L* capture,
207 int property_id, double value) {
208
209 /* limitation of the input value */
210 if (value < 0.0) {
211 value = 0.0;
212 } else if (value > 1.0) {
213 value = 1.0;
214 }
215
216
217 /* default value for min and max */
218 int v4l2_min = 0;
219 int v4l2_max = 255;
220
221 /* initialisations */
222 CLEAR (capture->control);
223
224 /* set which control we want to set */
225 switch (property_id) {
226
227 case CV_CAP_PROP_BRIGHTNESS:
228 capture->control.id = V4L2_CID_BRIGHTNESS;
229 break;
230 case CV_CAP_PROP_CONTRAST:
231 capture->control.id = V4L2_CID_CONTRAST;
232 break;
233 case CV_CAP_PROP_SATURATION:
234 capture->control.id = V4L2_CID_SATURATION;
235 break;
236 case CV_CAP_PROP_HUE:
237 capture->control.id = V4L2_CID_HUE;
238 break;
239 case CV_CAP_PROP_GAIN:
240 capture->control.id = V4L2_CID_GAIN;
241 break;
242 case CV_CAP_PROP_SHARPNESS:
243 capture->control.id = V4L2_CID_SHARPNESS;
244 break;
245 case CV_CAP_PROP_EXPOSURE:
246 capture->control.id = V4L2_CID_EXPOSURE;
247 break;
248 case CV_CAP_PROP_AUTOGAIN:
249 capture->control.id = V4L2_CID_AUTOGAIN;
250 break;
251 case CV_CAP_PROP_AWB:
252 capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE;
253 break;
254 default:
255 fprintf(stderr,
256 "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
257 property_id);
258 return -1;
259 }
260
261 /* get the min and max values */
262 if (-1 == xioctl (capture->deviceHandle,
263 VIDIOC_G_CTRL, &capture->control)) {
264// perror ("VIDIOC_G_CTRL for getting min/max values");
265 return -1;
266 }
267
268 /* get the min/max values */
269 switch (property_id) {
270
271 case CV_CAP_PROP_BRIGHTNESS:
272 v4l2_min = capture->v4l2_brightness_min;
273 v4l2_max = capture->v4l2_brightness_max;
274 break;
275 case CV_CAP_PROP_CONTRAST:
276 v4l2_min = capture->v4l2_contrast_min;
277 v4l2_max = capture->v4l2_contrast_max;
278 break;
279 case CV_CAP_PROP_SATURATION:
280 v4l2_min = capture->v4l2_saturation_min;
281 v4l2_max = capture->v4l2_saturation_max;
282 break;
283 case CV_CAP_PROP_HUE:
284 v4l2_min = capture->v4l2_hue_min;
285 v4l2_max = capture->v4l2_hue_max;
286 break;
287 case CV_CAP_PROP_GAIN:
288 v4l2_min = capture->v4l2_gain_min;
289 v4l2_max = capture->v4l2_gain_max;
290 break;
291 case CV_CAP_PROP_SHARPNESS:
292 v4l2_min = capture->v4l2_sharpness_min;
293 v4l2_max = capture->v4l2_sharpness_max;
294 break;
295 case CV_CAP_PROP_EXPOSURE:
296 v4l2_min = capture->v4l2_exposure_min;
297 v4l2_max = capture->v4l2_exposure_max;
298 break;
299 case CV_CAP_PROP_AUTOGAIN:
300 v4l2_min = capture->v4l2_autogain_min;
301 v4l2_max = capture->v4l2_autogain_max;
302 break;
303 case CV_CAP_PROP_AWB:
304 v4l2_min = capture->v4l2_awb_min;
305 v4l2_max = capture->v4l2_awb_max;
306 break;
307 }
308
309 /* initialisations */
310 CLEAR (capture->control);
311
312 /* set which control we want to set */
313 switch (property_id) {
314
315 case CV_CAP_PROP_BRIGHTNESS:
316 capture->control.id = V4L2_CID_BRIGHTNESS;
317 break;
318 case CV_CAP_PROP_CONTRAST:
319 capture->control.id = V4L2_CID_CONTRAST;
320 break;
321 case CV_CAP_PROP_SATURATION:
322 capture->control.id = V4L2_CID_SATURATION;
323 break;
324 case CV_CAP_PROP_HUE:
325 capture->control.id = V4L2_CID_HUE;
326 break;
327 case CV_CAP_PROP_GAIN:
328 capture->control.id = V4L2_CID_GAIN;
329 break;
330 case CV_CAP_PROP_SHARPNESS:
331 capture->control.id = V4L2_CID_SHARPNESS;
332 break;
333 case CV_CAP_PROP_EXPOSURE:
334 capture->control.id = V4L2_CID_EXPOSURE;
335 break;
336 case CV_CAP_PROP_AUTOGAIN:
337 capture->control.id = V4L2_CID_AUTOGAIN;
338 break;
339 case CV_CAP_PROP_AWB:
340 capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE;
341 break;
342 default:
343 fprintf(stderr,
344 "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
345 property_id);
346 return -1;
347 }
348
349 /* set the value we want to set to the scaled the value */
350 capture->control.value = (int)(value * (v4l2_max - v4l2_min) + v4l2_min);
351
352 /* The driver may clamp the value or return ERANGE, ignored here */
353 if (-1 == xioctl (capture->deviceHandle,
354 VIDIOC_S_CTRL, &capture->control) && errno != ERANGE) {
355 perror ("VIDIOC_S_CTRL");
356 return -1;
357 }
358
359 /* all was OK */
360 return 0;
361
362}
363int V4LCamera::_capture_V4L2 (CvCaptureCAM_V4L *capture)
364{
365 int detect_v4l2 = 0;
366
367 detect_v4l2 = try_init_v4l2(capture);
368
369 if (detect_v4l2 != 1) {
370 /* init of the v4l2 device is not OK */
371 return -1;
372 }
373
374 /* Init V4L2 control variables */
375 capture->v4l2_brightness = 0;
376 capture->v4l2_contrast = 0;
377 capture->v4l2_saturation = 0;
378 capture->v4l2_hue = 0;
379 capture->v4l2_gain = 0;
380 capture->v4l2_sharpness = 0;
381 capture->v4l2_exposure = 0;
382 capture->v4l2_autogain = 0;
383 capture->v4l2_awb = 0;
384
385 capture->v4l2_brightness_min = 0;
386 capture->v4l2_contrast_min = 0;
387 capture->v4l2_saturation_min = 0;
388 capture->v4l2_hue_min = 0;
389 capture->v4l2_gain_min = 0;
390 capture->v4l2_sharpness_min = 0;
391 capture->v4l2_exposure_min = 0;
392 capture->v4l2_autogain_min = 0;
393 capture->v4l2_awb_min = 0;
394
395 capture->v4l2_brightness_max = 0;
396 capture->v4l2_contrast_max = 0;
397 capture->v4l2_saturation_max = 0;
398 capture->v4l2_hue_max = 0;
399 capture->v4l2_gain_max = 0;
400 capture->v4l2_sharpness_max = 0;
401 capture->v4l2_exposure_max = 0;
402 capture->v4l2_autogain_max = 0;
403 capture->v4l2_awb_max = 0;
404
405
406 if ((capture->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
407 /* Nope. */
408 fprintf( stderr, "HIGHGUI ERROR: V4L2: device is unable to capture video memory.\n");
409
410 return -1;
411 }
412
413
414
415 /* Find Window info */
416 CLEAR (capture->form);
417 capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
418
419 if (-1 == xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form)) {
420 fprintf( stderr, "HIGHGUI ERROR: V4L2: Could not obtain specifics of capture window.\n\n");
421
422 return -1;
423 }
424
425
426
427 if (autosetup_capture_mode_v4l2(capture) == -1)
428 return -1;
429
430 icvSetVideoSize(capture, DEFAULT_V4L_WIDTH, DEFAULT_V4L_HEIGHT);
431
432 unsigned int min;
433
434 /* Buggy driver paranoia. */
435 min = capture->form.fmt.pix.width * 2;
436
437 if (capture->form.fmt.pix.bytesperline < min)
438 capture->form.fmt.pix.bytesperline = min;
439
440 min = capture->form.fmt.pix.bytesperline * capture->form.fmt.pix.height;
441
442 if (capture->form.fmt.pix.sizeimage < min)
443 capture->form.fmt.pix.sizeimage = min;
444
445 return 1;
446}
447int V4LCamera::icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h) {
448
449
450
451 CLEAR (capture->crop);
452 capture->crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
453 capture->crop.c.left = 0;
454 capture->crop.c.top = 0;
455 capture->crop.c.height = h*24;
456 capture->crop.c.width = w*24;
457
458 /* set the crop area, but don't exit if the device don't support croping */
459 xioctl (capture->deviceHandle, VIDIOC_S_CROP, &capture->crop);
460
461 CLEAR (capture->form);
462 capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
463
464 /* read the current setting, mainly to retreive the pixelformat information */
465 xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form);
466
467 /* set the values we want to change */
468 capture->form.fmt.pix.width = w;
469 capture->form.fmt.pix.height = h;
470 capture->form.fmt.win.chromakey = 0;
471 capture->form.fmt.win.field = V4L2_FIELD_ANY;
472 capture->form.fmt.win.clips = 0;
473 capture->form.fmt.win.clipcount = 0;
474 capture->form.fmt.pix.field = V4L2_FIELD_ANY;
475
476 /* ask the device to change the size
477 * don't test if the set of the size is ok, because some device
478 * don't allow changing the size, and we will get the real size
479 * later */
480 xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form);
481
482 /* try to set framerate to 30 fps */
483 struct v4l2_streamparm setfps;
484 memset (&setfps, 0, sizeof(struct v4l2_streamparm));
485 setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
486 setfps.parm.capture.timeperframe.numerator = 1;
487 setfps.parm.capture.timeperframe.denominator = 30;
488 xioctl (capture->deviceHandle, VIDIOC_S_PARM, &setfps);
489
490 /* we need to re-initialize some things, like buffers, because the size has
491 * changed */
492 capture->FirstCapture = 1;
493
494 /* Get window info again, to get the real value */
495 if (-1 == xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form))
496 {
497 fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: Could not obtain specifics of capture window.\n\n");
498
499
500
501 return 0;
502 }
503
504 return 0;
505
506
507
508
509 return 0;
510
511}
512int V4LCamera::try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace)
513{
514 CLEAR (capture->form);
515
516 capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
517 capture->form.fmt.pix.pixelformat = colorspace;
518 capture->form.fmt.pix.field = V4L2_FIELD_ANY;
519 capture->form.fmt.pix.width = DEFAULT_V4L_WIDTH;
520 capture->form.fmt.pix.height = DEFAULT_V4L_HEIGHT;
521
522 if (-1 == xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form))
523 return -1;
524
525
526 if (colorspace != capture->form.fmt.pix.pixelformat)
527 return -1;
528 else
529 return 0;
530}
531int V4LCamera::autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture)
532{
533 if (try_palette_v4l2(capture, V4L2_PIX_FMT_YUYV) == 0) {
534 PALETTE_YUYV = 1;
535 } else if (try_palette_v4l2(capture, V4L2_PIX_FMT_UYVY) == 0) {
536 PALETTE_UYVY = 1;
537 } else {
538 fprintf(stderr, "HIGHGUI ERROR: V4L2: Pixel format of incoming image is unsupported by OpenCV\n");
539
540 return -1;
541 }
542
543 return 0;
544
545}
546
547
548
549int V4LCamera::try_init_v4l2(CvCaptureCAM_V4L* capture)
550{
551
552 // if detect = -1 then unable to open device
553 // if detect = 0 then detected nothing
554 // if detect = 1 then V4L2 device
555 int detect = 0;
556
557
558 if (detect == 0)
559 {
560 CLEAR (capture->cap);
561 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QUERYCAP, &capture->cap))
562 {
563 detect = 0;
564
565
566 }
567 else
568 {
569 CLEAR (capture->capability);
570 capture->capability.type = capture->cap.capabilities;
571
572 /* Query channels number */
573 if (-1 != xioctl (capture->deviceHandle, VIDIOC_G_INPUT, &capture->capability.channels))
574 {
575 detect = 1;
576 }
577 }
578 }
579
580 return detect;
581
582}
583/*
584V4LCamera::V4LCamera(string name,
585 uint8_t camera_index, uint16_t width, uint16_t height,
586 Image::Type::Format format, uint8_t priority)
587 : Thread(getFrameworkManager(), name, priority),
588 Camera(name, width, height, format) {
589
590 string deviceName="/dev/video"+std::to_string(camera_index);
591 device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
592 if (device == -1) {
593 Thread::Err("Cannot open %s\n",deviceName.c_str());
594 } else {
595 Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
596 }
597
598 struct v4l2_capability cap;
599 memset(&cap, 0, sizeof (v4l2_capability));
600 if (xioctl (device, VIDIOC_QUERYCAP, &cap)==-1) {
601 Thread::Err("VIDIOC_QUERYCAP xioctl\n");
602 }
603 if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
604 Thread::Err("device is unable to capture video memory.\n");
605 }
606
607 //get v4l2_format
608 struct v4l2_format form;
609 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
610 if(xioctl (device, VIDIOC_G_FMT,&form)==-1) {
611 Thread::Err("VIDIOC_G_FMT xioctl\n");
612 }
613
614 //set width, height and format
615 if (format == Image::Type::Format::UYVY) {
616 form.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
617 } else if (format == Image::Type::Format::YUYV) {
618 form.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
619 } else {
620 Thread::Err("format not supported\n");
621 }
622
623 form.fmt.pix.width = width;
624 form.fmt.pix.height = height;
625 form.fmt.win.chromakey = 0;
626 form.fmt.win.field = V4L2_FIELD_ANY;
627 form.fmt.win.clips = 0;
628 form.fmt.win.clipcount = 0;
629 form.fmt.pix.field = V4L2_FIELD_ANY;
630 if(xioctl (device, VIDIOC_S_FMT,&form)==-1) {
631 Thread::Err("VIDIOC_S_FMT xioctl\n");
632 }
633
634 //alloc and queue bufs
635 AllocBuffers();
636 for (int bufferIndex = 0; bufferIndex < nbBuffers;++bufferIndex) {
637 QueueBuffer(bufferIndex);
638 }
639
640 // enable the streaming
641 v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
642 if (xioctl (device, VIDIOC_STREAMON,&type)==-1) {
643 Thread::Err("VIDIOC_STREAMON xioctl\n");
644 }
645
646 // skip first frame. it is often bad -- this is unnotied in traditional apps,
647 // but could be fatal if bad jpeg is enabled
648 bufferIndex=-1;
649 GrabFrame();
650
651 // ground station
652 gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
653 exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
654 bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
655 contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
656 hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
657 sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
658 sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
659 autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
660 autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
661 awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
662 fps = new Label(GetGroupBox()->NewRow(), "fps");
663
664 hasProblems=false;
665}
666
667V4LCamera::~V4LCamera() {
668 for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
669 FreeFunction((char*)buffers[n_buffers].start);
670 }
671 SafeStop();
672 Join();
673}
674
675void V4LCamera::Run(void) {
676 Time cam_time, new_time, fpsNow, fpsPrev;
677 int fpsCounter = 0;
678
679 // init image old
680 GrabFrame();
681 cam_time = GetTime();
682 fpsPrev = cam_time;
683
684 while (!ToBeStopped()) {
685 //check for ps3eye deconnection in hds uav
686 if(hasProblems==false) {
687 struct v4l2_format form;
688 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
689 xioctl(device, VIDIOC_G_FMT,&form);
690 if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
691 Thread::Warn("camera disconnected\n");
692 hasProblems=true;
693 }
694 }
695
696 // fps counter
697 fpsCounter++;
698 if (GetTime() > (fpsPrev + 5 * (Time)1000000000)) {
699 // every 5 secondes
700 fpsNow = GetTime();
701 fps->SetText("fps: %.1f",
702 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
703 fpsCounter = 0;
704 fpsPrev = fpsNow;
705 }
706
707 // cam properties
708 if (gain->ValueChanged() == true && autogain->Value() == false)
709 SetGain(gain->Value());
710 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
711 SetExposure(exposure->Value());
712 if (bright->ValueChanged() == true)
713 SetBrightness(bright->Value());
714 if (sat->ValueChanged() == true)
715 SetSaturation(sat->Value());
716 if (contrast->ValueChanged() == true)
717 SetContrast(contrast->Value());
718 if (hue->ValueChanged() == true)
719 SetHue(hue->Value());
720 if (sharpness->ValueChanged() == true)
721 SetProperty(V4L2_CID_SHARPNESS, sharpness->Value());
722 if (autogain->ValueChanged() == true) {
723 if (autogain->Value() == true) {
724 gain->setEnabled(false);
725 } else {
726 gain->setEnabled(true);
727 SetGain(gain->Value());
728 }
729 SetAutoGain(autogain->Value());
730 }
731 if (autoexposure->ValueChanged() == true) {
732 if (autoexposure->Value() == true) {
733 exposure->setEnabled(false);
734 } else {
735 exposure->setEnabled(true);
736 SetExposure(exposure->Value());
737 }
738 SetAutoExposure(autoexposure->Value());
739 }
740 if (awb->ValueChanged() == true)
741 SetProperty(V4L2_CID_AUTO_WHITE_BALANCE, awb->Value());
742
743 // get picture
744 GrabFrame();
745 new_time = GetTime();
746
747 //check for ps3eye deconnection in hds uav
748 if(new_time-cam_time>100*1000*1000) {
749 Thread::Warn("delta trop grand\n");
750 hasProblems=true;
751 }
752
753 output->GetMutex();
754 output->buffer=(char*)buffers[bufferIndex].start;
755 output->ReleaseMutex();
756
757 output->SetDataTime(cam_time);
758 ProcessUpdate(output);
759
760 cam_time = new_time;
761 }
762
763 close(device);
764}
765*/
766int V4LCamera::v4l2_alloc_buffers (CvCaptureCAM_V4L *capture, char *deviceName)
767{
768 CLEAR (capture->req);
769
770 unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
771
772 try_again:
773
774 capture->req.count = buffer_number;
775 capture->req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
776 capture->req.memory = V4L2_MEMORY_MMAP;
777
778 if (-1 == xioctl (capture->deviceHandle, VIDIOC_REQBUFS, &capture->req))
779 {
780 if (EINVAL == errno)
781 {
782 fprintf (stderr, "%s does not support memory mapping\n", deviceName);
783 } else {
784 perror ("VIDIOC_REQBUFS");
785 }
786 /* free capture, and returns an error code */
787
788 return -1;
789 }
790
791 if (capture->req.count < buffer_number)
792 {
793 if (buffer_number == 1)
794 {
795 fprintf (stderr, "Insufficient buffer memory on %s\n", deviceName);
796
797 /* free capture, and returns an error code */
798
799 return -1;
800 } else {
801 buffer_number--;
802 fprintf (stderr, "Insufficient buffer memory on %s -- decreaseing buffers\n", deviceName);
803
804 goto try_again;
805 }
806 }
807
808 for (n_buffers = 0; n_buffers < capture->req.count; ++n_buffers)
809 {
810 struct v4l2_buffer buf;
811
812 CLEAR (buf);
813
814 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
815 buf.memory = V4L2_MEMORY_MMAP;
816 buf.index = n_buffers;
817
818 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QUERYBUF, &buf)) {
819 perror ("VIDIOC_QUERYBUF");
820
821 /* free capture, and returns an error code */
822
823 return -1;
824 }
825
826 capture->buffers[n_buffers].length = buf.length;
827 capture->buffers[n_buffers].start =
828 mmap (NULL /* start anywhere */,
829 buf.length,
830 PROT_READ | PROT_WRITE /* required */,
831 MAP_SHARED /* recommended */,
832 capture->deviceHandle, buf.m.offset);
833
834 if (MAP_FAILED == capture->buffers[n_buffers].start) {
835 perror ("mmap");
836
837 /* free capture, and returns an error code */
838
839 return -1;
840 }
841
842
843 }
844
845 /* Set up Image data */
846 /*
847 cvInitImageHeader( &capture->frame,
848 cvSize( capture->captureWindow.width,
849 capture->captureWindow.height ),
850 IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );*/
851 /* Allocate space for RGBA data */
852 capture->imageSize=capture->form.fmt.pix.width*capture->form.fmt.pix.height*2;
853 capture->imageData = AllocFunction(capture->imageSize);
854 Printf("cmem allocated %i at %x\n",capture->imageSize,capture->imageData);
855
856 return 1;
857};
858int V4LCamera::cvGrabFrame(CvCaptureCAM_V4L* capture) {
859
860 if (capture->FirstCapture) {
861 /* Some general initialization must take place the first time through */
862
863 /* This is just a technicality, but all buffers must be filled up before any
864 staggered SYNC is applied. SO, filler up. (see V4L HowTo) */
865
866
867 v4l2_alloc_buffers (capture, NULL);
868
869 for (capture->bufferIndex = 0;
870 capture->bufferIndex < ((int)capture->req.count);
871 ++capture->bufferIndex)
872 {
873
874 struct v4l2_buffer buf;
875
876 CLEAR (buf);
877
878 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
879 buf.memory = V4L2_MEMORY_MMAP;
880 buf.index = (unsigned long)capture->bufferIndex;
881
882 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QBUF, &buf)) {
883 perror ("VIDIOC_QBUF");
884 return 0;
885 }
886 }
887
888 /* enable the streaming */
889 capture->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
890 if (-1 == xioctl (capture->deviceHandle, VIDIOC_STREAMON,
891 &capture->type)) {
892 /* error enabling the stream */
893 perror ("VIDIOC_STREAMON");
894 return 0;
895 }
896
897
898
899
900 /* preparation is ok */
901 capture->FirstCapture = 0;
902 }
903
904
905
906 mainloop_v4l2(capture);
907
908
909
910 return(1);
911}
912void V4LCamera::mainloop_v4l2(CvCaptureCAM_V4L* capture) {
913 unsigned int count;
914
915 count = 1;
916
917 while (count-- > 0) {
918 for (;;) {
919 fd_set fds;
920 struct timeval tv;
921 int r;
922
923 FD_ZERO (&fds);
924 FD_SET (capture->deviceHandle, &fds);
925
926 /* Timeout. */
927 tv.tv_sec = 2;
928 tv.tv_usec = 0;
929
930 r = select (capture->deviceHandle+1, &fds, NULL, NULL, &tv);
931
932 if (-1 == r) {
933 if (EINTR == errno)
934 continue;
935
936 perror ("select");
937 }
938
939 if (0 == r) {
940 fprintf (stderr, "select timeout\n");
941
942 /* end the infinite loop */
943 break;
944 }
945
946 if (read_frame_v4l2 (capture))
947 break;
948 }
949 }
950}
951int V4LCamera::read_frame_v4l2(CvCaptureCAM_V4L* capture) {
952 struct v4l2_buffer buf;
953
954 CLEAR (buf);
955
956 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
957 buf.memory = V4L2_MEMORY_MMAP;
958
959 if (-1 == xioctl (capture->deviceHandle, VIDIOC_DQBUF, &buf)) {
960 switch (errno) {
961 case EAGAIN:
962 return 0;
963
964 case EIO:
965 /* Could ignore EIO, see spec. */
966
967 /* fall through */
968
969 default:
970 /* display the error and stop processing */
971 perror ("VIDIOC_DQBUF");
972 return 1;
973 }
974 }
975
976 if(buf.index >= capture->req.count) {
977 Thread::Err("buf.index >= capture->req.count\n");
978 }
979
980
981 capture->bufferIndex = buf.index;
982
983
984 if (-1 == xioctl (capture->deviceHandle, VIDIOC_QBUF, &buf))
985 perror ("VIDIOC_QBUF");
986
987 return 1;
988}
989void V4LCamera::Run(void) {
990 Time cam_time, new_time, fpsNow, fpsPrev;
991 //IplImage *img; // raw image
992 int fpsCounter = 0;
993
994 // init image old
995 if (!cvGrabFrame(&capture)) {
996 Printf("Could not grab a frame\n");
997 }
998 cam_time = GetTime();
999 fpsPrev = cam_time;
1000
1001 while (!ToBeStopped()) {
1002
1003
1004 // fps counter
1005 fpsCounter++;
1006 if (fpsCounter == 100) {
1007 fpsNow = GetTime();
1008 fps->SetText("fps: %.1f",
1009 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
1010 fpsCounter = 0;
1011 fpsPrev = fpsNow;
1012 }
1013
1014 // cam properties
1015 if (gain->ValueChanged() == true && autogain->Value() == false)
1016 SetGain(gain->Value());
1017 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
1018 SetExposure(exposure->Value());
1019 if (bright->ValueChanged() == true)
1020 SetBrightness(bright->Value());
1021 if (sat->ValueChanged() == true)
1022 SetSaturation(sat->Value());
1023 if (contrast->ValueChanged() == true)
1024 SetContrast(contrast->Value());
1025 if (hue->ValueChanged() == true)
1026 SetHue(hue->Value());
1027 //if (sharpness->ValueChanged() == true)
1028 // cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value());
1029 if (autogain->ValueChanged() == true) {
1030 if (autogain->Value() == true) {
1031 gain->setEnabled(false);
1032 } else {
1033 gain->setEnabled(true);
1034 SetGain(gain->Value());
1035 }
1036 SetAutoGain(autogain->Value());
1037 }
1038 if (autoexposure->ValueChanged() == true) {
1039 if (autoexposure->Value() == true) {
1040 exposure->setEnabled(false);
1041 } else {
1042 exposure->setEnabled(true);
1043 SetExposure(exposure->Value());
1044 }
1045 SetAutoExposure(autoexposure->Value());
1046 }
1047 //if (awb->ValueChanged() == true)
1048 // cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value());
1049
1050 // cam pictures
1051 cvRetrieveRawFrame(&capture);
1052 if (!cvGrabFrame(&capture)) {
1053 Printf("Could not grab a frame\n");
1054 }
1055 new_time = GetTime();
1056
1057 //check for ps3eye deconnection in hds uav
1058 if(new_time-cam_time>100*1000*1000) {
1059 Thread::Warn("delta trop grand\n");
1060 hasProblems=true;
1061 }
1062
1063 output->GetMutex();
1064
1065 output->buffer=capture.imageData;
1066 output->ReleaseMutex();
1067
1068 output->SetDataTime(cam_time);
1069 ProcessUpdate(output);
1070 cam_time = new_time;
1071 }
1072
1073 //cvReleaseCapture(&capture);
1074}
1075
1076void V4LCamera::cvRetrieveRawFrame( CvCaptureCAM_V4L* capture) {
1077
1078
1079
1080
1081 /* Now get what has already been captured as a IplImage return */
1082
1083 /* First, reallocate imageData if the frame size changed */
1084
1085
1086/*
1087 if(((unsigned long)capture->frame.width != capture->form.fmt.pix.width)
1088 || ((unsigned long)capture->frame.height != capture->form.fmt.pix.height)) {
1089 if (PALETTE_YUYV == 1 || PALETTE_UYVY == 1)
1090 {
1091 cvFree(&capture->frame.imageData);
1092 cvInitImageHeader( &capture->frame,
1093 cvSize( capture->form.fmt.pix.width,
1094 capture->form.fmt.pix.height ),
1095 IPL_DEPTH_8U,2, IPL_ORIGIN_TL, 4 );
1096 capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
1097 }else
1098 {
1099 fprintf( stderr,
1100 "HIGHGUI ERROR: V4L: raw output not supported for this palette\n");
1101 }
1102
1103 }
1104
1105 */
1106
1107
1108 if (PALETTE_YUYV == 1){
1109 memcpy((char *)capture->imageData,
1110 (char *)capture->buffers[capture->bufferIndex].start,
1111 capture->imageSize);
1112 }
1113 if (PALETTE_UYVY == 1){
1114 memcpy((char *)capture->imageData,
1115 (char *)capture->buffers[capture->bufferIndex].start,
1116 capture->imageSize);
1117 }
1118
1119
1120
1121
1122}
1123int V4LCamera::QueueBuffer(int index) {
1124 struct v4l2_buffer buf;
1125 if(index>=0 && index<nbBuffers) {
1126 memset(&buf, 0, sizeof (v4l2_buffer));
1127 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1128 buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
1129 buf.index = (unsigned long)index;
1130 buf.m.userptr=(unsigned long)(buffers[index].start);
1131 buf.length=buffers[index].length;
1132
1133 int ret=xioctl (device, VIDIOC_QBUF, &buf);
1134 if (ret==-1) {
1135 Thread::Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
1136 return -1;
1137 }
1138 }
1139 return 0;
1140}
1141
1142int V4LCamera::GrabFrame(void) {
1143 //queue previous buffer
1144 if(QueueBuffer(bufferIndex)<0) return -1;
1145
1146 fd_set fds;
1147 struct timeval tv;
1148 FD_ZERO (&fds);
1149 FD_SET (device, &fds);
1150
1151 tv.tv_sec = 0;
1152 tv.tv_usec = 100000;
1153
1154 int r = select (device+1, &fds, NULL, NULL, &tv);
1155
1156 if (-1 == r) {
1157 char errorMsg[256];
1158 Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
1159 return -1;
1160 }
1161
1162 if (0 == r) {
1163 Thread::Err("select timeout\n");
1164 return -1;
1165 }
1166
1167 struct v4l2_buffer buf;
1168 memset(&buf, 0, sizeof (v4l2_buffer));
1169 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1170 buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
1171
1172 //get last captured image
1173 int prevDQbuf=-1;
1174 for(int i=0;i<4;i++) {
1175 if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
1176 if (errno==EAGAIN) {
1177 break;
1178 } else {
1179 Thread::Err("VIDIOC_DQBUF xioctl\n");
1180 return -1;
1181 }
1182 } else {
1183 if(prevDQbuf!=-1) {
1184 QueueBuffer(prevDQbuf);
1185 }
1186 for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
1187 if((void*)(buf.m.userptr)==buffers[n_buffers].start) {
1188 prevDQbuf=n_buffers;
1189 bufferIndex=n_buffers;
1190 break;
1191 }
1192 }
1193 }
1194 }
1195
1196 return 1;
1197}
1198
1199int V4LCamera::AllocBuffers(void) {
1200 struct v4l2_requestbuffers requestbuffers;
1201 memset(&requestbuffers, 0, sizeof (v4l2_requestbuffers));
1202
1203 unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
1204
1205 try_again:
1206
1207 requestbuffers.count = buffer_number;
1208 requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1209 requestbuffers.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
1210
1211 if (xioctl (device, VIDIOC_REQBUFS, &requestbuffers)==-1) {
1212 if (errno==EINVAL) {
1213 Thread::Err("VIDIOC_REQBUFS user memory not supported\n");
1214 } else {
1215 Thread::Err ("VIDIOC_REQBUFS xioctl\n");
1216 }
1217 return -1;
1218 }
1219
1220 nbBuffers=DEFAULT_V4L_BUFFERS;
1221 for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
1222 buffers[n_buffers].length = output->GetDataType().GetSize();
1223 buffers[n_buffers].start =AllocFunction(output->GetDataType().GetSize());
1224 }
1225
1226 return 1;
1227};
1228
1229bool V4LCamera::HasProblems(void) {
1230 return hasProblems;
1231}
1232
1233void V4LCamera::SetAutoGain(bool value) {
1234 SetProperty(V4L2_CID_AUTOGAIN, value);
1235}
1236
1237void V4LCamera::SetAutoExposure(bool value) {
1238 Thread::Warn("not implemented\n");
1239}
1240
1241void V4LCamera::SetGain(float value) {
1242 SetProperty(V4L2_CID_GAIN, value);
1243}
1244
1245void V4LCamera::SetExposure(float value) {
1246 SetProperty(V4L2_CID_EXPOSURE, value);
1247}
1248
1249void V4LCamera::SetBrightness(float value) {
1250 SetProperty(V4L2_CID_BRIGHTNESS, value);
1251}
1252
1253void V4LCamera::SetSaturation(float value) {
1254 SetProperty(V4L2_CID_SATURATION, value);
1255}
1256
1257void V4LCamera::SetHue(float value) {
1258 SetProperty(V4L2_CID_HUE, value);
1259}
1260
1261void V4LCamera::SetContrast(float value) {
1262 SetProperty(V4L2_CID_CONTRAST, value);
1263}
1264
1265float V4LCamera::GetProperty(int property) {
1266 //get min and max value
1267 struct v4l2_queryctrl queryctrl;
1268 queryctrl.id = property;
1269 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) return -1;
1270 int min = queryctrl.minimum;
1271 int max = queryctrl.maximum;
1272
1273 //set value
1274 struct v4l2_control control;
1275 memset (&control, 0, sizeof (v4l2_control));
1276 control.id = property;
1277 if(xioctl (device,VIDIOC_G_CTRL, &control)==-1) return -1;
1278
1279 return ((float)control.value - min + 1) / (max - min);
1280}
1281
1282void V4LCamera::SetProperty(int property,float value) {
1283 //get min and max value
1284 struct v4l2_queryctrl queryctrl;
1285 queryctrl.id = property;
1286 xioctl (device, VIDIOC_QUERYCTRL,&queryctrl);
1287 int min = queryctrl.minimum;
1288 int max = queryctrl.maximum;
1289
1290 //set value
1291 struct v4l2_control control;
1292 memset (&control, 0, sizeof (v4l2_control));
1293 control.id = property;
1294 control.value = (int)(value * (max - min) + min);
1295 xioctl (device,VIDIOC_S_CTRL, &control);
1296}
1297
1298int V4LCamera::xioctl( int fd, int request, void *arg) {
1299 int r;
1300
1301 do r = ioctl (fd, request, arg);
1302 while (-1 == r && EINTR == errno);
1303
1304 return r;
1305}
1306
1307} // end namespace sensor
1308} // end namespace flair
Note: See TracBrowser for help on using the repository browser.