• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

src/N900/V4L2Sensor.cpp

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         // Request format
00106         if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
00107             error(Event::DriverError, "VIDIOC_S_FMT");
00108             return;
00109         }
00110 
00111         // Try for the right frame rate, to be robust to other programs messing with the modes
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         if (m.height <= 960) {
00119             parm.parm.capture.timeperframe.numerator = 33000;
00120             parm.parm.capture.timeperframe.denominator = 1000000;
00121         } else {
00122             parm.parm.capture.timeperframe.numerator = 77000;
00123             parm.parm.capture.timeperframe.denominator = 1000000;
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         // set the starting parameters
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         // grab the histogram data for this frame
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         // For now we assume acquire_frame is being called quickly
00262         // enough that only the newest frame is relevant
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         // TODO: Deal with timestamps that are too early or too late
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             // we got the wrong histogram!
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];  // r
00302             hist(i, 1) = buf[i];       // g
00303             hist(i, 2) = buf[128 + i]; // b
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         // grab the sharpness map for this frame        
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             // we got the wrong sharpness map
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         // get the output size from the ccdc
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         // set the gains to slightly above 1 in 3Q5 format, in
00388         // order to use the full range in the histogram. Without
00389         // this, bucket #60 is saturated pixels, and 61-64 are
00390         // unused.
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         // set up its width and height
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         // enable the histogram generator
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         // Ignore the requested size and use 16x12
00425         Size size = Size(16, 12);
00426 
00427         // get the output size from the ccdc
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         // The IIR coefficients are as follows (yay reverse-engineering!)
00443         
00444         // The format is S6Q5 fixed point. A positive value of x
00445         // should be written as 32*x. A negatives value of x
00446         // should be written as 4096 - 32*x
00447         
00448         // 0: global gain? not sure.
00449         // 1-2: IIR taps on the first biquad
00450         // 3-5: FIR taps on the first biquad
00451         // 6-7: IIR taps on the second biquad
00452         // 8-10: FIR taps on the second biquad            
00453         
00454         // A high pass filter aimed at ~8 pixel frequencies and above
00455         af_config.iir_config.coeff_set0[0] = 32; // gain of 1
00456         
00457         af_config.iir_config.coeff_set0[1] = 0; //4096-27;
00458         af_config.iir_config.coeff_set0[2] = 0; //6;
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         // A high pass filter aimed at ~4 pixel frequencies and above
00473         af_config.iir_config.coeff_set1[0] = 32; // gain of 1
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         // These are internal errors because they should have been
00495         // caught and fixed earlier in the daemon class
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         // requeue the buffer
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             // TODO: Better error reporting for all the get/set
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         // Set the pre-demosiacing gains to one by default
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         // if any of the terms are particularly large, make use of the pre-demosaicing gains instead
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         // Similarly set the black level to zero (we take care of it in the matrix)
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 

Generated on Fri Sep 24 2010 15:53:00 for FCam by  doxygen 1.7.1