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

src/N900/Daemon.cpp

00001 #include <errno.h>
00002 #include <sys/types.h>
00003 #include <sys/stat.h>
00004 #include <sys/fcntl.h>
00005 #include <sys/ioctl.h>
00006 #include <poll.h>
00007 
00008 #include "FCam/Time.h"
00009 #include "FCam/Frame.h"
00010 #include "FCam/Action.h"
00011 
00012 #include "../Debug.h"
00013 #include "Daemon.h"
00014 #include "linux/omap34xxcam-fcam.h"
00015 
00016 namespace FCam { namespace N900 {
00017 
00018     void *daemon_setter_thread_(void *arg) {
00019         Daemon *d = (Daemon *)arg;
00020         d->runSetter();    
00021         d->setterRunning = false;    
00022         close(d->daemon_fd);
00023         pthread_exit(NULL);
00024     } 
00025 
00026     void *daemon_handler_thread_(void *arg) {
00027         Daemon *d = (Daemon *)arg;
00028         d->runHandler();
00029         d->handlerRunning = false;
00030         pthread_exit(NULL);    
00031     }
00032 
00033     void *daemon_action_thread_(void *arg) {
00034         Daemon *d = (Daemon *)arg;
00035         d->runAction();
00036         d->actionRunning = false;
00037         pthread_exit(NULL);
00038     }
00039 
00040     Daemon::Daemon(Sensor *sensor) :
00041         sensor(sensor),
00042         stop(false), 
00043         frameLimit(128),
00044         dropPolicy(Sensor::DropNewest),
00045         setterRunning(false), 
00046         handlerRunning(false), 
00047         actionRunning(false),
00048         threadsLaunched(false) {
00049         
00050         // tie ourselves to the correct sensor
00051         v4l2Sensor = V4L2Sensor::instance("/dev/video0");
00052 
00053         // make the mutexes for the producer-consumer queues
00054         if ((errno = 
00055              -(pthread_mutex_init(&actionQueueMutex, NULL) ||               
00056                pthread_mutex_init(&cameraMutex, NULL)))) {
00057             error(Event::InternalError, sensor, "Error creating mutexes: %d", errno);
00058         }
00059     
00060         // make the semaphore
00061         sem_init(&actionQueueSemaphore, 0, 0);
00062 
00063         lastGoodShot.wanted = false;
00064 
00065         pipelineFlush = true;
00066     }
00067 
00068     void Daemon::launchThreads() {    
00069         if (threadsLaunched) return;
00070         threadsLaunched = true;
00071 
00072         // launch the threads
00073         pthread_attr_t attr;
00074         struct sched_param param;
00075 
00076         // Open the device as a daemon
00077         daemon_fd = open("/dev/video0", O_RDWR, 0);
00078 
00079         if (daemon_fd < 0) {
00080             error(Event::InternalError, sensor, "Error opening /dev/video0: %d", errno);
00081             return;
00082         }
00083 
00084         // Try to register myself as the fcam camera client
00085         if (ioctl(daemon_fd, VIDIOC_FCAM_INSTALL, NULL)) {
00086             if (errno == EBUSY) {
00087                 error(Event::DriverLockedError, sensor,
00088                       "An FCam program is already running");
00089             } else {
00090                 error(Event::DriverMissingError, sensor, 
00091                       "Error %d calling VIDIOC_FCAM_INSTALL: Are the FCam drivers installed?", errno);
00092             }
00093             return;
00094         }
00095         // I should now have CAP_SYS_NICE
00096 
00097         // make the setter thread
00098         param.sched_priority = sched_get_priority_min(SCHED_FIFO)+1;
00099 
00100         pthread_attr_init(&attr);
00101 
00102         if ((errno =
00103              -(pthread_attr_setschedparam(&attr, &param) ||
00104                pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00105                pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00106                pthread_create(&setterThread, &attr, daemon_setter_thread_, this)))) {
00107             error(Event::InternalError, sensor, "Error creating daemon setter thread: %d", errno);
00108             return;
00109         } else {
00110             setterRunning = true;
00111         }
00112 
00113         // make the handler thread
00114         param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00115 
00116         if ((errno =
00117              -(pthread_attr_setschedparam(&attr, &param) ||
00118                pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00119                pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00120                pthread_create(&handlerThread, &attr, daemon_handler_thread_, this)))) {
00121             error(Event::InternalError, sensor, "Error creating daemon handler thread: %d", errno);
00122             return;
00123         } else {
00124             handlerRunning = true;
00125         }
00126 
00127         // make the actions thread
00128         param.sched_priority = sched_get_priority_max(SCHED_FIFO);
00129 
00130         if ((errno =
00131              -(pthread_attr_setschedparam(&attr, &param) ||
00132                pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00133                pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00134                pthread_create(&actionThread, &attr, daemon_action_thread_, this)))) {
00135             error(Event::InternalError, sensor, "Error creating daemon action thread: %d", errno);
00136             return;
00137         } else {
00138             actionRunning = true;
00139         }
00140 
00141         pthread_attr_destroy(&attr);
00142     }
00143 
00144     Daemon::~Daemon() {
00145         stop = true;
00146 
00147         // post a wakeup call to the action thread
00148         sem_post(&actionQueueSemaphore);
00149 
00150         if (setterRunning) 
00151             pthread_join(setterThread, NULL);
00152     
00153         if (handlerRunning)
00154             pthread_join(handlerThread, NULL);
00155 
00156         if (actionRunning)
00157             pthread_join(actionThread, NULL);
00158 
00159         pthread_mutex_destroy(&actionQueueMutex);
00160         pthread_mutex_destroy(&cameraMutex);
00161 
00162         // Clean up all the internal queues
00163         while (inFlightQueue.size()) delete inFlightQueue.pull();        
00164         while (requestQueue.size()) delete requestQueue.pull();
00165         while (frameQueue.size()) delete frameQueue.pull();
00166         while (actionQueue.size()) {
00167             delete actionQueue.top().action;
00168             actionQueue.pop();
00169         }
00170 
00171         v4l2Sensor->stopStreaming();
00172 
00173         v4l2Sensor->close();
00174     }
00175 
00176 
00177     void Daemon::setDropPolicy(Sensor::DropPolicy p, int f) {
00178         dropPolicy = p;
00179         frameLimit = f;
00180         enforceDropPolicy();
00181     }
00182 
00183     void Daemon::enforceDropPolicy() {
00184         if (frameQueue.size() > frameLimit) {
00185             warning(Event::FrameLimitHit, sensor,
00186                     "WARNING: frame limit hit (%d), silently dropping %d frames.\n"
00187                    "You're not draining the frame queue quickly enough. Use longer \n"
00188                    "frame times or drain the frame queue until empty every time you \n"
00189                    "call getFrame()\n", frameLimit, frameQueue.size() - frameLimit);
00190             if (dropPolicy == Sensor::DropOldest) {
00191                 while (frameQueue.size() >= frameLimit) {
00192                     sensor->decShotsPending();
00193                     delete frameQueue.pull();
00194                 }
00195             } else if (dropPolicy == Sensor::DropNewest) {
00196                 while (frameQueue.size() >= frameLimit) {
00197                     sensor->decShotsPending();
00198                     delete frameQueue.pullBack();
00199                 }
00200             } else {
00201                 error(Event::InternalError, sensor, 
00202                       "Unknown drop policy! Not dropping frames.\n");
00203             }
00204         }
00205     }
00206 
00207     void Daemon::runSetter() {
00208         dprintf(2, "Running setter...\n"); fflush(stdout);
00209         tickSetter(Time::now());
00210         while (!stop) {
00211             struct timeval t;
00212             if (ioctl(daemon_fd, VIDIOC_FCAM_WAIT_FOR_HS_VS, &t)) {                
00213                 if (stop) break;
00214                 error(Event::DriverError, sensor, 
00215                       "error in VIDIOC_FCAM_WAIT_FOR_HS_VS: %s", strerror(errno));
00216                 usleep(100000);
00217                 continue;
00218             }
00219             tickSetter(Time(t));
00220         }
00221     // When shutting down, make sure we leave V4L2 with the last requested Shot's 
00222     // settings, so that they can be used by later programs. Otherwise, the bubbles
00223     // produced during shutdown will clobber them. Gain and WB are not written by bubbles
00224     // so don't need to be dealt with here.
00225         if (lastGoodShot.wanted == true) {
00226             v4l2Sensor->setFrameTime(lastGoodShot.frameTime);
00227             v4l2Sensor->setExposure(lastGoodShot.exposure);
00228             v4l2Sensor->setGain(lastGoodShot.gain);
00229         }
00230     }
00231 
00232     void Daemon::tickSetter(Time hs_vs) {
00233         static _Frame *req = NULL;
00234 
00235         dprintf(3, "Current hs_vs was at %d %d\n", hs_vs.s(), hs_vs.us());
00236 
00237         // how long will the previous frame take to readout
00238         static int lastReadout = 33000;
00239 
00240         static bool ignoreNextHSVS = false;
00241 
00242         dprintf(4, "Setter: got HS_VS\n");
00243 
00244         if (ignoreNextHSVS) {
00245             dprintf(4, "Setter: ignoring it\n");
00246             ignoreNextHSVS = false;
00247             return;
00248         }
00249 
00250         // Is there a request for which I have set resolution and exposure, but not gain and WB?
00251         if (req) {
00252             dprintf(4, "Setter: setting gain and WB\n");
00253             // set the gain and predicted done time on the pending request
00254             // and then push it onto the handler's input queue and the v4l2 buffer queue
00255         // Only update gain/wb for wanted requests, bubbles shouldn't change these.
00256         if (req->shot().wanted == true) {
00257         if (req->shot().gain != current._shot.gain) {
00258             v4l2Sensor->setGain(req->shot().gain);
00259             current._shot.gain = req->shot().gain;
00260         }
00261         current.gain = req->gain = v4l2Sensor->getGain();
00262         
00263         if (req->shot().colorMatrix().size() != 12) {
00264             if (req->shot().whiteBalance != current._shot.whiteBalance) {                
00265             int wb = req->shot().whiteBalance;
00266             
00267             // Very lenient sanity checks - restricting the range is up to the auto-wb algorithm
00268             if (wb < 0) wb = 0; // a black-body radiator at absolute zero.
00269             if (wb > 25000) wb = 25000; // A type 'O' star.
00270             
00271             float matrix[12];
00272             sensor->platform().rawToRGBColorMatrix(wb, matrix);
00273             v4l2Sensor->setWhiteBalance(matrix);
00274             
00275             current._shot.whiteBalance = req->shot().whiteBalance;
00276             current.whiteBalance = wb;            
00277             }
00278             req->whiteBalance = current.whiteBalance;
00279         } else {
00280             // custom WB - always set it                
00281             v4l2Sensor->setWhiteBalance(&req->shot().colorMatrix()[0]);
00282             current._shot.setColorMatrix(&req->shot().colorMatrix()[0]);
00283         }
00284         }
00285 
00286             // Predict when this frame will be done. It should be HS_VS +
00287             // the readout time for the previous frame + the frame time
00288             // for this request + however long the ISP takes to process
00289             // this frame.
00290             req->processingDoneTime = hs_vs;
00291         
00292             // first there's the readout time for the previous frame
00293             req->processingDoneTime += lastReadout;
00294         
00295             // then there's the time to expose and read out this frame
00296             req->processingDoneTime += req->frameTime;
00297 
00298             // then there's some significant time inside the ISP if it's YUV and large
00299             // (this may be N900 specific)
00300             int ispTime = 0;
00301             if (req->image.type() == UYVY) {
00302                 if (req->image.height() > 1024 && req->image.width() > 1024) {
00303                     ispTime = 65000;
00304                 } else {
00305                     ispTime = 10000;
00306                 }
00307             }
00308             req->processingDoneTime += ispTime;               
00309         
00310             // Also compute when the exposure starts and finishes
00311             req->exposureStartTime = hs_vs + req->frameTime - req->exposure;
00312 
00313             // Now updated the readout time for this frame. This formula
00314             // is specific to the toshiba et8ek8 sensor on the n900
00315             lastReadout = (current.image.height() > 1008) ? 76000 : 33000;
00316 
00317             req->exposureEndTime  = req->exposureStartTime + req->exposure + lastReadout;
00318 
00319             // now queue up this request's actions
00320             pthread_mutex_lock(&actionQueueMutex);
00321             for (std::set<FCam::Action*>::const_iterator i = req->shot().actions().begin();
00322                  i != req->shot().actions().end();
00323                  i++) {
00324                 Action a;
00325                 a.time = req->exposureStartTime + (*i)->time - (*i)->latency;
00326                 a.action = (*i)->copy();
00327                 actionQueue.push(a);
00328             }
00329             pthread_mutex_unlock(&actionQueueMutex);
00330             for (size_t i = 0; i < req->shot().actions().size(); i++) {
00331                 sem_post(&actionQueueSemaphore);
00332             }
00333 
00334             // The setter is done with this frame. Push it into the
00335             // in-flight queue for the handler to deal with.
00336             inFlightQueue.push(req);
00337             req = NULL;
00338         } else {
00339             // a bubble!
00340             lastReadout = (current.image.height() > 1008) ? 76000 : 33000;
00341         }
00342 
00343         // grab a new request and set an appropriate exposure and
00344         // frame time for it make sure there's a request ready for us
00345         if (!requestQueue.size()) {
00346             sensor->generateRequest();
00347         }
00348 
00349         // Peek ahead into the request queue to see what request we're
00350         // going to be handling next
00351         if (requestQueue.size()) {
00352             dprintf(4, "Setter: grabbing next request\n");
00353             // There's a real request for us to handle
00354             req = requestQueue.front();
00355             // Keep track of the last good shot parameters so we can set those on exit
00356             // since bubbles will clobber the V4L2 state on shutdown otherwise.
00357             // This allows us to be nice to other programs hoping to use our last settings.
00358             if (req->_shot.wanted == true) {
00359                 lastGoodShot.wanted = true;
00360                 lastGoodShot.exposure = req->_shot.exposure;
00361                 lastGoodShot.gain = req->_shot.gain;
00362                 lastGoodShot.frameTime = req->_shot.frameTime;
00363             }
00364         } else {
00365             dprintf(4, "Setter: inserting a bubble\n");
00366             // There are no pending requests, push a bubble into the
00367             // pipeline. The default parameters for a frame work nicely as
00368             // a bubble (as short as possible, no stats generated, output
00369             // unwanted).
00370             req = new _Frame;
00371             req->_shot.wanted = false;
00372 
00373             // bubbles should just run at whatever resolution is going. If
00374             // fast mode switches were possible, it might be nice to run
00375             // at the minimum resolution to go even faster, but they're
00376             // not.
00377             req->_shot.image = Image(current._shot.image.size(), current._shot.image.type(), Image::Discard);
00378 
00379             // generate histograms and sharpness maps if they're going,
00380             // but drop the data.
00381             req->_shot.histogram  = current._shot.histogram;
00382             req->_shot.sharpness  = current._shot.sharpness;
00383 
00384             // push the bubble into the pipe
00385             requestQueue.pushFront(req);
00386         }
00387 
00388         // Check if the next request requires a mode switch
00389         if (req->shot().image.size() != current._shot.image.size() ||
00390             req->shot().image.type() != current._shot.image.type() ||
00391             req->shot().histogram  != current._shot.histogram  ||
00392             req->shot().sharpness  != current._shot.sharpness) {
00393 
00394             // flush the pipeline
00395             dprintf(3, "Setter: Mode switch required - flushing pipe\n");
00396             pipelineFlush = true;
00397 
00398             pthread_mutex_lock(&cameraMutex);
00399             dprintf(3, "Setter: Handler done flushing pipe, passing control back to setter\n");
00400         
00401             // do the mode switch
00402         
00403             if (current.image.width() > 0) {
00404                 dprintf(3, "Setter: Shutting down camera\n");
00405                 v4l2Sensor->stopStreaming();
00406                 v4l2Sensor->close();
00407             }
00408             dprintf(3, "Setter: Starting up camera in new mode\n");
00409             v4l2Sensor->open();
00410         
00411             // set all the params for the new frame
00412             V4L2Sensor::Mode m;
00413             m.width  = req->shot().image.width();
00414             m.height = req->shot().image.height();
00415             // enforce UYVY or RAW
00416             m.type   = req->shot().image.type() == RAW ? RAW : UYVY;
00417             v4l2Sensor->startStreaming(m, req->shot().histogram, req->shot().sharpness);
00418             v4l2Sensor->setFrameTime(0);
00419         
00420             dprintf(3, "Setter: Setter done bringing up camera, passing control back "
00421                     "to handler. Expect two mystery frames.\n");
00422             pipelineFlush = false;
00423             pthread_mutex_unlock(&cameraMutex);
00424         
00425             m = v4l2Sensor->getMode();
00426             // Set destination image to new mode settings
00427             req->image = Image(m.width, m.height, m.type, Image::Discard);
00428         
00429             current._shot.image = Image(req->shot().image.size(), req->shot().image.type(), Image::Discard);
00430             current._shot.histogram  = req->shot().histogram;
00431             current._shot.sharpness  = req->shot().sharpness;
00432             
00433             // make sure we set everything else for the next frame,
00434             // because restarting streaming will have nuked it
00435             current._shot.frameTime = -1;
00436             current._shot.exposure = -1;
00437             current._shot.gain = -1.0;
00438             current._shot.whiteBalance = -1;
00439             current.image = Image(m.width, m.height, m.type, Image::Discard);
00440          
00441             req = NULL;
00442         
00443             // Wait for the second HS_VS before proceeding
00444             ignoreNextHSVS = true;
00445         
00446             return;
00447         } else {
00448             // no mode switch required
00449         }
00450 
00451         // pop the request 
00452         requestQueue.pop(); 
00453 
00454         Time next = hs_vs + current.frameTime;
00455         dprintf(3, "The current %d x %d frame has a frametime of %d\n", 
00456                 current.image.width(), current.image.height(), current.frameTime);
00457         dprintf(3, "Predicting that the next HS_VS will be at %d %d\n",
00458                 next.s(), next.us());
00459         
00460         int exposure = req->shot().exposure;
00461         int frameTime = req->shot().frameTime; 
00462 
00463         if (frameTime < exposure + 400) {
00464             frameTime = exposure + 400;
00465         }
00466 
00467         dprintf(4, "Setter: setting frametime and exposure\n");
00468         // Set the exposure
00469         v4l2Sensor->setFrameTime(frameTime);
00470         v4l2Sensor->setExposure(exposure);
00471     
00472         // Tag the request with the actual params. Also store them in
00473         // current to avoid unnecessary I2C.
00474         current._shot.frameTime = frameTime;
00475         current._shot.exposure  = exposure;
00476         current.exposure  = req->exposure  = v4l2Sensor->getExposure();
00477         current.frameTime = req->frameTime = v4l2Sensor->getFrameTime();
00478         req->image = current.image;
00479 
00480         // Exposure and frame time are set. Return and wait for the next
00481         // HS_VS before setting gain for this request-> (and pulling the next request).
00482     
00483         dprintf(4, "Setter: Done with this HS_VS, waiting for the next one\n");
00484 
00485     }
00486 
00487 
00488     void Daemon::runHandler() {
00489         _Frame *req = NULL;       
00490         V4L2Sensor::V4L2Frame *f = NULL;
00491 
00492         pthread_mutex_lock(&cameraMutex);
00493 
00494         while (!stop) {
00495 
00496             // the setter may be waiting for me to finish processing
00497             // outstanding requests
00498             if (!req && pipelineFlush && inFlightQueue.size() == 0) {        
00499                 dprintf(3, "Handler: Handler done flushing pipe, passing control back to setter\n");
00500                 while (pipelineFlush) {
00501                     pthread_mutex_unlock(&cameraMutex);
00502                     // let the setter grab the mutex. It has higher priority,
00503                     // so it should happen instantly. We put this in a while
00504                     // loop just to be sure.
00505                     usleep(10000);
00506                     pthread_mutex_lock(&cameraMutex);
00507                 }
00508                 dprintf(3, "Handler: Setter done bringing up camera, passing control back to handler\n");
00509             
00510             }
00511         
00512             if (pipelineFlush) {
00513                 dprintf(3, "Handler: Setter would like me to flush the pipeline, but I have requests in flight\n");
00514             }
00515         
00516             // wait for a frame
00517             if (!f)
00518                 f = v4l2Sensor->acquireFrame(true);
00519 
00520             if (!f) {                
00521                 error(Event::InternalError, "Handler got a NULL frame\n");
00522                 usleep(300000);
00523                 continue;
00524             } 
00525         
00526             // grab a request to match to it
00527             if (!req) {
00528                 if (inFlightQueue.size()) {
00529                     dprintf(4, "Handler: popping a frame request\n");
00530                     req = inFlightQueue.pull();
00531                 } else {
00532                     // there's no request for this frame - probably coming up
00533                     // from a mode switch or starting up
00534                     dprintf(3, "Handler: Got a frame without an outstanding request,"
00535                             " dropping it.\n");
00536                     v4l2Sensor->releaseFrame(f);
00537                     f = NULL;
00538                     continue;
00539                 }
00540             }
00541         
00542             // at this point we have a frame and a request, now look at
00543             // the time delta between them to see if they're a match
00544             int dt = req->processingDoneTime - f->processingDoneTime;
00545         
00546             dprintf(4, "Handler dt = %d\n", dt);
00547         
00548             if (dt < -25000) { // more than 25 ms late
00549                 dprintf(3, "Handler: Expected a frame at %d %d, but one didn't arrive until %d %d\n",
00550                         req->processingDoneTime.s(), req->processingDoneTime.us(),
00551                         f->processingDoneTime.s(), f->processingDoneTime.us());
00552                 error(Event::ImageDroppedError, sensor,
00553                       "Expected image data not returned from V4L2. Likely caused by excess"
00554                       " page faults (thrashing).");
00555                 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00556                 if (!req->shot().wanted) {
00557                     delete req;
00558                 } else {
00559                     // the histogram and sharpness map may still have appeared
00560                     req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime, req->shot().histogram);
00561                     req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime, req->shot().sharpness);
00562                     frameQueue.push(req);
00563                     enforceDropPolicy();
00564                 }
00565                 req = NULL;
00566             } else if (dt < 10000) {
00567                 // Is this frame wanted or a bubble?
00568                 if (!req->shot().wanted) {
00569                     // it's a bubble - drop it
00570                     dprintf(4, "Handler: discarding a bubble\n");
00571                     delete req;
00572                     v4l2Sensor->releaseFrame(f);
00573                 } else {
00574                 
00575                     // this looks like a match - bag and tag it
00576                     req->processingDoneTime = f->processingDoneTime;
00577 
00578                     size_t bytes = req->image.width()*req->image.height()*2;
00579                     if (f->length < bytes) bytes = f->length;
00580 
00581                     if (req->shot().image.autoAllocate()) {
00582                         req->image = Image(req->image.size(), req->image.type(), f->data).copy();
00583                     } else if (req->shot().image.discard()) {
00584                         req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00585                     } else {
00586                         if (req->image.size() != req->shot().image.size()) {
00587                             error(Event::ResolutionMismatch, sensor, 
00588                                   "Requested image size (%d x %d) "
00589                                   "on an already allocated image does not "
00590                                   "match actual image size (%d x %d). Dropping image data.",
00591                                   req->shot().image.width(), req->shot().image.height(),
00592                                   req->image.width(), req->image.height());
00593                             req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00594                             // TODO: crop instead?
00595                         } else if (req->image.type() != req->shot().image.type()) {
00596                             error(Event::FormatMismatch, sensor, 
00597                                   "Requested unsupported image format %d "
00598                                   "for an already allocated image.",
00599                                   req->shot().image.type());
00600                             req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00601                         } else { // the size matches
00602                             req->image = req->shot().image;
00603                             // figure out how long I can afford to wait
00604                             // For now, 10000 us should be safe
00605                             Time lockStart = Time::now();
00606                             if (req->image.lock(10000)) {
00607                                 req->image.copyFrom(Image(req->image.size(), req->image.type(), f->data));
00608                                 req->image.unlock();
00609                             } else {
00610                                 warning(Event::ImageTargetLocked, sensor,
00611                                         "Daemon discarding image data (target is still locked, "
00612                                         "waited for %d us)\n", Time::now() - lockStart);
00613                                 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00614                             }
00615                         }
00616                     }
00617 
00618                     v4l2Sensor->releaseFrame(f);
00619                     req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime, req->shot().histogram);
00620                     req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime, req->shot().sharpness);
00621                 
00622                     frameQueue.push(req);
00623                     enforceDropPolicy();
00624 
00625                 }
00626             
00627                 req = NULL;
00628                 f = NULL;
00629 
00630             } else { // more than 10ms early. Perhaps there was a mode switch.
00631                 dprintf(3, "Handler: Received an early mystery frame (%d %d) vs (%d %d), dropping it.\n",
00632                         f->processingDoneTime.s(), f->processingDoneTime.us(),
00633                         req->processingDoneTime.s(), req->processingDoneTime.us());
00634                 v4l2Sensor->releaseFrame(f);
00635                 f = NULL;
00636             }
00637         
00638         }
00639         pthread_mutex_unlock(&cameraMutex);
00640     
00641     }
00642 
00643 
00644     void Daemon::runAction() {
00645         dprintf(2, "Action thread running...\n");
00646         while (1) {       
00647             sem_wait(&actionQueueSemaphore);
00648             if (stop) break;
00649             // priority inversion :(
00650             pthread_mutex_lock(&actionQueueMutex);
00651             Action a = actionQueue.top();
00652             actionQueue.pop();
00653             pthread_mutex_unlock(&actionQueueMutex);
00654 
00655             Time t = Time::now();
00656             int delay = (a.time - t) - 500;
00657             if (delay > 0) usleep(delay);
00658             Time before = Time::now();
00659             // busy wait until go time
00660             while (a.time > before) before = Time::now();
00661             a.action->doAction();
00662             //Time after = Time::now();
00663             dprintf(3, "Action thread: Initiated action %d us after scheduled time\n", before - a.time);
00664             delete a.action;
00665         }
00666     }
00667 
00668 }}
00669  

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