00001 #include <linux/videodev2.h>
00002 #include <asm/types.h>
00003 #include <sys/types.h>
00004 #include <sys/syscall.h>
00005 #include <sys/prctl.h>
00006 #include <linux/capability.h>
00007
00008 #include <pthread.h>
00009 #include <poll.h>
00010
00011 #include <stdio.h>
00012 #include <stdlib.h>
00013 #include <string.h>
00014 #include <unistd.h>
00015 #include <math.h>
00016
00017 #include <sys/fcntl.h>
00018 #include <sys/ioctl.h>
00019 #include <sys/mman.h>
00020 #include <sys/time.h>
00021 #include <time.h>
00022
00023 #include <errno.h>
00024 #include <malloc.h>
00025
00026 #include "../Debug.h"
00027 #include "V4L2Sensor.h"
00028 #include "linux/isp_user.h"
00029 #include "linux/omap34xxcam-fcam.h"
00030 #include "FCam/Event.h"
00031
00032 namespace FCam { namespace N900 {
00033
00034 V4L2Sensor *V4L2Sensor::instance(std::string fname) {
00035 std::map<std::string, V4L2Sensor *>::iterator i;
00036 i = instances_.find(fname);
00037 if (i == instances_.end()) {
00038 instances_[fname] = new V4L2Sensor(fname);
00039 }
00040
00041 return instances_[fname];
00042 };
00043
00044 V4L2Sensor::V4L2Sensor(std::string fname) : state(CLOSED), filename(fname) {
00045
00046 }
00047
00048 std::map<std::string, V4L2Sensor *> V4L2Sensor::instances_;
00049
00050 void V4L2Sensor::open() {
00051 if (state != CLOSED) {
00052 return;
00053 }
00054
00055 fd = ::open(filename.c_str(), O_RDWR | O_NONBLOCK, 0);
00056
00057 if (fd < 0) {
00058 error(Event::DriverError, "V4L2Sensor: Error opening %s: %s", filename.c_str(), strerror(errno));
00059 return;
00060 }
00061
00062 state = IDLE;
00063 }
00064
00065 void V4L2Sensor::close() {
00066 if (state != CLOSED) {
00067 ::close(fd);
00068 }
00069 state = CLOSED;
00070 }
00071
00072 int V4L2Sensor::getFD() {
00073 if (state == CLOSED) {
00074 return -1;
00075 }
00076 return fd;
00077 }
00078
00079 void V4L2Sensor::startStreaming(Mode m,
00080 const HistogramConfig &histogram,
00081 const SharpnessMapConfig &sharpness) {
00082
00083 if (state != IDLE) {
00084 error(Event::InternalError, "V4L2Sensor: Can only initiate streaming if sensor is idle");
00085 return;
00086 }
00087
00088 struct v4l2_format fmt;
00089
00090 memset(&fmt, 0, sizeof(struct v4l2_format));
00091
00092 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00093 fmt.fmt.pix.width = m.width;
00094 fmt.fmt.pix.height = m.height;
00095 if (m.type == UYVY) {
00096 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
00097 } else if (m.type == RAW) {
00098 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SGRBG10;
00099 } else {
00100 error(Event::InternalError, "V4L2Sensor: Unknown image format requested");
00101 return;
00102 }
00103 fmt.fmt.pix.field = V4L2_FIELD_NONE;
00104
00105
00106 if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
00107 error(Event::DriverError, "VIDIOC_S_FMT");
00108 return;
00109 }
00110
00111
00112 struct v4l2_streamparm parm;
00113 memset(&parm, 0, sizeof(struct v4l2_streamparm));
00114 parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00115 parm.parm.capture.timeperframe.numerator = 0;
00116 parm.parm.capture.timeperframe.denominator = 1000000;
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126 if (ioctl(fd, VIDIOC_S_PARM, &parm) < 0) {
00127 error(Event::DriverError, "VIDIOC_S_PARM");
00128 return;
00129 }
00130
00131 currentMode.width = fmt.fmt.pix.width;
00132 currentMode.height = fmt.fmt.pix.height;
00133 currentMode.type = (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) ? UYVY : RAW;
00134
00135 struct v4l2_requestbuffers req;
00136 memset(&req, 0, sizeof(req));
00137 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00138 req.memory = V4L2_MEMORY_MMAP;
00139 req.count = 8;
00140
00141 if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
00142 error(Event::DriverError, "VIDIOC_REQBUFS: %s", strerror(errno));
00143 return;
00144 }
00145
00146 buffers.resize(req.count);
00147
00148 for (size_t i = 0; i < buffers.size(); i++) {
00149 v4l2_buffer buf;
00150 memset(&buf, 0, sizeof(v4l2_buffer));
00151 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00152 buf.memory = V4L2_MEMORY_MMAP;
00153 buf.index = i;
00154
00155 if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
00156 error(Event::DriverError, "VIDIOC_QUERYBUF: %s", strerror(errno));
00157 return;
00158 }
00159
00160 buffers[i].index = i;
00161 buffers[i].length = buf.length;
00162 buffers[i].data =
00163 (unsigned char *)mmap(NULL, buffers[i].length, PROT_READ | PROT_WRITE,
00164 MAP_SHARED, fd, buf.m.offset);
00165
00166 if (buffers[i].data == MAP_FAILED) {
00167 error(Event::InternalError, "V4L2Sensor: mmap failed: %s", strerror(errno));
00168 return;
00169 }
00170 }
00171
00172 for (size_t i = 0; i < buffers.size(); i++) {
00173 releaseFrame(&buffers[i]);
00174 }
00175
00176
00177 setHistogramConfig(histogram);
00178 setSharpnessMapConfig(sharpness);
00179
00180 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00181 if (ioctl(fd, VIDIOC_STREAMON, &type) < 0) {
00182 error(Event::DriverError, "VIDIOC_STREAMON: %s", strerror(errno));
00183 return;
00184 }
00185
00186 dprintf(2, "Sensor now streaming\n");
00187 state = STREAMING;
00188 }
00189
00190
00191 void V4L2Sensor::stopStreaming() {
00192 if (state != STREAMING) {
00193 return;
00194 }
00195
00196 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00197 if (ioctl(fd, VIDIOC_STREAMOFF, &type) < 0) {
00198 error(Event::DriverError, "VIDIOC_STREAMOFF: %s", strerror(errno));
00199 return;
00200 }
00201
00202 for (size_t i = 0; i < buffers.size(); i++) {
00203 if (munmap(buffers[i].data, buffers[i].length)) {
00204 error(Event::InternalError, "munmap failed: %s", strerror(errno));
00205 }
00206 }
00207
00208 state = IDLE;
00209 }
00210
00211
00212
00213 V4L2Sensor::V4L2Frame *V4L2Sensor::acquireFrame(bool blocking) {
00214 if (state != STREAMING) {
00215 error(Event::InternalError, "V4L2Sensor: Can't acquire a frame when not streaming");
00216 return NULL;
00217 }
00218
00219 v4l2_buffer buf;
00220 memset(&buf, 0, sizeof(v4l2_buffer));
00221 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00222 buf.memory = V4L2_MEMORY_MMAP;
00223
00224 if (blocking) {
00225 struct pollfd p = {fd, POLLIN, 0};
00226 poll(&p, 1, -1);
00227 if (!(p.revents & POLLIN)) {
00228 error(Event::DriverError, "Poll returned without data being available: %s", strerror(errno));
00229 return NULL;
00230 }
00231 }
00232
00233 if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
00234 if (errno == EAGAIN && !blocking) {
00235 return NULL;
00236 }
00237
00238 error(Event::DriverError, "VIDIOC_DQBUF: %s", strerror(errno));
00239 return NULL;
00240 }
00241
00242 buffers[buf.index].processingDoneTime = Time(buf.timestamp);
00243 return &(buffers[buf.index]);
00244 }
00245
00246
00247
00248 Histogram V4L2Sensor::getHistogram(Time t, const HistogramConfig &conf) {
00249
00250 if (!conf.enabled) {
00251 return Histogram();
00252 }
00253
00254
00255
00256 struct isp_hist_data hist_data;
00257 unsigned buf[64 * 4];
00258 hist_data.hist_statistics_buf = buf;
00259 hist_data.update = REQUEST_STATISTICS;
00260
00261
00262
00263 hist_data.frame_number = NEWEST_FRAME;
00264 hist_data.curr_frame = 0;
00265 hist_data.config_counter = 0;
00266 hist_data.ts.tv_sec = 0;
00267 hist_data.ts.tv_usec = 0;
00268
00269 if (ioctl(fd, VIDIOC_PRIVATE_ISP_HIST_REQ, &hist_data)) {
00270 if (errno != EBUSY)
00271 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_HIST_REQ: %s", strerror(errno));
00272 return Histogram();
00273 }
00274
00275
00276
00277 Time h(hist_data.ts);
00278
00279 if ((t - h) > 4000) {
00280 warning(Event::DriverError, "Missing histogram (%d)\n", t-h);
00281 return Histogram();
00282 }
00283
00284 while ((t-h) < -4000) {
00285
00286 if (hist_data.frame_number == 0) hist_data.frame_number = 4095;
00287 else hist_data.frame_number--;
00288
00289 if (ioctl(fd, VIDIOC_PRIVATE_ISP_HIST_REQ, &hist_data)) {
00290 if (errno != EBUSY)
00291 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_HIST_REQ: %s",
00292 strerror(errno));
00293 return Histogram();
00294 }
00295
00296 h = Time(hist_data.ts);
00297 }
00298
00299 Histogram hist(64, 3, conf.region);
00300 for (int i = 0; i < 64; i++) {
00301 hist(i, 0) = buf[64 + i];
00302 hist(i, 1) = buf[i];
00303 hist(i, 2) = buf[128 + i];
00304 }
00305 return hist;
00306 }
00307
00308 SharpnessMap V4L2Sensor::getSharpnessMap(Time t, const SharpnessMapConfig &conf) {
00309 if (!conf.enabled) {
00310 return SharpnessMap();
00311 }
00312
00313
00314 struct isp_af_data af_data;
00315 af_data.frame_number = NEWEST_FRAME;
00316 af_data.update = REQUEST_STATISTICS;
00317 af_data.curr_frame = 0;
00318 af_data.config_counter = 0;
00319 af_data.xtrastats.ts.tv_sec = 0;
00320 af_data.xtrastats.ts.tv_usec = 0;
00321 unsigned buf[16*12*12];
00322 af_data.af_statistics_buf = buf;
00323
00324 if (ioctl(fd, VIDIOC_PRIVATE_ISP_AF_REQ, &af_data)) {
00325 if (errno != EBUSY)
00326 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_AF_REQ: %s", strerror(errno));
00327 return SharpnessMap();
00328 }
00329
00330 Time s(af_data.xtrastats.ts);
00331 if ((t - s) > 4000) {
00332 warning(Event::DriverError, "Missing sharpness (%d)\n", t-s);
00333 }
00334
00335 while ((t-s) < -4000) {
00336
00337 if (af_data.frame_number == 0) af_data.frame_number = 4095;
00338 else af_data.frame_number--;
00339
00340 if (ioctl(fd, VIDIOC_PRIVATE_ISP_AF_REQ, &af_data)) {
00341 if (errno != EBUSY)
00342 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_AF_REQ: %s", strerror(errno));
00343 return SharpnessMap();
00344 }
00345 s = Time(af_data.xtrastats.ts);
00346 }
00347
00348 SharpnessMap m(Size(16, 12), 3);
00349 unsigned *bufPtr = &buf[0];
00350 for (int y = 0; y < m.size().height; y++) {
00351 for (int x = 0; x < m.size().width; x++) {
00352 m(x, y, 0) = bufPtr[1];
00353 m(x, y, 1) = bufPtr[5];
00354 m(x, y, 2) = bufPtr[9];
00355 bufPtr += 12;
00356 }
00357 }
00358
00359 return m;
00360 }
00361
00362 void V4L2Sensor::setHistogramConfig(const HistogramConfig &histogram) {
00363 if (!histogram.enabled) return;
00364
00365
00366 isp_pipeline_stats pstats;
00367 if (ioctl(fd, VIDIOC_PRIVATE_ISP_PIPELINE_STATS_REQ, &pstats) < 0) {
00368 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_PIPELINE_STATS_REQ: %s", strerror(errno));
00369 return;
00370 }
00371
00372
00373 dprintf(4, "CCDC output: %d x %d\n", pstats.ccdc_out_w, pstats.ccdc_out_h);
00374 dprintf(4, "PRV output: %d x %d\n", pstats.prv_out_w, pstats.prv_out_h);
00375 dprintf(4, "RSZ input: %d x %d + %d, %d\n",
00376 pstats.rsz_in_w, pstats.rsz_in_h,
00377 pstats.rsz_in_x, pstats.rsz_in_y);
00378 dprintf(4, "RSZ output: %d x %d\n", pstats.rsz_out_w, pstats.rsz_out_h);
00379
00380 struct isp_hist_config hist_cfg;
00381 hist_cfg.enable = 1;
00382 hist_cfg.source = HIST_SOURCE_CCDC;
00383 hist_cfg.input_bit_width = 10;
00384 hist_cfg.num_acc_frames = 1;
00385 hist_cfg.hist_bins = HIST_BINS_64;
00386 hist_cfg.cfa = HIST_CFA_BAYER;
00387
00388
00389
00390
00391 hist_cfg.wg[0] = 35;
00392 hist_cfg.wg[1] = 35;
00393 hist_cfg.wg[2] = 35;
00394 hist_cfg.wg[3] = 35;
00395 hist_cfg.num_regions = 1;
00396
00397
00398 unsigned x = ((unsigned)histogram.region.x * pstats.ccdc_out_w) / currentMode.width;
00399 unsigned y = ((unsigned)histogram.region.y * pstats.ccdc_out_h) / currentMode.height;
00400 unsigned w = ((unsigned)histogram.region.width * pstats.ccdc_out_w) / currentMode.width;
00401 unsigned h = ((unsigned)histogram.region.height * pstats.ccdc_out_h) / currentMode.height;
00402 if (x > pstats.ccdc_out_w) x = pstats.ccdc_out_w-1;
00403 if (y > pstats.ccdc_out_h) y = pstats.ccdc_out_h-1;
00404 if (w > pstats.ccdc_out_w) w = pstats.ccdc_out_w-x;
00405 if (h > pstats.ccdc_out_h) h = pstats.ccdc_out_h-y;
00406 hist_cfg.reg_hor[0] = (x << 16) | w;
00407 hist_cfg.reg_ver[0] = (y << 16) | h;
00408 dprintf(4, "Histogram size: %d x %d + %d, %d\n", w, h, x, y);
00409
00410 dprintf(3, "Enabling histogram generator\n");
00411
00412 if (ioctl(fd, VIDIOC_PRIVATE_ISP_HIST_CFG, &hist_cfg)) {
00413 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_HIST_CFG: %s", strerror(errno));
00414 return;
00415 }
00416
00417 currentHistogram = histogram;
00418 currentHistogram.buckets = 64;
00419 }
00420
00421 void V4L2Sensor::setSharpnessMapConfig(const SharpnessMapConfig &sharpness) {
00422 if (!sharpness.enabled) return;
00423
00424
00425 Size size = Size(16, 12);
00426
00427
00428 isp_pipeline_stats pstats;
00429 if (ioctl(fd, VIDIOC_PRIVATE_ISP_PIPELINE_STATS_REQ, &pstats) < 0) {
00430 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_PIPELINE_STATS_REQ: %s", strerror(errno));
00431 return;
00432 }
00433
00434 struct af_configuration af_config;
00435
00436 af_config.alaw_enable = H3A_AF_ALAW_DISABLE;
00437 af_config.hmf_config.enable = H3A_AF_HMF_ENABLE;
00438 af_config.hmf_config.threshold = 10;
00439 af_config.rgb_pos = RG_GB_BAYER;
00440 af_config.iir_config.hz_start_pos = 0;
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455 af_config.iir_config.coeff_set0[0] = 32;
00456
00457 af_config.iir_config.coeff_set0[1] = 0;
00458 af_config.iir_config.coeff_set0[2] = 0;
00459
00460 af_config.iir_config.coeff_set0[3] = 16;
00461 af_config.iir_config.coeff_set0[4] = 4096-32;
00462 af_config.iir_config.coeff_set0[5] = 16;
00463
00464 af_config.iir_config.coeff_set0[6] = 0;
00465 af_config.iir_config.coeff_set0[7] = 0;
00466
00467 af_config.iir_config.coeff_set0[8] = 32;
00468 af_config.iir_config.coeff_set0[9] = 0;
00469 af_config.iir_config.coeff_set0[10] = 0;
00470
00471
00472
00473 af_config.iir_config.coeff_set1[0] = 32;
00474
00475 af_config.iir_config.coeff_set1[1] = 0;
00476 af_config.iir_config.coeff_set1[2] = 0;
00477
00478 af_config.iir_config.coeff_set1[3] = 16;
00479 af_config.iir_config.coeff_set1[4] = 4096-32;
00480 af_config.iir_config.coeff_set1[5] = 16;
00481
00482 af_config.iir_config.coeff_set1[6] = 0;
00483 af_config.iir_config.coeff_set1[7] = 0;
00484
00485 af_config.iir_config.coeff_set1[8] = 32;
00486 af_config.iir_config.coeff_set1[9] = 0;
00487 af_config.iir_config.coeff_set1[10] = 0;
00488
00489 af_config.mode = ACCUMULATOR_SUMMED;
00490 af_config.af_config = H3A_AF_CFG_ENABLE;
00491 int paxWidth = ((pstats.ccdc_out_w-4) / (2*size.width))*2;
00492 int paxHeight = ((pstats.ccdc_out_h-4) / (2*size.height))*2;
00493
00494
00495
00496
00497 if (paxWidth > 256) {
00498 error(Event::InternalError, "AF paxels are too wide. Use a higher resolution sharpness map\n");
00499 return;
00500 }
00501 if (paxHeight > 256) {
00502 error(Event::InternalError, "AF paxels are too tall. Use a higher resolution sharpness map\n");
00503 return;
00504 }
00505 if (paxWidth < 16) {
00506 error(Event::InternalError, "AF paxels are too narrow. Use a lower resolution sharpness map\n");
00507 return;
00508 }
00509 if (paxHeight < 2) {
00510 error(Event::InternalError, "AF paxels are too short. Use a lower resolution sharpness map\n");
00511 return;
00512 }
00513
00514 dprintf(4, "Using %d x %d paxels for af\n", paxWidth, paxHeight);
00515 af_config.paxel_config.width = (paxWidth-2)/2;
00516 af_config.paxel_config.height = (paxHeight-2)/2;
00517 af_config.paxel_config.hz_start = (pstats.ccdc_out_w - size.width * paxWidth)/2;
00518 af_config.paxel_config.vt_start = (pstats.ccdc_out_h - size.height * paxHeight)/2;
00519 af_config.paxel_config.hz_cnt = size.width-1;
00520 af_config.paxel_config.vt_cnt = size.height-1;
00521 af_config.paxel_config.line_incr = 0;
00522
00523 dprintf(3, "Enabling sharpness detector\n");
00524 if (ioctl(fd, VIDIOC_PRIVATE_ISP_AF_CFG, &af_config)) {
00525 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_AF_CFG: %s", strerror(errno));
00526 return;
00527 }
00528
00529 currentSharpness = sharpness;
00530 }
00531
00532 void V4L2Sensor::releaseFrame(V4L2Frame *frame) {
00533
00534 v4l2_buffer buf;
00535 memset(&buf, 0, sizeof(v4l2_buffer));
00536 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00537 buf.memory = V4L2_MEMORY_MMAP;
00538 buf.index = frame->index;
00539
00540 if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
00541 error(Event::DriverError, "VIDIOC_QBUF: %s", strerror(errno));
00542 return;
00543 }
00544 }
00545
00546 void V4L2Sensor::setControl(unsigned int id, int value) {
00547 if (state == CLOSED) return;
00548 v4l2_control ctrl;
00549 ctrl.id = id;
00550 ctrl.value = value;
00551 if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
00552
00553 error(Event::DriverError, "VIDIOC_S_CTRL: %s", strerror(errno));
00554 return;
00555 }
00556 }
00557
00558 int V4L2Sensor::getControl(unsigned int id) {
00559 if (state == CLOSED) return -1;
00560 v4l2_control ctrl;
00561 ctrl.id = id;
00562 if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
00563 error(Event::DriverError, "VIDIOC_G_CTRL: %s", strerror(errno));
00564 return -1;
00565 }
00566
00567 return ctrl.value;
00568 }
00569
00570 void V4L2Sensor::setExposure(int e) {
00571 if (state == CLOSED) return;
00572
00573 dprintf(3, "Setting exposure to %d\n", e);
00574
00575 struct v4l2_control ctrl;
00576 ctrl.id = V4L2_CID_EXPOSURE;
00577 ctrl.value = e;
00578 if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
00579 error(Event::DriverError, "VIDIOC_S_CTRL: %s", strerror(errno));
00580 return;
00581 }
00582
00583 }
00584
00585 int V4L2Sensor::getExposure() {
00586 if (state == CLOSED) return -1;
00587
00588 struct v4l2_control ctrl;
00589 ctrl.id = V4L2_CID_EXPOSURE;
00590
00591 if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
00592 error(Event::DriverError, "VIDIOC_G_CTRL: %s", strerror(errno));
00593 return -1;
00594 }
00595
00596 dprintf(3, "Getting exposure: %d\n", ctrl.value);
00597
00598 return ctrl.value;
00599 }
00600
00601 #define V4L2_CID_FRAME_TIME (V4L2_CTRL_CLASS_CAMERA | 0x10ff)
00602
00603 void V4L2Sensor::setFrameTime(int e) {
00604 if (state == CLOSED) return;
00605
00606 struct v4l2_control ctrl;
00607 ctrl.id = V4L2_CID_FRAME_TIME;
00608 ctrl.value = e;
00609 dprintf(3, "Setting frame time to %d\n", e);
00610 if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
00611 error(Event::DriverError, "VIDIOC_S_CTRL: %s", strerror(errno));
00612 return;
00613 }
00614 }
00615
00616 int V4L2Sensor::getFrameTime() {
00617 if (state == CLOSED) return -1;
00618
00619 struct v4l2_control ctrl;
00620 ctrl.id = V4L2_CID_FRAME_TIME;
00621
00622 if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
00623 error(Event::DriverError, "VIDIOC_G_CTRL: %s", strerror(errno));
00624 return -1;
00625 }
00626
00627 dprintf(3, "Getting frame time: %d\n", ctrl.value);
00628
00629 return ctrl.value;
00630 }
00631
00632 void V4L2Sensor::setGain(float g) {
00633 if (state == CLOSED) return;
00634
00635 unsigned int gain;
00636 struct v4l2_control ctrl;
00637
00638 gain = (int)(g * 32.0 + 0.5);
00639
00640 ctrl.id = V4L2_CID_GAIN_EXACT;
00641 ctrl.value = gain;
00642 if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
00643 error(Event::DriverError, "VIDIOC_S_CTRL: %s", strerror(errno));
00644 return;
00645 }
00646 }
00647
00648 float V4L2Sensor::getGain() {
00649 if (state == CLOSED) return -1.0f;
00650
00651 struct v4l2_control ctrl;
00652
00653 ctrl.id = V4L2_CID_GAIN_EXACT;
00654 if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
00655 error(Event::DriverError, "VIDIOC_G_CTRL: %s", strerror(errno));
00656 return -1.0f;
00657 }
00658
00659 return ctrl.value / 32.0f;
00660 }
00661
00662 float max3(float a, float b, float c) {
00663 if (a > b && a > c) return a;
00664 else if (b > c) return b;
00665 return c;
00666 }
00667
00668 void V4L2Sensor::setWhiteBalance(const float *matrix) {
00669 if (state == CLOSED) return;
00670
00671
00672 struct ispprv_update_config prvcfg;
00673 prvcfg.update = ISP_ABS_PREV_RGB2RGB | ISP_ABS_PREV_WB | ISP_ABS_PREV_BLKADJ;
00674 struct ispprev_wbal wbal;
00675 wbal.dgain = 256;
00676 wbal.coef0 = 32;
00677 wbal.coef1 = 32;
00678 wbal.coef2 = 32;
00679 wbal.coef3 = 32;
00680 prvcfg.prev_wbal = &wbal;
00681
00682
00683 float rs = 1.0, gs = 1.0, bs = 1.0;
00684 float maxC = max3(fabs(matrix[0]), fabs(matrix[4]), fabs(matrix[8]));
00685 while (maxC > 8) {
00686 wbal.coef1 *= 2;
00687 maxC /= 2;
00688 rs *= 0.5;
00689 }
00690
00691 maxC = max3(fabs(matrix[1]), fabs(matrix[5]), fabs(matrix[9]));
00692 while (maxC > 8) {
00693 wbal.coef0 *= 2;
00694 wbal.coef3 *= 2;
00695 maxC /= 2;
00696 gs *= 0.5;
00697 }
00698
00699 maxC = max3(fabs(matrix[2]), fabs(matrix[6]), fabs(matrix[10]));
00700 while (maxC > 8) {
00701 wbal.coef2 *= 2;
00702 maxC /= 2;
00703 bs *= 0.5;
00704 }
00705
00706 ispprev_rgbtorgb rgb2rgb;
00707 for (int i = 0; i < 3; i++) {
00708 rgb2rgb.matrix[i][0] = (signed short)(matrix[i*4+0]*rs*256);
00709 rgb2rgb.matrix[i][1] = (signed short)(matrix[i*4+1]*gs*256);
00710 rgb2rgb.matrix[i][2] = (signed short)(matrix[i*4+2]*bs*256);
00711 }
00712 rgb2rgb.offset[0] = (signed short)(matrix[3]);
00713 rgb2rgb.offset[1] = (signed short)(matrix[7]);
00714 rgb2rgb.offset[2] = (signed short)(matrix[11]);
00715 prvcfg.rgb2rgb = &rgb2rgb;
00716
00717
00718 struct ispprev_blkadj blkadj;
00719 blkadj.red = blkadj.green = blkadj.blue = 0;
00720 prvcfg.prev_blkadj = &blkadj;
00721
00722 if (ioctl(fd, VIDIOC_PRIVATE_ISP_PRV_CFG, &prvcfg) < 0) {
00723 error(Event::DriverError, "VIDIOC_PRIVATE_ISP_PRV_CFG: %s", strerror(errno));
00724 }
00725
00726 }
00727 }}
00728
00729