src/N900/Lens.cpp

#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>

#include "FCam/N900/Lens.h"
#include "FCam/Frame.h"

#include "../Debug.h"
#include "V4L2Sensor.h"

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) {
            error(Event::DriverError, this, "VIDIOC_G_CTRL: %d, %d", key, errno);
            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) {
            error(Event::DriverError, this, "VIDIOC_S_CTRL: %d = %d, %d", key, val, errno);
            return -1;
        }
        if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
            error(Event::DriverError, this, "VIDIOC_G_CTRL: %d, %d", key, errno);
            return -1;
        }
        return ctrl.value;
    }
    

    void Lens::setFocus(float f, float speed = -1) {

        // 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);
    
        int newTicks = ioctlSet(V4L2_CID_FOCUS_ABSOLUTE, ticks);

        Time start = Time::now();    

        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;

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

    float Lens::getFocus() const {
        return getFocus(Time::now());
    }
    
    float Lens::getFocus(Time t) const {
        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!
        error(Event::LensHistoryError, "Lens position at time %d %d is unknown", t.s(), t.us());
        return std::numeric_limits<float>::quiet_NaN(); // unknown
    }

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

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


    float Lens::ticksToDiopters(int ticks) const {
        float d = 0.0315f*(ticks-227);
        // 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) const {
        return (int)(diopter*31.746f + 227.5f);
    }
    
    
    float Lens::tickRateToDiopterRate(int ticks) const {
        return ticks * 0.0315f;
    }

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

  
    void Lens::tagFrame(FCam::Frame f) {
        float initialFocus = getFocus(f.exposureStartTime());
        float finalFocus = getFocus(f.exposureEndTime());

        f["lens.initialFocus"] = initialFocus;
        f["lens.finalFocus"] = finalFocus;
        f["lens.focus"] = (initialFocus + finalFocus)/2;
        f["lens.focusSpeed"] = (1000000.0f * (finalFocus - initialFocus)/
                                (f.exposureEndTime() - f.exposureStartTime()));

        float zoom = getZoom();
        f["lens.zoom"] = zoom;
        f["lens.initialZoom"] = zoom;
        f["lens.finalZoom"] = zoom;
        f["lens.zoomSpeed"] = 0;

        float aperture = getAperture();
        f["lens.aperture"] = aperture;
        f["lens.initialAperture"] = aperture;
        f["lens.finalAperture"] = aperture;
        f["lens.apertureSpeed"] = 0;

        // static properties of the N900's lens. In the future, we may
        // just add "lens.*" with a pointer to this lens to the tags,
        // and have the tag map lazily evaluate anything starting with
        // devicename.* by asking the appropriate device for more
        // details. For now there are only four more fields, so we
        // don't mind.
        f["lens.minZoom"] = minZoom();
        f["lens.maxZoom"] = maxZoom();
        f["lens.wideApertureMin"] = wideAperture(minZoom());
        f["lens.wideApertureMax"] = wideAperture(maxZoom());
    }

}}