F2/Daemon.cpp

#include "Daemon.h"
#include <linux/videodev2.h>
// local copy of kernel header
#include "linux/mt9p031.h"

#warning Do not forget to update the mt9p031.h header when changing the kernel!

#include "../Time.h"
#include "../Action.h"

#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/ioctl.h>
#include <poll.h>
#include <string.h>

//#define DEBUG
#ifdef DEBUG
#define dprintf(...) printf(__VA__ARGS__)
#else
#define dprintf(...)
#endif

namespace FCam { namespace F2 {

void *daemon_setter_thread_(void *arg) {
    Daemon *d = (Daemon *)arg;
    d->runSetter();    
    d->setterRunning = false;    
    pthread_exit(NULL);
} 

void *daemon_handler_thread_(void *arg) {
    Daemon *d = (Daemon *)arg;
    d->runHandler();
    d->handlerRunning = false;
    pthread_exit(NULL);    
}

void *daemon_action_thread_(void *arg) {
    Daemon *d = (Daemon *)arg;
    d->runAction();
    d->actionRunning = false;
    pthread_exit(NULL);
}

Daemon::Daemon() :
    stop(false), 
    generator(NULL),
    frameLimit(128),
    dropPolicy(FCam::Sensor::DropNewest),
    setterRunning(false), 
    handlerRunning(false), 
    actionRunning(false),
    waitingForFirstRequest(true),
    debugMode(false) {

    // add a dummy shot to the current request
    current.f2Request = new Shot;

    // tie ourselves to the correct sensor
    sensor = V4L2Sensor::instance("/dev/video0");

    // launch the daemon thread with realtime priority

    // check I'm root
    if (getuid()) {
    printf("Camera Daemon can only run as root. Aborting.\n");
    return;
    }
    
    // enable real-time scheduling modes
    FILE *f = fopen("/proc/sys/kernel/sched_rt_runtime_us", "w");
    if (f) {
    fprintf(f, "-1");
    fclose(f);
    } else {
    printf("Could not enable real-time scheduling modes, daemon thread creating may fail\n");
    }

    // make the mutexes for the producer-consumer queues
    if ((errno = 
     -(pthread_mutex_init(&requestQueueMutex, NULL) ||
       pthread_mutex_init(&frameQueueMutex, NULL) ||
       pthread_mutex_init(&actionQueueMutex, NULL) ||
       pthread_mutex_init(&inFlightQueueMutex, NULL) ||
       pthread_mutex_init(&cameraMutex, NULL)))) {
    perror("Error creating mutexes");
    }
    
    // make the semaphores
    sem_init(&frameQueueSemaphore, 0, 0);
    sem_init(&actionQueueSemaphore, 0, 0);

    // make the semaphore for pipeline flushes
    pipelineFlush = true;
    
}

void Daemon::launchThreads() {

    pthread_attr_t attr;
    struct sched_param param;

    // make the setter thread

    param.sched_priority = sched_get_priority_min(SCHED_FIFO)+1;

    pthread_attr_init(&attr);

    if ((errno =
     -(pthread_attr_setschedparam(&attr, &param) ||
       pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
       pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
       pthread_create(&setterThread, &attr, daemon_setter_thread_, this)))) {
    perror("Error creating daemon setter thread");
    return;
    } else {
    setterRunning = true;
    }

    // make the handler thread
    param.sched_priority = sched_get_priority_min(SCHED_FIFO);

    if ((errno =
     -(pthread_attr_setschedparam(&attr, &param) ||
       pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
       pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
       pthread_create(&handlerThread, &attr, daemon_handler_thread_, this)))) {
    perror("Error creating daemon handler thread");
    return;
    } else {
    handlerRunning = true;
    }

    // make the actions thread
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);

    if ((errno =
     -(pthread_attr_setschedparam(&attr, &param) ||
       pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
       pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
       pthread_create(&actionThread, &attr, daemon_action_thread_, this)))) {
    perror("Error creating daemon action thread");
    return;
    } else {
    actionRunning = true;
    }

