N900/Lens.cpp

#include "Lens.h"
#include "../Frame.h"
#include "V4L2Sensor.h"

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>

#include <linux/videodev2.h>
#include <limits>

namespace FCam { namespace N900 {
    
    Lens::Lens() : lensHistory(512) {

    // focus at infinity
    setFocus(0, -1);
    }
    
    Lens::~Lens() {
    }
    
    
    int Lens::ioctlGet(unsigned key) {
    struct v4l2_control ctrl;
    ctrl.id = key;
    int fd = V4L2Sensor::instance("/dev/video0")->getFD();
    if (fd < 0) {
        V4L2Sensor::instance("/dev/video0")->open();
        fd = V4L2Sensor::instance("/dev/video0")->getFD();
    }
    if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
        perror("VIDIOC_G_CTRL");
        return -1;
    }
    return ctrl.value;
    }

    int Lens::ioctlSet(unsigned key, int val) {
    struct v4l2_control ctrl;
    ctrl.id = key;
    ctrl.value = val;
    int fd = V4L2Sensor::instance("/dev/video0")->getFD();
    if (fd < 0) {
        V4L2Sensor::instance("/dev/video0")->open();
        fd = V4L2Sensor::instance("/dev/video0")->getFD();
    }
    if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
        perror("VIDIOC_S_CTRL");
        return -1;
    }
    if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
        perror("VIDIOC_G_CTRL");
        return -1;
    }
    return ctrl.value;
    }
    

  void Lens::setFocus(float f, float speed = -1) {
    //printf("In setFocus\n"); fflush(stdout);

    // set the focus speed
    if (speed < 0) speed = maxFocusSpeed();
    if (speed < minFocusSpeed()) speed = minFocusSpeed();
    if (speed > maxFocusSpeed()) speed = maxFocusSpeed();

    // set the ramp mode to linear
    ioctlSet(V4L2_CID_FOCUS_AD5820_RAMP_MODE, 0);

    // the value we set is us per tick
    // = us per s / ticks per us
    int val = ioctlSet(V4L2_CID_FOCUS_AD5820_RAMP_TIME, 1000000.0f/diopterRateToTickRate(speed));

    float focusSpeed = tickRateToDiopterRate(1000000.0f / val);

    // set the focus
    if (f < farFocus()) f = farFocus();
    if (f > nearFocus()) f = nearFocus();

    int ticks = dioptersToTicks(f);
    
    int oldTicks = ioctlGet(V4L2_CID_FOCUS_ABSOLUTE);
    float oldDiopters = ticksToDiopters(oldTicks);
    
    //printf("Focus: set to %d ticks (was %d)\n", ticks, oldTicks);;
    int newTicks = ioctlSet(V4L2_CID_FOCUS_ABSOLUTE, ticks);

    Time start = Time::now();    

    //printf("Focus: %d\n", ctrl.value);
    f = ticksToDiopters(newTicks);
    float dDiopters = f - oldDiopters; 
    if (dDiopters < 0) dDiopters = -dDiopters;

    // time taken to focus = diopters to move / (diopters per second)
    Time end = start + 1000000 * dDiopters / focusSpeed;

    /*
    printf("Focus speed is %f diopters per second\n", getFocusSpeed());
    printf("Focus Diopters to travel = %f\n", dDiopters);
    printf("Focus will take %f us\n", 1000000 * dDiopters/getFocusSpeed());
    */

    LensState s;
    s.time = start;
    s.position = oldDiopters;
    lensHistory.push(s);
    s.time = end;
    s.position = f;
    lensHistory.push(s);
    }

    float Lens::getFocus() {
    return getFocus(Time::now());
    }
    
    float Lens::getFocus(Time t) {
    if (lensHistory.size() && t > lensHistory[0].time) {
        return lensHistory[0].position;
    }

    for (size_t i = 0; i < lensHistory.size()-1; i++) {
        if (t < lensHistory[i+1].time) continue;
        if (t < lensHistory[i].time) {
        // linearly interpolate
        float alpha = float(t - lensHistory[i+1].time)/(lensHistory[i].time - lensHistory[i+1].time);
        return alpha * lensHistory[i].position + (1-alpha) * lensHistory[i+1].position;
        }
    }

    // uh oh, we ran out of history!
    printf("Lens position at time %d %d is unknown!\n", t.s(), t.us());
    return std::numeric_limits<float>::quiet_NaN(); // unknown
    }

    bool Lens::focusChanging() {
    Time t = Time::now();
    return (lensHistory.size()) && (lensHistory[0].time > t);
    }

    float Lens::minFocusSpeed() {
    // slowest speed is 1 tick / 3200 us
    return tickRateToDiopterRate(1/0.003200f);
    }
    
    float Lens::maxFocusSpeed() {
    // fastest speed is 1 tick / 50 us
    return tickRateToDiopterRate(1/0.000050f);
    }


    float Lens::ticksToDiopters(int ticks) {
    float d = 0.029f*(ticks-165);
    // the lens hits internal blockers and doesn't actually move
    // beyond a certain range
    if (d < 0.0f)  d = 0.0f;
    if (d > 20.0f) d = 20.0f;
    return d;
    }

    int Lens::dioptersToTicks(float diopter) {
    return (int)(diopter*34.483f + 165.5f);
    }
    
    
    float Lens::tickRateToDiopterRate(int ticks) {
    return ticks * 0.029f;
    }

    int Lens::diopterRateToTickRate(float diopter) {
    return (int)(diopter*34.482f + 0.5f);
    }

  
    void Lens::tagFrame(FCam::Frame *f) {
    if (!f) {
        return;
    }

    Lens::Tags *tags = new Lens::Tags;
    
    tags->initialFocus = getFocus(f->exposureStartTime);
    tags->finalFocus   = getFocus(f->exposureEndTime);
    tags->focus = (tags->initialFocus + tags->finalFocus)/2;
    tags->focusSpeed = (1000000.0f * (tags->finalFocus - tags->initialFocus)/
                (f->exposureEndTime - f->exposureStartTime));

    tags->zoom = tags->initialZoom = tags->finalZoom = getZoom();
    tags->zoomSpeed = 0;

    tags->aperture = tags->initialAperture = tags->finalAperture = getAperture();
    tags->apertureSpeed = 0;

    f->addTags(this, tags);
    }

}}

---