source: flair-src/branches/sanscv/lib/FlairSensorActuator/src/V4LCamera.cpp@ 324

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

removing opencv dependency
File size: 12.7 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
32#define DEFAULT_V4L_BUFFERS 4
33
34using std::string;
35using namespace flair::core;
36using namespace flair::gui;
37
38namespace flair {
39namespace sensor {
40
41V4LCamera::V4LCamera(string name,
42 uint8_t camera_index, uint16_t width, uint16_t height,
43 Image::Type::Format format, uint8_t priority)
44 : Thread(getFrameworkManager(), name, priority),
45 Camera(name, width, height, format) {
46
47 string deviceName="/dev/video"+std::to_string(camera_index);
48 device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
49 if (device == -1) {
50 Thread::Err("Cannot open %s\n",deviceName.c_str());
51 } else {
52 Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
53 }
54
55 struct v4l2_capability cap;
56 memset(&cap, 0, sizeof (v4l2_capability));
57 if (xioctl (device, VIDIOC_QUERYCAP, &cap)==-1) {
58 Thread::Err("VIDIOC_QUERYCAP xioctl\n");
59 }
60 if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
61 Thread::Err("device is unable to capture video memory.\n");
62 }
63 /*
64 * unnecessary?
65 struct video_capability capability;
66 memset(&capability, 0, sizeof (video_capability));
67 capability.type = cap.capabilities;
68
69 // Query channels number
70 if (xioctl(device, VIDIOC_G_INPUT, &capability.channels)==-1) {
71 Thread::Err("VIDIOC_G_INPUT xioctl\n");
72 }
73 */
74 //get v4l2_format
75 struct v4l2_format form;
76 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
77 if(xioctl (device, VIDIOC_G_FMT,&form)==-1) {
78 Thread::Err("VIDIOC_G_FMT xioctl\n");
79 }
80
81 //set width, height and format
82 if (format == Image::Type::Format::UYVY) {
83 form.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
84 } else if (format == Image::Type::Format::YUYV) {
85 form.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
86 } else {
87 Thread::Err("format not supported\n");
88 }
89
90 form.fmt.pix.width = width;
91 form.fmt.pix.height = height;
92 form.fmt.win.chromakey = 0;
93 form.fmt.win.field = V4L2_FIELD_ANY;
94 form.fmt.win.clips = 0;
95 form.fmt.win.clipcount = 0;
96 form.fmt.pix.field = V4L2_FIELD_ANY;
97 if(xioctl (device, VIDIOC_S_FMT,&form)==-1) {
98 Thread::Err("VIDIOC_S_FMT xioctl\n");
99 }
100
101 //alloc and queue bufs
102 AllocBuffers();
103 for (int bufferIndex = 0; bufferIndex < nbBuffers;++bufferIndex) {
104 QueueBuffer(bufferIndex);
105 }
106
107 // enable the streaming
108 v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
109 if (xioctl (device, VIDIOC_STREAMON,&type)==-1) {
110 Thread::Err("VIDIOC_STREAMON xioctl\n");
111 }
112
113 // skip first frame. it is often bad -- this is unnotied in traditional apps,
114 // but could be fatal if bad jpeg is enabled
115 bufferIndex=-1;
116 GrabFrame();
117
118 // ground station
119 gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
120 exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
121 bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
122 contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
123 hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
124 sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
125 sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
126 autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
127 autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
128 awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
129 fps = new Label(GetGroupBox()->NewRow(), "fps");
130
131 hasProblems=false;
132}
133
134V4LCamera::~V4LCamera() {
135 SafeStop();
136 Join();
137}
138
139void V4LCamera::Run(void) {
140 Time cam_time, new_time, fpsNow, fpsPrev;
141 char* buffer; // raw image
142 int fpsCounter = 0;
143
144 // init image old
145 GrabFrame();
146 cam_time = GetTime();
147 fpsPrev = cam_time;
148
149 while (!ToBeStopped()) {
150 //check for ps3eye deconnection in hds uav
151 if(hasProblems==false) {
152 struct v4l2_format form;
153 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
154 xioctl(device, VIDIOC_G_FMT,&form);
155 if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
156 Thread::Warn("camera disconnected\n");
157 hasProblems=true;
158 }
159 }
160
161 // fps counter
162 fpsCounter++;
163 if (GetTime() > (fpsPrev + 5 * (Time)1000000000)) {
164 // every 5 secondes
165 fpsNow = GetTime();
166 fps->SetText("fps: %.1f",
167 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
168 fpsCounter = 0;
169 fpsPrev = fpsNow;
170 }
171
172 // cam properties
173 if (gain->ValueChanged() == true && autogain->Value() == false)
174 SetGain(gain->Value());
175 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
176 SetExposure(exposure->Value());
177 if (bright->ValueChanged() == true)
178 SetBrightness(bright->Value());
179 if (sat->ValueChanged() == true)
180 SetSaturation(sat->Value());
181 if (contrast->ValueChanged() == true)
182 SetContrast(contrast->Value());
183 if (hue->ValueChanged() == true)
184 SetHue(hue->Value());
185 if (sharpness->ValueChanged() == true)
186 SetProperty(V4L2_CID_SHARPNESS, sharpness->Value());
187 if (autogain->ValueChanged() == true) {
188 if (autogain->Value() == true) {
189 gain->setEnabled(false);
190 } else {
191 gain->setEnabled(true);
192 SetGain(gain->Value());
193 }
194 SetAutoGain(autogain->Value());
195 }
196 if (autoexposure->ValueChanged() == true) {
197 if (autoexposure->Value() == true) {
198 exposure->setEnabled(false);
199 } else {
200 exposure->setEnabled(true);
201 SetExposure(exposure->Value());
202 }
203 SetAutoExposure(autoexposure->Value());
204 }
205 if (awb->ValueChanged() == true)
206 SetProperty(V4L2_CID_AUTO_WHITE_BALANCE, awb->Value());
207
208 // get picture
209 GrabFrame();
210 new_time = GetTime();
211
212 //check for ps3eye deconnection in hds uav
213 if(new_time-cam_time>100*1000*1000) {
214 Thread::Warn("delta trop grand\n");
215 hasProblems=true;
216 }
217Printf("todo allocate 1 frame and copy here\n");
218 output->GetMutex();
219 output->buffer = buffer;
220 output->ReleaseMutex();
221
222 output->SetDataTime(cam_time);
223 ProcessUpdate(output);
224
225 cam_time = new_time;
226 }
227
228 close(device);
229}
230
231int V4LCamera::QueueBuffer(int index) {
232 struct v4l2_buffer buf;
233 if(index>=0 && index<nbBuffers) {
234 memset(&buf, 0, sizeof (v4l2_buffer));
235 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
236 buf.memory = V4L2_MEMORY_MMAP;
237 buf.index = (unsigned long)index;
238
239 if (xioctl (device, VIDIOC_QBUF, &buf)==-1) {
240 Thread::Err("VIDIOC_QBUF xioctl\n");
241 return -1;
242 }
243 }
244 return 0;
245}
246
247int V4LCamera::GrabFrame(void) {
248 //queue previous buffer
249 if(QueueBuffer(bufferIndex)<0) return -1;
250
251 fd_set fds;
252 struct timeval tv;
253 FD_ZERO (&fds);
254 FD_SET (device, &fds);
255
256 tv.tv_sec = 0;
257 tv.tv_usec = 100000;
258//Printf("%lld\n",GetTimeMS());
259 int r = select (device+1, &fds, NULL, NULL, &tv);
260//Printf("%lld select ok\n",GetTimeMS());
261 if (-1 == r) {
262 char errorMsg[256];
263 Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
264 return -1;
265 }
266
267 if (0 == r) {
268 Thread::Err("select timeout\n");
269 return -1;
270 }
271
272 struct v4l2_buffer buf;
273 memset(&buf, 0, sizeof (v4l2_buffer));
274 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
275 buf.memory = V4L2_MEMORY_MMAP;
276
277 //get last captured image
278 int prevDQbuf=-1;
279 for(int i=0;i<4;i++) {
280 //Printf("%lld ioctl\n",GetTimeMS());
281 if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
282 // Printf("iter %i err %i\n",i,errno);
283 if (errno==EAGAIN) {
284 //Printf("exit buf %i\n",buf.index);
285 break;
286 } else {
287 Thread::Err("VIDIOC_DQBUF xioctl\n");
288 return -1;
289 }
290 } else {
291 //Printf("buf %i\n",buf.index);
292 if(prevDQbuf!=-1) {
293 QueueBuffer(prevDQbuf);
294 //Printf("queued %i\n",prevDQbuf);
295 }
296 prevDQbuf=buf.index;
297 }
298 //Printf("%lld ioctl ok\n",GetTimeMS());
299 }
300
301 if(buf.index >= nbBuffers) {
302 Thread::Err("buf.index >= nbBuffers\n");
303 return -1;
304 }
305
306 bufferIndex=buf.index;
307
308 return 1;
309}
310
311int V4LCamera::AllocBuffers(void) {
312 struct v4l2_requestbuffers requestbuffers;
313 memset(&requestbuffers, 0, sizeof (v4l2_requestbuffers));
314
315 unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
316
317 try_again:
318
319 requestbuffers.count = buffer_number;
320 requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
321 requestbuffers.memory = V4L2_MEMORY_MMAP;
322
323 if (xioctl (device, VIDIOC_REQBUFS, &requestbuffers)==-1) {
324 if (EINVAL == errno) {
325 Thread::Err("memory mapping not supported\n");
326 } else {
327 Thread::Err ("VIDIOC_REQBUFS xioctl\n");
328 }
329 return -1;
330 }
331
332 if (requestbuffers.count < buffer_number) {
333 if (buffer_number == 1) {
334 Thread::Err("Insufficient buffer memory\n");
335 return -1;
336 } else {
337 buffer_number--;
338 Thread::Warn ("Insufficient buffer memory, decreasing buffers\n");
339 goto try_again;
340 }
341 }
342
343 for (int n_buffers = 0; n_buffers < requestbuffers.count; ++n_buffers) {
344 struct v4l2_buffer buf;
345
346 memset(&buf, 0, sizeof (v4l2_buffer));
347
348 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
349 buf.memory = V4L2_MEMORY_MMAP;
350 buf.index = n_buffers;
351
352 if (xioctl (device, VIDIOC_QUERYBUF, &buf)==-1) {
353 Thread::Err("VIDIOC_QUERYBUF xioctl\n");
354 return -1;
355 }
356
357 buffers[n_buffers].length = buf.length;
358 buffers[n_buffers].start =
359 mmap (NULL /* start anywhere */,
360 buf.length,
361 PROT_READ | PROT_WRITE /* required */,
362 MAP_SHARED /* recommended */,
363 device, buf.m.offset);
364Printf("buffer %i start %x length %i\n",n_buffers,buffers[n_buffers].start,buffers[n_buffers].length);
365 if (MAP_FAILED == buffers[n_buffers].start) {
366 Thread::Err("mmap\n");
367 return -1;
368 }
369 }
370 nbBuffers=requestbuffers.count;
371Printf("allocated %i buffers\n",nbBuffers);
372 return 1;
373};
374
375bool V4LCamera::HasProblems(void) {
376 return hasProblems;
377}
378
379void V4LCamera::SetAutoGain(bool value) {
380 SetProperty(V4L2_CID_AUTOGAIN, value);
381}
382
383void V4LCamera::SetAutoExposure(bool value) {
384 Thread::Warn("not implemented\n");
385}
386
387void V4LCamera::SetGain(float value) {
388 SetProperty(V4L2_CID_GAIN, value);
389}
390
391void V4LCamera::SetExposure(float value) {
392 SetProperty(V4L2_CID_EXPOSURE, value);
393}
394
395void V4LCamera::SetBrightness(float value) {
396 SetProperty(V4L2_CID_BRIGHTNESS, value);
397}
398
399void V4LCamera::SetSaturation(float value) {
400 SetProperty(V4L2_CID_SATURATION, value);
401}
402
403void V4LCamera::SetHue(float value) {
404 SetProperty(V4L2_CID_HUE, value);
405}
406
407void V4LCamera::SetContrast(float value) {
408 SetProperty(V4L2_CID_CONTRAST, value);
409}
410
411float V4LCamera::GetProperty(int property) {
412 //get min and max value
413 struct v4l2_queryctrl queryctrl;
414 queryctrl.id = property;
415 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) return -1;
416 int min = queryctrl.minimum;
417 int max = queryctrl.maximum;
418
419 //set value
420 struct v4l2_control control;
421 memset (&control, 0, sizeof (v4l2_control));
422 control.id = property;
423 if(xioctl (device,VIDIOC_G_CTRL, &control)==-1) return -1;
424
425 return ((float)control.value - min + 1) / (max - min);
426}
427
428void V4LCamera::SetProperty(int property,float value) {
429 //get min and max value
430 struct v4l2_queryctrl queryctrl;
431 queryctrl.id = property;
432 xioctl (device, VIDIOC_QUERYCTRL,&queryctrl);
433 int min = queryctrl.minimum;
434 int max = queryctrl.maximum;
435
436 //set value
437 struct v4l2_control control;
438 memset (&control, 0, sizeof (v4l2_control));
439 control.id = property;
440 control.value = (int)(value * (max - min) + min);
441 xioctl (device,VIDIOC_S_CTRL, &control);
442}
443
444int V4LCamera::xioctl( int fd, int request, void *arg) {
445 int r;
446
447 do r = ioctl (fd, request, arg);
448 while (-1 == r && EINTR == errno);
449
450 return r;
451}
452
453} // end namespace sensor
454} // end namespace flair
Note: See TracBrowser for help on using the repository browser.