    pthread_attr_destroy(&attr);
}

Daemon::~Daemon() {
    stop = true;

    // post a wakeup call to the action thread
    sem_post(&actionQueueSemaphore);

    if (setterRunning) 
    pthread_join(setterThread, NULL);
    
    if (handlerRunning)
    pthread_join(handlerThread, NULL);

    if (actionRunning)
    pthread_join(actionThread, NULL);

    pthread_mutex_destroy(&requestQueueMutex);    
    pthread_mutex_destroy(&frameQueueMutex);
    pthread_mutex_destroy(&inFlightQueueMutex);
    pthread_mutex_destroy(&actionQueueMutex);

    sensor->close();
}


void Daemon::setRequestGenerator(Daemon::RequestGenerator *gen) {
    generator = gen;
    if (waitingForFirstRequest) {
    // get things started
    printf("Calling generator for the first time\n");
    generator->generateRequest();
    }
}

Daemon::RequestGenerator *Daemon::getRequestGenerator() {
    return generator;
}


int Daemon::requestFrame(Frame *req) {
    // enqueue the request
    pthread_mutex_lock(&requestQueueMutex);
    requestQueue.push(req);
    pthread_mutex_unlock(&requestQueueMutex);    

    if (waitingForFirstRequest) {
    printf("First frame requested\n");
    //tickSetter(Time::now());
    waitingForFirstRequest = false;
    launchThreads();
    }
    return 0;
}

Frame *Daemon::getNextFrame() {
    //.printf("frameQueue: %d\n", frameQueue.size());
    sem_wait(&frameQueueSemaphore);
    pthread_mutex_lock(&frameQueueMutex);
    Frame *f = frameQueue.front();
    frameQueue.pop();
    pthread_mutex_unlock(&frameQueueMutex);
    return f;
}

void Daemon::setDropPolicy(FCam::Sensor::DropPolicy p, int f) {
    dropPolicy = p;
    frameLimit = f;
    enforceDropPolicy();
}

void Daemon::enforceDropPolicy() {
    if (frameQueue.size() > frameLimit) {
    pthread_mutex_lock(&frameQueueMutex);
    int drops = frameQueue.size() - frameLimit;
    if (drops > 0) {
        printf("WARNING: frame limit hit (%d), silently dropping %d frames.\n"
           "You're not draining the frame queue quickly enough. Use longer \n"
           "frame times or drain the frame queue until empty every time you \n"
           "call getFrame()\n", frameLimit, drops);
        if (dropPolicy == FCam::Sensor::DropOldest) {
        for (int i = 0; i < drops; i++) {
            sem_wait(&frameQueueSemaphore);
            delete frameQueue.front();
            frameQueue.pop();
        }
        } else if (dropPolicy == FCam::Sensor::DropNewest) {
        // recycle the first bunch
        for (size_t i = 0; i < frameQueue.size() - drops; i++) {
            frameQueue.push(frameQueue.front());
            frameQueue.pop();
        }
        for (int i = 0; i < drops; i++) {
            sem_wait(&frameQueueSemaphore);
            delete frameQueue.front();
            frameQueue.pop();
        }
        } else {
        printf("Unknown drop policy! Not dropping frames.\n");
        }
    }
    pthread_mutex_unlock(&frameQueueMutex);
    }
}

void Daemon::debugTiming(bool enable) {
    debugMode = enable;
}

void Daemon::runSetter() {

    // The first tick of the universe
    // Lyme disease doesn't even exist yet.
    tickSetter(Time::now());
    while (!stop) {
    //printf("Daemon ioctl hsvs wait\n");
    int err;
    timeval hs_vs_stamp;
    v4l2_control ctrl;
    ctrl.id = MT9P031_CID_WAIT_HSVS_1;
    ctrl.value = 0; // Irrelevant for a get
    
    int currentFD = sensor->getFD();

    Time now = Time::now();
    //printf("HSVS\n");
    /* Not using sensor->getControl() here because of 
       real possibility of timeout (error condition */
    if (ioctl(currentFD, VIDIOC_G_CTRL, &ctrl) != 0) {
        printf("HSVS error\n");
    } else {
        if (ctrl.value == -1) {
        printf("HSVS waiting timeout (probably)\n");
        hs_vs_stamp.tv_sec = now.s();
        hs_vs_stamp.tv_usec = now.us();
        } else {
        //printf("Sec: %d\n", ctrl.value);
        hs_vs_stamp.tv_sec = ctrl.value;
        
        ctrl.id = MT9P031_CID_WAIT_HSVS_2;
        err = ioctl(currentFD, VIDIOC_G_CTRL, &ctrl);
        if (err != 0) {
            perror ("Error reading HSVS_2");
        } else {        
            hs_vs_stamp.tv_usec = ctrl.value;
        }
        }
        //printf("USec: %d\n", ctrl.value);
        //printf("Now: %d %d, DeltaT HSVS: %d usec\n", now.s(), now.us(), Time(hs_vs_stamp)-now);
        tickSetter(Time(hs_vs_stamp));      
    }
    }
    
}

void Daemon::setReadoutParams(Frame *req, bool modeSwitch) {

    bool updatedParameters = false;

    // Do atomic write of all the above 
    if (updatedParameters) 
    sensor->setControl(MT9P031_CID_WRITE_PARAMS, 0);

}

void Daemon::setTimes(Frame *req, const Time &hs_vs, bool modeSwitch) {
    // how long will the previous frame take to readout
    static int lastReadout = 0; //30000;   
    static bool firstFrame = true;
    int modeSwitchLag;

    if (req) {
    if (firstFrame) {
        modeSwitchLag = req->frameTime;
        firstFrame = false;
    } else {
        modeSwitchLag = modeSwitch ? 20000 + req->frameTime : 0;    
    }

    // Predict when this frame will be done. It should be HS_VS +
    // the readout time for the previous frame + the frame time
    // for this request + however long the ISP takes to process
    // this frame.
    req->processingDoneTime = hs_vs + modeSwitchLag;
    
    // first there's the readout time for the previous frame
    req->processingDoneTime += lastReadout;
    
    // then there's the time to expose and read out this frame
    req->processingDoneTime += req->frameTime;
    
    // then there's some significant time inside the ISP if it's YUV and large
    // (this may be N900 specific)
    int ispTime = 0;
    if (req->image.type == UYVY) {
        if (req->image.size.height > 1024 && req->image.size.width > 1024) {
        ispTime = 60000;
        }
    }
    req->processingDoneTime += ispTime;         
    
    // Also compute when the exposure starts and finishes
    req->exposureStartTime = hs_vs + modeSwitchLag 
        + req->frameTime - req->exposure;

    // Now update the readout time for this frame. This formula
    // is specific to the mt9p031 sensor on the F2.
    if (req->f2Request->colSkip != prevReq.colSkip ||
        req->f2Request->colBin != prevReq.colBin ||
        req->f2Request->rowSkip != prevReq.rowSkip ||
        req->f2Request->rowBin != prevReq.rowBin) {
        if (req->f2Request->colSkip > prevReq.colSkip) {
        //lastReadout = 35000;
        //lastReadout = 15000;
        //printf("Increasing skipping, lr: %d\n", lastReadout);
        } else {
        //lastReadout = 35000;
        //printf("Decreasing skipping, lr: %d\n", lastReadout);
        }
    } else {
        //lastReadout = 0; //(current.image.size.height > 1008) ? 76000 : 33000;
    }
    //lastReadout = 30000;

    req->exposureEndTime  = req->exposureStartTime + req->exposure + lastReadout;

    dprintf("  Predicted time taken to read out previous frame: %d\n", lastReadout);
    dprintf("  Predicted time to expose and read out this frame: %d\n", req->frameTime);
    dprintf("  Predicted time to take in the ISP: %d\n", ispTime);
    dprintf("  Predicted processingDone: %d %d\n", req->processingDoneTime.s(), req->processingDoneTime.us());
    dprintf("  Predicted exposureEndTime: %d %d\n", req->exposureEndTime.s(), req->exposureEndTime.us());
    

    } else {
    // a bubble!
    //lastReadout = (current.image.size.height > 1008) ? 76000 : 33000;
    //lastReadout = 0;
    }
}

void Daemon::setExposureParams(Frame *req, bool modeSwitch) {
    bool updatedParameters = false;

    // Set ROI parameters
    if (req->f2Request->rowSkip != current.f2Request->rowSkip || modeSwitch){
    sensor->setControl(MT9P031_CID_ROW_SKIPPING, rowSkipDriver(req->f2Request->rowSkip) );
    current.f2Request->rowSkip = req->f2Request->rowSkip;
    updatedParameters = true;
    }
    if (req->f2Request->colSkip != current.f2Request->colSkip || modeSwitch){
    sensor->setControl(MT9P031_CID_COL_SKIPPING, colSkipDriver(req->f2Request->colSkip) );
    current.f2Request->colSkip = req->f2Request->colSkip;
    updatedParameters = true;
    }
    if (req->f2Request->rowBin != current.f2Request->rowBin || modeSwitch){
    sensor->setControl(MT9P031_CID_ROW_BINNING, rowBinDriver(req->f2Request->rowBin) );
    current.f2Request->rowBin = req->f2Request->rowBin;
    updatedParameters = true;
    }
    if (req->f2Request->colBin != current.f2Request->colBin || modeSwitch){
    sensor->setControl(MT9P031_CID_COL_BINNING, colBinDriver(req->f2Request->colBin) );
    current.f2Request->colBin = req->f2Request->colBin;
    updatedParameters = true;
    }
    if (req->f2Request->roiCentered ) {
    if (!current.f2Request->roiCentered || modeSwitch) {
        sensor->setControl(MT9P031_CID_ROI_X, 
                   MT9P031_ROI_AUTO_X);
        sensor->setControl(MT9P031_CID_ROI_Y, 
                   MT9P031_ROI_AUTO_Y);
        updatedParameters = true;
    }
    } else if (current.f2Request->roiCentered 
           || req->f2Request->roiStartX != current.f2Request->roiStartX 
           || req->f2Request->roiStartY != current.f2Request->roiStartY
           || modeSwitch) {
    sensor->setControl(MT9P031_CID_ROI_X,
               req->f2Request->roiStartX);
    sensor->setControl(MT9P031_CID_ROI_Y,
               req->f2Request->roiStartY);
    updatedParameters = true;
    }
    current.f2Request->roiCentered = req->f2Request->roiCentered;
    current.f2Request->roiStartX = req->f2Request->roiStartX;
    current.f2Request->roiStartY = req->f2Request->roiStartY;

    // Set the exposure and frame time
    if (req->f2Request->frameTime != current.f2Request->frameTime
    || modeSwitch) {
    dprintf("new ft: %d\n", req->f2Request->frameTime);
    sensor->setFrameTime(req->f2Request->frameTime);
    current.f2Request->frameTime = req->f2Request->frameTime;
    updatedParameters = true;
    }
    if (req->f2Request->exposure != current.f2Request->exposure
    || modeSwitch) {
    dprintf("new exposure: %d\n", req->f2Request->exposure);
    sensor->setExposure(req->f2Request->exposure);
    current.f2Request->exposure  = req->f2Request->exposure;
    updatedParameters = true;
    }

    // Set the gain

    if (req->f2Request->gain != current.f2Request->gain || modeSwitch) {
    sensor->setGain(req->f2Request->gain);
    current.f2Request->gain = req->f2Request->gain;     
    updatedParameters = true;
    }


    // Send the atomic setting write request
    if (updatedParameters)
    sensor->setControl(MT9P031_CID_WRITE_PARAMS, 0);
    
    // Tag the request with the actual params. Also store them in
    // current to avoid unnecessary ioctls (gets never cause I2C).
    current.gain = req->gain = sensor->getGain();

    current.exposure  = req->exposure  = sensor->getExposure();
    current.frameTime = req->frameTime = sensor->getFrameTime();

    dprintf("actual exposure: %d\n", req->exposure);
    dprintf("actual ft: %d\n", req->frameTime);

    current.rowSkip = req->rowSkip = 
    rowSkipFCam(sensor->getControl(MT9P031_CID_ROW_SKIPPING));
    current.colSkip = req->colSkip = 
    colSkipFCam(sensor->getControl(MT9P031_CID_COL_SKIPPING));
    current.rowBin = req->rowBin = 
    rowBinFCam(sensor->getControl(MT9P031_CID_ROW_BINNING));
    current.colBin = req->colBin = 
    colBinFCam(sensor->getControl(MT9P031_CID_COL_BINNING));
    current.roiStartX = req->roiStartX =
    sensor->getControl(MT9P031_CID_ROI_X);
    current.roiStartX = req->roiStartY =
    sensor->getControl(MT9P031_CID_ROI_Y);


    //    printf("  tickSetter took %d us\n", after2-after);

    //printf("Rs: %d, expected ft %d\n", current.rowSkip, current.frameTime);
}


void Daemon::tickSetter(Time hs_vs) {
    static Frame *req = NULL;
    static Time prev_hs_vs;
    static bool wasModeswitch = false;


    //printf("HSVS %d %d, delta %.3f ms\n", hs_vs.s(), hs_vs.us(), (hs_vs-prev_hs_vs)/1000.0);
 repeat_modeswitch:
    prev_hs_vs = hs_vs;

    // Step 1 - finish dealing with second half of last request (final parameters, actions)

    usleep(1000); // Get past real HSVS

    // The MT9P031 apparently needs all its parameters set at the same time. This isn't per spec,
    // but does appear to work.  
    if (req) {
    //printf("TS: Setting sensor params\n");    
    }

    // setTimes must be done after setExposureParams, or old time estimates will be used
    setTimes(req, hs_vs, wasModeswitch);
    wasModeswitch = false;

    if (req) {
    dprintf("TS: Pushing to inflight\n");
    // set the sensor readout parameters and predicted done time on the pending request
    // and then push it onto the handler's input queue and the v4l2 buffer queue       
    //setReadoutParams(req);

    // now queue up this request's actions
    pthread_mutex_lock(&actionQueueMutex);
    for (std::set<FCam::Action*>::iterator i = req->f2Request->actions.begin();
         i != req->f2Request->actions.end();
         i++) {
        Action a;
        a.time = req->exposureStartTime + (*i)->time - (*i)->latency;
        a.action = (*i)->copy();
        actionQueue.push(a);
    }
    pthread_mutex_unlock(&actionQueueMutex);
    for (size_t i = 0; i < req->f2Request->actions.size(); i++) {
        sem_post(&actionQueueSemaphore);
    }

    pthread_mutex_lock(&inFlightQueueMutex);
    inFlightQueue.push(req);
    pthread_mutex_unlock(&inFlightQueueMutex);
    req = NULL;
    } else {
    // an unpredictable bubble, so do nothing
    }

    // Step 2 - Start processing next request (exposure, frame time, mode switches)
    // If the queue is empty, try to generate some requests from a stream
    if (generator && !requestQueue.size()) {
    dprintf("TS: Requesting generator\n");
    generator->generateRequest();
    }

    pthread_mutex_lock(&requestQueueMutex); 
    if (requestQueue.size()) {
    dprintf("TS: Grabbing next request \n");
    // peek at the next request
    req = requestQueue.front();
    } else {
    // There are no pending requests, push a bubble into the
    // pipeline. The default parameters for a frame work nicely as
    // a bubble (as short as possible, no stats generated), except 
    // that it should be unwanted.
    dprintf("TS: Creating a bubble request\n");
    req = new Frame;
    req->f2Request = new Shot;

    req->f2Request->wanted = false;
    // bubbles should just run at whatever resolution is going. If
    // fast mode switches were possible, it might be nice to run
    // at the minimum resolution to go even faster, but they're
    // not.
    req->f2Request->image.size = current.f2Request->image.size;
    req->f2Request->image.type = current.f2Request->image.type;

    // Temporary hack - keep the same exposure to keep everything flowing
    // smoothly.  This is bad for long exposure bubbles!
    req->f2Request->exposure = current.f2Request->exposure;

    // generate histograms and sharpness maps if they're going,
    // but drop the data.
    req->f2Request->histogram  = current.f2Request->histogram;
    req->f2Request->sharpness  = current.f2Request->sharpness;
    req->histogram.data = NULL;
    req->sharpness.data = NULL;

    // push the bubble into the pipe
    requestQueue.push(req);
    }
    pthread_mutex_unlock(&requestQueueMutex);

    prevReq = *current.f2Request; // Keep around the settings for one more go-around

    //printf("TS: Checking mode switch\n");
    // Check if the next request requires a mode switch
    if (req->f2Request->image.size != current.f2Request->image.size ||
    req->f2Request->image.type != current.f2Request->image.type ||
    req->f2Request->histogram  != current.f2Request->histogram  ||
    req->f2Request->sharpness  != current.f2Request->sharpness) {

    printf("TS:   Mode Requested %d %d\n", req->f2Request->image.size.width, req->f2Request->image.size.height);
    printf("TS:   Mode Current %d %d\n", current.f2Request->image.size.width, current.f2Request->image.size.height);
    
    // flush the pipeline
    dprintf("Setter: Mode switch required - flushing pipe\n");
    pipelineFlush = true;

    pthread_mutex_lock(&cameraMutex);
    dprintf("Setter: Handler done flushing pipe, passing control back to setter\n");
    
    // do the mode switch
    
    if (current.image.size.width > 0) {
        printf("Setter: Stopping camera for mode switch\n");
        sensor->stopStreaming();
        sensor->close();
        sensor->open();
    } else {
        printf("Setter: Opening camera for the first time\n");
        sensor->open();
    }

    // Ensure we're in direct control mode (sensor doesn't auto-update parameters on frame bounds)
    sensor->setControl(MT9P031_CID_DIRECT_MODE, 1);
    
    // Set parameters before starting stream
    setReadoutParams(req, true);
    setExposureParams(req, true);

    // set all the params for the new frame
    V4L2Sensor::Mode m;
    m.width  = req->f2Request->image.size.width;
    m.height = req->f2Request->image.size.height;
    m.type   = req->f2Request->image.type;
    
    sensor->startStreaming(m, &req->f2Request->histogram, &req->f2Request->sharpness);

    /* Read this again because we actually told the sensor to switch modes,
       and only now does it know its new resolution */
    current.frameTime = req->frameTime = sensor->getFrameTime();
    dprintf("Setter: First frame time %f ms\n", current.frameTime/1000.);

    hs_vs = Time::now();
    dprintf("Setter: Setter done bringing up camera, passing control back to handler\n");
    pipelineFlush = false;
    pthread_mutex_unlock(&cameraMutex);
    
    m = sensor->getMode();
    req->image.size.width  = m.width;
    req->image.size.height = m.height;
    req->image.type        = m.type;
    
    current.f2Request->image.type = req->f2Request->image.type;
    current.f2Request->image.size = req->f2Request->image.size;
    current.f2Request->histogram  = req->f2Request->histogram;
    current.f2Request->sharpness  = req->f2Request->sharpness;
        
    current.image.size.width = m.width;
    current.image.size.height = m.height;
    current.image.type = m.type;

    dprintf("TS: Popping mode switch request\n");
    // pop the request  
    pthread_mutex_lock(&requestQueueMutex);
    requestQueue.pop();
    pthread_mutex_unlock(&requestQueueMutex);    

    printf("TS: Mode switch done at %d %d, delta %f ms\n", hs_vs.s(), hs_vs.us(), (hs_vs-prev_hs_vs)/1000.0);
    //hs_vs += req->frame_time;
    wasModeswitch = true;
    goto repeat_modeswitch;
    } 
    
    dprintf("Setter: no mode switch required\n");
    Time b = Time::now();
    setReadoutParams(req);
    setExposureParams(req);
    Time a = Time::now();
    dprintf("    Time to set %f ms\n", (a - b) / 1000.0);

    dprintf("TS: Popping request\n");
    // pop the request 
    pthread_mutex_lock(&requestQueueMutex);
    requestQueue.pop();
    pthread_mutex_unlock(&requestQueueMutex);    

    req->image.size  = current.image.size;
    req->image.type  = current.image.type;
    //req->histogram = req->f2Request->histogram;
    //req->sharpness = req->f2Request->sharpness;

    // Exposure and frame time are set. Return and wait for the next
    // HS_VS before setting readout parameters for this request. (and pulling the next request).
    //usleep(500);

}


void Daemon::runHandler() {
    Frame *req = NULL;       
    V4L2Sensor::V4L2Frame *f = NULL;
    Time prevDoneTime = Time::now();

    pthread_mutex_lock(&cameraMutex);

    while (!stop) {

    // the setter may be waiting for me to finish processing
    // outstanding requests
    if (!req && pipelineFlush && inFlightQueue.size() == 0) {   
        printf("Handler: Handler done flushing pipe, passing control back to setter\n");
        while (pipelineFlush) {
        pthread_mutex_unlock(&cameraMutex);
        // let the setter grab the mutex. It has higher priority,
        // so it should happen instantly. We put this in a while
        // loop just to be sure.
        usleep(10000);
        pthread_mutex_lock(&cameraMutex);
        }
        printf("Handler: Setter done bringing up camera, passing control back to handler\n");
        
    }
    
    if (pipelineFlush) {
        printf("Handler: Setter would like me to flush the pipeline, but I have requests in flight\n");
    }
    
    // wait for a frame
    if (!f) {
        f = sensor->acquireFrame(true);
    }

    if (!f) {
        printf("Handler got a NULL frame\n");
        usleep(10000);
        continue;
    } 

    dprintf("Handler: frame %d %d, delta %f\n",
           f->processingDoneTime.s(), f->processingDoneTime.us(),
           (f->processingDoneTime-prevDoneTime)/1000.);
    prevDoneTime = f->processingDoneTime;

    // grab a request to match to it
    if (!req) {
        if (inFlightQueue.size()) {
        //.printf("Handler: popping a frame request\n");
        pthread_mutex_lock(&inFlightQueueMutex);
        req = inFlightQueue.front();
        inFlightQueue.pop();
        pthread_mutex_unlock(&inFlightQueueMutex);
        } else {
        // there's no request for this frame - probably coming up
        // from a mode switch or starting up
        //printf("Handler: Got a frame without an outstanding request. Waiting some for one.\n");
        int timeout = 5;
        while (timeout > 0 && (inFlightQueue.size()==0) ) {
            timeout--;
            usleep(1000);
        }
        if (timeout > 0) {
            pthread_mutex_lock(&inFlightQueueMutex);
            req = inFlightQueue.front();
            inFlightQueue.pop();
            pthread_mutex_unlock(&inFlightQueueMutex);
        } else {
            printf("  Giving up on waiting for request\n");
            sensor->releaseFrame(f);
            f = NULL;
            continue;
        }
        }
    }
    dprintf("Handler:     Expected at %d %d, delta %f ms\n", 
           req->processingDoneTime.s(), req->processingDoneTime.us(),
           (f->processingDoneTime - req->processingDoneTime)/1000.);
        
    // at this point we have a frame and a request, now look at
    // the time delta between them to see if they're a match
    int dt = req->processingDoneTime - f->processingDoneTime;
    
    if (dt < -25000 && !debugMode) { // more than 25 ms late
        printf("Handler: Expected a frame at %d %d, but one didn't arrive until %d %d, dt %f ms\n",
           req->processingDoneTime.s(), req->processingDoneTime.us(),
           f->processingDoneTime.s(), f->processingDoneTime.us(),
           dt/1000.0);
        req->image.setData(Image::Discard);
        if (!req->f2Request->wanted) {
        delete req;
        } else {
        // the histogram and sharpness map may still have appeared
        sensor->getHistogram(req->exposureEndTime-2300, &req->histogram, &req->f2Request->histogram);
        sensor->getSharpnessMap(req->exposureEndTime-2300, &req->sharpness, &req->f2Request->sharpness);

        pthread_mutex_lock(&frameQueueMutex);
        frameQueue.push(req);
        pthread_mutex_unlock(&frameQueueMutex);
        sem_post(&frameQueueSemaphore);     
        enforceDropPolicy();
        }
        req = NULL;
    } else if (dt < 15000 || debugMode) { // In debugMode, accept all comers
        // Is this frame wanted or a bubble?
        if (!req->f2Request->wanted) {
        // it's a bubble - drop it
        //printf("Handler: discarding a bubble\n");
        delete req;
        sensor->releaseFrame(f);
        } else {
        
        // this looks like a match - bag and tag it
        req->processingDoneTime = f->processingDoneTime;

        size_t bytes = req->image.size.width*req->image.size.height*2;
        if (f->length < bytes) bytes = f->length;
        
        if (req->shot().image.data == Image::AutoAllocate) {
            req->image = Image(req->image.size, req->image.type);
            memcpy(req->image.data, f->data, bytes);
            dprintf("Autoallocate: %d x %d, %d\n", req->image.size.width, req->image.size.height, req->image.data);
        } else if (req->shot().image.data == Image::Discard) {
            req->image.setData(Image::Discard);
        } else {
            if (req->image.size != req->shot().image.size) {
            printf("ERROR: Requested image size (%d x %d) "
                   "on an already allocated image does not "
                   "match actual image size (%d x %d). Dropping image data.\n",
                   req->shot().image.size.width, req->shot().image.size.height,
                   req->image.size.width, req->image.size.height);
            req->image.setData(Image::Discard);
            } else { // the size matches
            req->image = req->shot().image;
            // Need to decide on a more dynamic timeout for lock() here,
            // but 10ms shouldn't be too bad
            if(req->image.lock(10000)) {
                memcpy(req->image.data, f->data, bytes);
                req->image.unlock();
            }  else {
                printf("WARNING: Daemon discarding image data (Target is locked)\n");
                req->image.setData(Image::Discard);
            }
            }
        }

        sensor->releaseFrame(f);
        sensor->getHistogram(req->exposureEndTime-2300, &req->histogram, &req->f2Request->histogram);
        sensor->getSharpnessMap(req->exposureEndTime-2300, &req->sharpness, &req->f2Request->sharpness);

        pthread_mutex_lock(&frameQueueMutex);
        frameQueue.push(req);
        pthread_mutex_unlock(&frameQueueMutex);
        sem_post(&frameQueueSemaphore);
        enforceDropPolicy();

        
        //      printf("Handler: finalized a request for a %d x %d frame\n", req->image.size.width, req->image.size.height);        
        }
        
        req = NULL;
        f = NULL;

    } else { // more than 10ms early... something weird's going on
        printf("Handler: Received an early mystery frame (%d %d) vs (%d %d), dropping it.\n",
           f->processingDoneTime.s(), f->processingDoneTime.us(),
           req->processingDoneTime.s(), req->processingDoneTime.us());
        sensor->releaseFrame(f);
        f = NULL;
    }
    
    }
    pthread_mutex_unlock(&cameraMutex);
    
}


void Daemon::runAction() {
    printf("Action thread running...\n");
    while (1) {       
    sem_wait(&actionQueueSemaphore);
    if (stop) break;
    // priority inversion :(
    pthread_mutex_lock(&actionQueueMutex);
    Action a = actionQueue.top();
    actionQueue.pop();
    pthread_mutex_unlock(&actionQueueMutex);

    Time t = Time::now();
    int delay = (a.time - t) - 500;
    if (delay > 0) usleep(delay);
    Time before = Time::now();
    // busy wait until go time
    while (a.time > before) before = Time::now();
    a.action->doAction();
    //Time after = Time::now();
    printf("Action thread: Initiated action %d us after scheduled time\n", before - a.time);
    delete a.action;
    }
}

}}

---