close Warning: Can't use blame annotator: 
svn blame failed on trunk/lib/FlairSensorActuator/src/V4LCamera.cpp: 200029 - Couldn't perform atomic initialization

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

Last change on this file since 346 was 346, checked in by Sanahuja Guillaume, 4 years ago

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