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