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

src/F2/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 #include <string.h>
00008 #include <linux/videodev2.h>
00009 
00010 #include "FCam/Time.h"
00011 #include "FCam/Action.h"
00012 
00013 #include "Daemon.h"
00014 #include "../Debug.h"
00015 
00016 // local copy of kernel headers - keep in sync!
00017 #warning Do not forget to update the mt9p031.h header when changing the kernel!
00018 #include "linux/mt9p031.h"
00019 
00020 namespace FCam { namespace F2 {
00021 
00022     void *daemon_setter_thread_(void *arg) {
00023         Daemon *d = (Daemon *)arg;
00024         d->runSetter();    
00025         d->setterRunning = false;    
00026         pthread_exit(NULL);
00027     } 
00028 
00029     void *daemon_handler_thread_(void *arg) {
00030         Daemon *d = (Daemon *)arg;
00031         d->runHandler();
00032         d->handlerRunning = false;
00033         pthread_exit(NULL);    
00034     }
00035 
00036     void *daemon_action_thread_(void *arg) {
00037         Daemon *d = (Daemon *)arg;
00038         d->runAction();
00039         d->actionRunning = false;
00040         pthread_exit(NULL);
00041     }
00042 
00043     Daemon::Daemon(Sensor *_sensor) :
00044         sensor(_sensor),
00045         stop(false), 
00046         frameLimit(128),
00047         dropPolicy(FCam::Sensor::DropNewest),
00048         setterRunning(false), 
00049         handlerRunning(false), 
00050         actionRunning(false),
00051         waitingForFirstRequest(true),
00052         debugMode(false) {
00053 
00054         // tie ourselves to the correct sensor
00055         v4l2Sensor = V4L2Sensor::instance("/dev/video0");
00056 
00057         // launch the daemon thread with realtime priority
00058 
00059         // check I'm root
00060         if (getuid()) {
00061             printf("F2 camera daemon can only run as root. Aborting.\n");
00062             return;
00063         }
00064     
00065         // enable real-time scheduling modes
00066         FILE *f = fopen("/proc/sys/kernel/sched_rt_runtime_us", "w");
00067         if (f) {
00068             fprintf(f, "-1");
00069             fclose(f);
00070         } else {
00071             printf("Could not enable real-time scheduling modes, daemon thread creating may fail\n");
00072         }
00073 
00074         // make the mutexes for the producer-consumer queues
00075         if ((errno = 
00076              -(pthread_mutex_init(&actionQueueMutex, NULL) ||
00077                pthread_mutex_init(&cameraMutex, NULL)))) {
00078             perror("Error creating mutexes");
00079         }
00080     
00081         // make the semaphores
00082         sem_init(&actionQueueSemaphore, 0, 0);
00083 
00084         // make the semaphore for pipeline flushes
00085         pipelineFlush = true;
00086     
00087     }
00088 
00089     void Daemon::launchThreads() {
00090 
00091         pthread_attr_t attr;
00092         struct sched_param param;
00093 
00094         // make the setter thread
00095 
00096         param.sched_priority = sched_get_priority_min(SCHED_FIFO)+1;
00097 
00098         pthread_attr_init(&attr);
00099 
00100         if ((errno =
00101              -(pthread_attr_setschedparam(&attr, &param) ||
00102                pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00103                pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00104                pthread_create(&setterThread, &attr, daemon_setter_thread_, this)))) {
00105             perror("Error creating daemon setter thread");
00106             return;
00107         } else {
00108             setterRunning = true;
00109         }
00110 
00111         // make the handler thread
00112         param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00113 
00114         if ((errno =
00115              -(pthread_attr_setschedparam(&attr, &param) ||
00116                pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00117                pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00118                pthread_create(&handlerThread, &attr, daemon_handler_thread_, this)))) {
00119             perror("Error creating daemon handler thread");
00120             return;
00121         } else {
00122             handlerRunning = true;
00123         }
00124 
00125         // make the actions thread
00126         param.sched_priority = sched_get_priority_max(SCHED_FIFO);
00127 
00128         if ((errno =
00129              -(pthread_attr_setschedparam(&attr, &param) ||
00130                pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00131                pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00132                pthread_create(&actionThread, &attr, daemon_action_thread_, this)))) {
00133             perror("Error creating daemon action thread");
00134             return;
00135         } else {
00136             actionRunning = true;
00137         }
00138 
00139         pthread_attr_destroy(&attr);
00140     }
00141 
00142     Daemon::~Daemon() {
00143         stop = true;
00144 
00145         // post a wakeup call to the action thread
00146         sem_post(&actionQueueSemaphore);
00147 
00148         if (setterRunning) 
00149             pthread_join(setterThread, NULL);
00150     
00151         if (handlerRunning)
00152             pthread_join(handlerThread, NULL);
00153 
00154         if (actionRunning)
00155             pthread_join(actionThread, NULL);
00156 
00157         pthread_mutex_destroy(&actionQueueMutex);
00158         pthread_mutex_destroy(&cameraMutex);
00159 
00160         v4l2Sensor->close();
00161     }
00162 
00163     void Daemon::setDropPolicy(FCam::Sensor::DropPolicy p, int f) {
00164         dropPolicy = p;
00165         frameLimit = f;
00166         enforceDropPolicy();
00167     }
00168 
00169     void Daemon::enforceDropPolicy() {
00170         if (frameQueue.size() > frameLimit) {
00171             printf("WARNING: frame limit hit (%d), silently dropping %d frames.\n"
00172                    "You're not draining the frame queue quickly enough. Use longer \n"
00173                    "frame times or drain the frame queue until empty every time you \n"
00174                    "call getFrame()\n", frameLimit, frameQueue.size() - frameLimit);
00175             if (dropPolicy == FCam::Sensor::DropOldest) {
00176                 while (frameQueue.size() >= frameLimit) {
00177                     _Frame *f = frameQueue.pull();
00178                     delete f;
00179                 }
00180             } else if (dropPolicy == FCam::Sensor::DropNewest) {
00181                 while (frameQueue.size() >= frameLimit) {
00182                     _Frame *f = frameQueue.pullBack();
00183                     delete f;
00184                 }
00185             } else {
00186                 printf("Unknown drop policy! Not dropping frames.\n");
00187             }
00188         }
00189     
00190     }
00191 
00192     void Daemon::debugTiming(bool enable) {
00193         debugMode = enable;
00194     }
00195 
00196     void Daemon::runSetter() {
00197 
00198         // The first tick of the universe
00199         // Lyme disease doesn't even exist yet.
00200         tickSetter(Time::now());
00201         while (!stop) {
00202             //printf("Daemon ioctl hsvs wait\n");
00203             int err;
00204             timeval hs_vs_stamp = {0,0};
00205             v4l2_control ctrl;
00206             ctrl.id = MT9P031_CID_WAIT_HSVS_1;
00207             ctrl.value = 0; // Irrelevant for a get
00208         
00209             int currentFD = v4l2Sensor->getFD();
00210 
00211             Time now = Time::now();
00212             //printf("HSVS\n");
00213             /* Not using sensor->getControl() here because of 
00214                real possibility of timeout (error condition) */
00215             if (ioctl(currentFD, VIDIOC_G_CTRL, &ctrl) != 0) {
00216                 printf("HSVS error\n");
00217             } else {
00218                 if (ctrl.value == -1) {
00219                     printf("HSVS waiting timeout (probably)\n");
00220                     hs_vs_stamp.tv_sec = now.s();
00221                     hs_vs_stamp.tv_usec = now.us();
00222                 } else {
00223                     //printf("Sec: %d\n", ctrl.value);
00224                     hs_vs_stamp.tv_sec = ctrl.value;
00225             
00226                     ctrl.id = MT9P031_CID_WAIT_HSVS_2;
00227                     err = ioctl(currentFD, VIDIOC_G_CTRL, &ctrl);
00228                     if (err != 0) {
00229                         perror ("Error reading HSVS_2");
00230                     } else {                
00231                         hs_vs_stamp.tv_usec = ctrl.value;
00232                     }
00233                 }
00234                 //printf("USec: %d\n", ctrl.value);
00235                 //printf("Now: %d %d, DeltaT HSVS: %d usec\n", now.s(), now.us(), Time(hs_vs_stamp)-now);
00236                 tickSetter(Time(hs_vs_stamp));          
00237             }
00238         }
00239     
00240     }
00241 
00242     void Daemon::setReadoutParams(_Frame *req, bool modeSwitch) {
00243 
00244         bool updatedParameters = false;
00245 
00246         // Do atomic write of all the above 
00247         if (updatedParameters) 
00248             v4l2Sensor->setControl(MT9P031_CID_WRITE_PARAMS, 0);
00249 
00250     }
00251 
00252     void Daemon::setTimes(_Frame *req, const Time &hs_vs, bool modeSwitch) {
00253         // how long will the previous frame take to readout
00254         static int lastReadout = 0; //30000;   
00255         static bool firstFrame = true;
00256         int modeSwitchLag;
00257 
00258         if (req) {
00259 
00260             if (firstFrame) {
00261                 modeSwitchLag = req->frameTime;
00262                 firstFrame = false;
00263             } else {
00264                 modeSwitchLag = modeSwitch ? 20000 + req->frameTime : 0;   
00265             }
00266 
00267             // Predict when this frame will be done. It should be HS_VS +
00268             // the readout time for the previous frame + the frame time
00269             // for this request + however long the ISP takes to process
00270             // this frame.
00271             req->processingDoneTime = hs_vs + modeSwitchLag;
00272         
00273             // first there's the readout time for the previous frame
00274             req->processingDoneTime += lastReadout;
00275         
00276             // then there's the time to expose and read out this frame
00277             req->processingDoneTime += req->frameTime;
00278         
00279             // then there's some significant time inside the ISP if it's YUV and large
00280             // (this may be N900 specific)
00281             int ispTime = 0;
00282             if (req->image.type() == UYVY) {
00283                 if (req->image.height() > 1024 && req->image.width() > 1024) {
00284                     ispTime = 60000;
00285                 }
00286             }
00287             req->processingDoneTime += ispTime;            
00288         
00289             // Also compute when the exposure starts and finishes
00290             req->exposureStartTime = hs_vs + modeSwitchLag 
00291                 + req->frameTime - req->exposure;
00292 
00293             // Now update the readout time for this frame. This formula
00294             // is specific to the mt9p031 sensor on the F2.
00295             if (req->shot().colSkip != prevShot.colSkip ||
00296                 req->shot().colBin != prevShot.colBin ||
00297                 req->shot().rowSkip != prevShot.rowSkip ||
00298                 req->shot().rowBin != prevShot.rowBin) {
00299                 if (req->shot().colSkip > prevShot.colSkip) {
00300                     //lastReadout = 35000;
00301                     //lastReadout = 15000;
00302                     //printf("Increasing skipping, lr: %d\n", lastReadout);
00303                 } else {
00304                     //lastReadout = 35000;
00305                     //printf("Decreasing skipping, lr: %d\n", lastReadout);
00306                 }
00307             } else {
00308                 //lastReadout = 0; //(current.image.size.height > 1008) ? 76000 : 33000;
00309             }
00310             //lastReadout = 30000;
00311 
00312             req->exposureEndTime  = req->exposureStartTime + req->exposure + lastReadout;
00313 
00314             dprintf(2,"  Predicted time taken to read out previous frame: %d\n", lastReadout);
00315             dprintf(2,"  Predicted time to expose and read out this frame: %d\n", req->frameTime);
00316             dprintf(2,"  Predicted time to take in the ISP: %d\n", ispTime);
00317             dprintf(2,"  Predicted processingDone: %d %d\n", req->processingDoneTime.s(), req->processingDoneTime.us());
00318             dprintf(2,"  Predicted exposureEndTime: %d %d\n", req->exposureEndTime.s(), req->exposureEndTime.us());
00319         
00320 
00321         } else {
00322             // a bubble!
00323             //lastReadout = (current.image.size.height > 1008) ? 76000 : 33000;
00324             //lastReadout = 0;
00325         }
00326     }
00327 
00328     void Daemon::setExposureParams(_Frame *req, bool modeSwitch) {
00329         bool updatedParameters = false;
00330 
00331         // Set ROI parameters
00332         if (req->shot().rowSkip != current._shot.rowSkip || modeSwitch){
00333             v4l2Sensor->setControl(MT9P031_CID_ROW_SKIPPING, rowSkipDriver(req->shot().rowSkip) );
00334             current._shot.rowSkip = req->shot().rowSkip;
00335             updatedParameters = true;
00336         }
00337         if (req->shot().colSkip != current._shot.colSkip || modeSwitch){
00338             v4l2Sensor->setControl(MT9P031_CID_COL_SKIPPING, colSkipDriver(req->shot().colSkip) );
00339             current._shot.colSkip = req->shot().colSkip;
00340             updatedParameters = true;
00341         }
00342         if (req->shot().rowBin != current._shot.rowBin || modeSwitch){
00343             v4l2Sensor->setControl(MT9P031_CID_ROW_BINNING, rowBinDriver(req->shot().rowBin) );
00344             current._shot.rowBin = req->shot().rowBin;
00345             updatedParameters = true;
00346         }
00347         if (req->shot().colBin != current._shot.colBin || modeSwitch){
00348             v4l2Sensor->setControl(MT9P031_CID_COL_BINNING, colBinDriver(req->shot().colBin) );
00349             current._shot.colBin = req->shot().colBin;
00350             updatedParameters = true;
00351         }
00352         if (req->shot().roiCentered ) {
00353             if (!current._shot.roiCentered || modeSwitch) {
00354                 v4l2Sensor->setControl(MT9P031_CID_ROI_X, 
00355                                    MT9P031_ROI_AUTO_X);
00356                 v4l2Sensor->setControl(MT9P031_CID_ROI_Y, 
00357                                    MT9P031_ROI_AUTO_Y);
00358                 updatedParameters = true;
00359             }
00360         } else if (current._shot.roiCentered 
00361                    || req->shot().roiStartX != current._shot.roiStartX 
00362                    || req->shot().roiStartY != current._shot.roiStartY
00363                    || modeSwitch) {
00364             v4l2Sensor->setControl(MT9P031_CID_ROI_X,
00365                                req->shot().roiStartX);
00366             v4l2Sensor->setControl(MT9P031_CID_ROI_Y,
00367                                req->shot().roiStartY);
00368             updatedParameters = true;
00369         }
00370         current._shot.roiCentered = req->shot().roiCentered;
00371         current._shot.roiStartX = req->shot().roiStartX;
00372         current._shot.roiStartY = req->shot().roiStartY;
00373 
00374         // Set the exposure and frame time
00375         if (req->shot().frameTime != current._shot.frameTime
00376             || modeSwitch) {
00377             dprintf(2,"new ft: %d\n", req->shot().frameTime);
00378             v4l2Sensor->setFrameTime(req->shot().frameTime);
00379             current._shot.frameTime = req->shot().frameTime;
00380             updatedParameters = true;
00381         }
00382         if (req->shot().exposure != current._shot.exposure
00383             || modeSwitch) {
00384             dprintf(2,"new exposure: %d\n", req->shot().exposure);
00385             v4l2Sensor->setExposure(req->shot().exposure);
00386             current._shot.exposure  = req->shot().exposure;
00387             updatedParameters = true;
00388         }
00389 
00390         // Set the gain
00391 
00392         if (req->shot().gain != current._shot.gain || modeSwitch) {
00393             v4l2Sensor->setGain(req->shot().gain);
00394             current._shot.gain = req->shot().gain;     
00395             updatedParameters = true;
00396         }
00397 
00398 
00399         // Send the atomic setting write request
00400         if (updatedParameters)
00401             v4l2Sensor->setControl(MT9P031_CID_WRITE_PARAMS, 0);
00402     
00403         // Tag the request with the actual params. Also store them in
00404         // current to avoid unnecessary ioctls (gets never cause I2C).
00405         current.gain = req->gain = v4l2Sensor->getGain();
00406 
00407         current.exposure  = req->exposure  = v4l2Sensor->getExposure();
00408         current.frameTime = req->frameTime = v4l2Sensor->getFrameTime();
00409 
00410         dprintf(2,"actual exposure: %d\n", req->exposure);
00411         dprintf(2,"actual ft: %d\n", req->frameTime);
00412 
00413         current.rowSkip = req->rowSkip = 
00414             rowSkipFCam(v4l2Sensor->getControl(MT9P031_CID_ROW_SKIPPING));
00415         current.colSkip = req->colSkip = 
00416             colSkipFCam(v4l2Sensor->getControl(MT9P031_CID_COL_SKIPPING));
00417         current.rowBin = req->rowBin = 
00418             rowBinFCam(v4l2Sensor->getControl(MT9P031_CID_ROW_BINNING));
00419         current.colBin = req->colBin = 
00420             colBinFCam(v4l2Sensor->getControl(MT9P031_CID_COL_BINNING));
00421         current.roiStartX = req->roiStartX =
00422             v4l2Sensor->getControl(MT9P031_CID_ROI_X);
00423         current.roiStartX = req->roiStartY =
00424             v4l2Sensor->getControl(MT9P031_CID_ROI_Y);
00425 
00426 
00427         //    printf("  tickSetter took %d us\n", after2-after);
00428 
00429         //printf("Rs: %d, expected ft %d\n", current.rowSkip, current.frameTime);
00430     }
00431 
00432 
00433     void Daemon::tickSetter(Time hs_vs) {
00434         static _Frame *req = NULL;
00435         static Time prev_hs_vs;
00436         static bool wasModeswitch = false;
00437 
00438 
00439         //printf("HSVS %d %d, delta %.3f ms\n", hs_vs.s(), hs_vs.us(), (hs_vs-prev_hs_vs)/1000.0);
00440     repeat_modeswitch:
00441         prev_hs_vs = hs_vs;
00442 
00444         // Step 1 - finish dealing with second half of last request (final parameters, actions)
00445 
00446         usleep(1000); // Get past real HSVS
00447 
00448         // The MT9P031 apparently needs all its parameters set at the same time. This isn't per spec,
00449         // but does appear to work.  
00450         if (req) {
00451             //printf("TS: Setting sensor params\n");        
00452         }
00453 
00454         // setTimes must be done after setExposureParams, or old time estimates will be used
00455         setTimes(req, hs_vs, wasModeswitch);
00456         wasModeswitch = false;
00457 
00458         if (req) {
00459             dprintf(1,"TS: Pushing to inflight\n");
00460             // set the sensor readout parameters and predicted done time on the pending request
00461             // and then push it onto the handler's input queue and the v4l2 buffer queue       
00462             //setReadoutParams(req);
00463 
00464             // now queue up this request's actions
00465             pthread_mutex_lock(&actionQueueMutex);
00466             for (std::set<FCam::Action*>::iterator i = req->shot().actions.begin();
00467                  i != req->shot().actions.end();
00468                  i++) {
00469                 Action a;
00470                 a.time = req->exposureStartTime + (*i)->time - (*i)->latency;
00471                 a.action = (*i)->copy();
00472                 actionQueue.push(a);
00473             }
00474             pthread_mutex_unlock(&actionQueueMutex);
00475             for (size_t i = 0; i < req->shot().actions.size(); i++) {
00476                 sem_post(&actionQueueSemaphore);
00477             }
00478 
00479             inFlightQueue.push(req);
00480             req = NULL;
00481         } else {
00482             // an unpredictable bubble, so do nothing
00483         }
00484 
00486         // Step 2 - Start processing next request (exposure, frame time, mode switches)
00487         // If the queue is empty, try to generate some requests from a stream
00488         if (!requestQueue.size()) {
00489             dprintf(1,"TS: Asking the sensor to generate a request\n");
00490             sensor->generateRequest();
00491         }
00492 
00493         if (requestQueue.size()) {
00494             dprintf(1,"TS: Grabbing next request \n");
00495             // peek at the next request
00496             req = requestQueue.front();
00497         } else {
00498             // There are no pending requests, push a bubble into the
00499             // pipeline. The default parameters for a frame work nicely as
00500             // a bubble (as short as possible, no stats generated), except 
00501             // that it should be unwanted.
00502             dprintf(1,"TS: Creating a bubble request\n");
00503             _Frame *req = new _Frame;
00504 
00505             req->_shot.wanted = false;
00506 
00507             // bubbles should just run at whatever resolution is going. If
00508             // fast mode switches were possible, it might be nice to run
00509             // at the minimum resolution to go even faster, but they're
00510             // not.
00511             req->_shot.image = Image(current._shot.image.size(), current._shot.image.type(), Image::Discard);
00512 
00513             // Temporary hack - keep the same exposure to keep everything flowing
00514             // smoothly.  This is bad for long exposure bubbles!
00515             req->_shot.exposure = current._shot.exposure;
00516 
00517             // generate histograms and sharpness maps if they're going,
00518             // but drop the data.
00519             req->_shot.histogram  = current._shot.histogram;
00520             req->_shot.sharpness  = current._shot.sharpness;
00521 
00522             // push the bubble into the pipe
00523             requestQueue.push(req);
00524         }
00525         prevShot = current._shot; // Keep around the settings for one more go-around
00526 
00527         //printf("TS: Checking mode switch\n");
00528         // Check if the next request requires a mode switch
00529         if (req->shot().image.size() != current._shot.image.size() ||
00530             req->shot().image.type() != current._shot.image.type() ||
00531             req->shot().histogram  != current._shot.histogram  ||
00532             req->shot().sharpness  != current._shot.sharpness) {
00533 
00534             printf("TS:   Mode Requested %d %d\n", req->shot().image.width(), req->shot().image.height());
00535             printf("TS:   Mode Current %d %d\n", current._shot.image.width(), current._shot.image.height());
00536         
00537             // flush the pipeline
00538             dprintf(1,"Setter: Mode switch required - flushing pipe\n");
00539             pipelineFlush = true;
00540 
00541             pthread_mutex_lock(&cameraMutex);
00542             dprintf(1,"Setter: Handler done flushing pipe, passing control back to setter\n");
00543         
00544             // do the mode switch
00545         
00546             if (current.image.width() > 0) {
00547                 printf("Setter: Stopping camera for mode switch\n");
00548                 v4l2Sensor->stopStreaming();
00549                 v4l2Sensor->close();
00550                 v4l2Sensor->open();
00551             } else {
00552                 printf("Setter: Opening camera for the first time\n");
00553                 v4l2Sensor->open();
00554             }
00555 
00556             // Ensure we're in direct control mode (sensor doesn't auto-update parameters on frame bounds)
00557             v4l2Sensor->setControl(MT9P031_CID_DIRECT_MODE, 1);
00558         
00559             // Set parameters before starting stream
00560             setReadoutParams(req, true);
00561             setExposureParams(req, true);
00562 
00563             // set all the params for the new frame
00564             V4L2Sensor::Mode m;
00565             m.width  = req->shot().image.width();
00566             m.height = req->shot().image.height();
00567             m.type   = req->shot().image.type();
00568         
00569             v4l2Sensor->startStreaming(m, req->shot().histogram, req->shot().sharpness);
00570 
00571             /* Read this again because we actually told the sensor to switch modes,
00572                and only now does it know its new resolution */
00573             current.frameTime = req->frameTime = v4l2Sensor->getFrameTime();
00574             dprintf(1,"Setter: First frame time %f ms\n", current.frameTime/1000.);
00575 
00576             hs_vs = Time::now();
00577             dprintf(1,"Setter: Setter done bringing up camera, passing control back to handler\n");
00578             pipelineFlush = false;
00579             pthread_mutex_unlock(&cameraMutex);
00580         
00581             m = v4l2Sensor->getMode();
00582             req->image = Image(m.width, m.height, m.type, Image::Discard);
00583         
00584             current._shot.image = req->shot().image;
00585             current._shot.histogram  = req->shot().histogram;
00586             current._shot.sharpness  = req->shot().sharpness;
00587 
00588             current.image = req->image;
00589 
00590             dprintf(2,"TS: Popping mode switch request\n");
00591             // pop the request      
00592             requestQueue.pop();
00593 
00594             dprintf(1,"TS: Mode switch done at %d %d, delta %f ms\n", hs_vs.s(), hs_vs.us(), (hs_vs-prev_hs_vs)/1000.0);
00595             //hs_vs += req->frame_time;
00596             wasModeswitch = true;
00597             goto repeat_modeswitch;
00598         } 
00599     
00600         dprintf(2,"Setter: no mode switch required\n");
00601         Time b = Time::now();
00602         setReadoutParams(req);
00603         setExposureParams(req);
00604         Time a = Time::now();
00605         dprintf(2,"    Time to set %f ms\n", (a - b) / 1000.0);
00606 
00607         dprintf(2,"TS: Popping request\n");
00608         // pop the request 
00609         requestQueue.pop();
00610 
00611         req->image = current.image;
00612         //req->histogram = req->shot().histogram;
00613         //req->sharpness = req->shot().sharpness;
00614 
00615         // Exposure and frame time are set. Return and wait for the next
00616         // HS_VS before setting readout parameters for this request. (and pulling the next request).
00617         //usleep(500);
00618 
00619     }
00620 
00621 
00622     void Daemon::runHandler() {
00623         _Frame *req = NULL;
00624         V4L2Sensor::V4L2Frame *f = NULL;
00625         Time prevDoneTime = Time::now();
00626 
00627         pthread_mutex_lock(&cameraMutex);
00628 
00629         while (!stop) {
00630 
00631             // the setter may be waiting for me to finish processing
00632             // outstanding requests
00633             if (!req && pipelineFlush && inFlightQueue.size() == 0) { 
00634                 printf("Handler: Handler done flushing pipe, passing control back to setter\n");
00635                 while (pipelineFlush) {
00636                     pthread_mutex_unlock(&cameraMutex);
00637                     // let the setter grab the mutex. It has higher priority,
00638                     // so it should happen instantly. We put this in a while
00639                     // loop just to be sure.
00640                     usleep(10000);
00641                     pthread_mutex_lock(&cameraMutex);
00642                 }
00643                 printf("Handler: Setter done bringing up camera, passing control back to handler\n");
00644             
00645             }
00646         
00647             if (pipelineFlush) {
00648                 printf("Handler: Setter would like me to flush the pipeline, but I have requests in flight\n");
00649             }
00650         
00651             // wait for a frame
00652             if (!f) {
00653                 f = v4l2Sensor->acquireFrame(true);
00654             }
00655 
00656             if (!f) {
00657                 printf("Handler got a NULL frame\n");
00658                 usleep(10000);
00659                 continue;
00660             } 
00661 
00662             dprintf(2,"Handler: frame %d %d, delta %f\n",
00663                     f->processingDoneTime.s(), f->processingDoneTime.us(),
00664                     (f->processingDoneTime-prevDoneTime)/1000.);
00665             prevDoneTime = f->processingDoneTime;
00666 
00667             // grab a request to match to it
00668             if (!req) {
00669                 if (inFlightQueue.size()) {
00670                     //.printf("Handler: popping a frame request\n");
00671                     req = inFlightQueue.pull();
00672                 } else {
00673                     // there's no request for this frame - probably coming up
00674                     // from a mode switch or starting up
00675                     //printf("Handler: Got a frame without an outstanding request. Waiting some for one.\n");
00676                     int timeout = 5;
00677                     while (timeout > 0 && (inFlightQueue.size()==0) ) {
00678                         timeout--;
00679                         usleep(1000);
00680                     }
00681                     if (timeout > 0) {
00682                         req = inFlightQueue.pull();
00683                     } else {
00684                         printf("  Giving up on waiting for request\n");
00685                         v4l2Sensor->releaseFrame(f);
00686                         f = NULL;
00687                         continue;
00688                     }
00689                 }
00690             }
00691 
00692             dprintf(1,"Handler:     Expected at %d %d, delta %f ms\n", 
00693                     req->processingDoneTime.s(), req->processingDoneTime.us(),
00694                     (f->processingDoneTime - req->processingDoneTime)/1000.);
00695         
00696             // at this point we have a frame and a request, now look at
00697             // the time delta between them to see if they're a match
00698             int dt = req->processingDoneTime - f->processingDoneTime;
00699 
00700             if (dt < -25000 && !debugMode) { // more than 25 ms late
00701                 dprintf(0,"Handler: Expected a frame at %d %d, but one didn't arrive until %d %d, dt %f ms\n",
00702                        req->processingDoneTime.s(), req->processingDoneTime.us(),
00703                        f->processingDoneTime.s(), f->processingDoneTime.us(),
00704                        dt/1000.0);
00705                 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00706                 if (!req->shot().wanted) {
00707                     /* Get rid of bubble allocated by setter */
00708                     delete req;
00709                 } else {
00710                     // the histogram and sharpness map may still have appeared
00711                     req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime-2300, 
00712                                                               req->shot().histogram);
00713                     req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime-2300,
00714                                                                  req->shot().sharpness);
00715 
00716                     frameQueue.push(req);
00717                     enforceDropPolicy();
00718                 }
00719                 req = NULL;
00720             } else if (dt < 15000 || debugMode) { // In debugMode, accept all comers
00721                 // Is this frame wanted or a bubble?
00722                 if (!req->shot().wanted) {
00723                     // it's a bubble - drop it
00724                     //printf("Handler: discarding a bubble\n");
00725                     delete req;
00726                     v4l2Sensor->releaseFrame(f);
00727                 } else {
00728                 
00729                     // this looks like a match - bag and tag it
00730                     req->processingDoneTime = f->processingDoneTime;
00731 
00732                     size_t bytes = req->image.width()*req->image.height()*2;
00733                     if (f->length < bytes) bytes = f->length;
00734                 
00735                     if (req->shot().image.autoAllocate()) {
00736                         req->image = Image(req->image.size(), req->image.type(),f->data).copy();
00737                         dprintf(2,"Autoallocate: %d x %d, %d\n", 
00738                                 req->image.width(), req->image.height(), req->image(0,0));
00739                     } else if (req->shot().image.discard()) {
00740                         req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00741                     } else {
00742                         if (req->image.size() != req->shot().image.size()) {
00743                             dprintf(0,"ERROR: Requested image size (%d x %d) "
00744                                     "on an already allocated image does not "
00745                                     "match actual image size (%d x %d). Dropping image data.\n",
00746                                    req->shot().image.width(), req->shot().image.height(),
00747                                    req->image.width(), req->image.height());
00748                             req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00749                         } else { // the size matches
00750                             req->image = req->shot().image;
00751                             // Need to decide on a more dynamic timeout for lock() here,
00752                             // but 10ms shouldn't be too bad
00753                             if(req->image.lock(10000)) {
00754                                 req->image.copyFrom(Image(req->image.size(), req->image.type(),f->data));
00755                                 req->image.unlock();
00756                             }  else {
00757                                 dprintf(0,"WARNING: Daemon discarding image data (Target is locked)\n");
00758                                 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00759                             }
00760                         }
00761                     }
00762 
00763                     v4l2Sensor->releaseFrame(f);
00764                     req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime-2300, 
00765                                                               req->shot().histogram);
00766                     req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime-2300, 
00767                                                                  req->shot().sharpness);
00768 
00769                     frameQueue.push(req);
00770                     enforceDropPolicy();
00771 
00772                 
00773                     //              printf("Handler: finalized a request for a %d x %d frame\n", req->image.size.width, req->image.size.height);            
00774                 }
00775             
00776                 req = NULL;
00777                 f = NULL;
00778 
00779             } else { // more than 10ms early... something weird's going on
00780                 dprintf(0,"Handler: Received an early mystery frame (%d %d) vs (%d %d), dropping it.\n",
00781                        f->processingDoneTime.s(), f->processingDoneTime.us(),
00782                        req->processingDoneTime.s(), req->processingDoneTime.us());
00783                 v4l2Sensor->releaseFrame(f);
00784                 f = NULL;
00785             }
00786         
00787         }
00788         pthread_mutex_unlock(&cameraMutex);
00789     
00790     }
00791 
00792 
00793     void Daemon::runAction() {
00794         printf("Action thread running...\n");
00795         while (1) {       
00796             sem_wait(&actionQueueSemaphore);
00797             if (stop) break;
00798             // priority inversion :(
00799             pthread_mutex_lock(&actionQueueMutex);
00800             Action a = actionQueue.top();
00801             actionQueue.pop();
00802             pthread_mutex_unlock(&actionQueueMutex);
00803 
00804             Time t = Time::now();
00805             int delay = (a.time - t) - 500;
00806             if (delay > 0) usleep(delay);
00807             Time before = Time::now();
00808             // busy wait until go time
00809             while (a.time > before) before = Time::now();
00810             a.action->doAction();
00811             //Time after = Time::now();
00812             printf("Action thread: Initiated action %d us after scheduled time\n", before - a.time);
00813             delete a.action;
00814         }
00815     }
00816 
00817 } }

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