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

src/N900/Lens.cpp

00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <sys/types.h>
00004 #include <sys/stat.h>
00005 #include <sys/ioctl.h>
00006 #include <fcntl.h>
00007 
00008 #include <linux/videodev2.h>
00009 #include <limits>
00010 
00011 #include "FCam/N900/Lens.h"
00012 #include "FCam/Frame.h"
00013 
00014 #include "../Debug.h"
00015 #include "V4L2Sensor.h"
00016 
00017 namespace FCam { namespace N900 {
00018     
00019     Lens::Lens() : lensHistory(512) {
00020 
00021         // focus at infinity
00022         setFocus(0, -1);
00023     }
00024     
00025     Lens::~Lens() {
00026     }
00027     
00028     
00029     int Lens::ioctlGet(unsigned key) {
00030         struct v4l2_control ctrl;
00031         ctrl.id = key;
00032         int fd = V4L2Sensor::instance("/dev/video0")->getFD();
00033         if (fd < 0) {
00034             V4L2Sensor::instance("/dev/video0")->open();
00035             fd = V4L2Sensor::instance("/dev/video0")->getFD();
00036         }
00037         if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
00038             error(Event::DriverError, this, "VIDIOC_G_CTRL: %d, %d", key, errno);
00039             return -1;
00040         }
00041         return ctrl.value;
00042     }
00043 
00044     int Lens::ioctlSet(unsigned key, int val) {
00045         struct v4l2_control ctrl;
00046         ctrl.id = key;
00047         ctrl.value = val;
00048         int fd = V4L2Sensor::instance("/dev/video0")->getFD();
00049         if (fd < 0) {
00050             V4L2Sensor::instance("/dev/video0")->open();
00051             fd = V4L2Sensor::instance("/dev/video0")->getFD();
00052         }
00053         if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
00054             error(Event::DriverError, this, "VIDIOC_S_CTRL: %d = %d, %d", key, val, errno);
00055             return -1;
00056         }
00057         if (ioctl(fd, VIDIOC_G_CTRL, &ctrl) < 0) {
00058             error(Event::DriverError, this, "VIDIOC_G_CTRL: %d, %d", key, errno);
00059             return -1;
00060         }
00061         return ctrl.value;
00062     }
00063     
00064 
00065     void Lens::setFocus(float f, float speed = -1) {
00066 
00067         // set the focus speed
00068         if (speed < 0) speed = maxFocusSpeed();
00069         if (speed < minFocusSpeed()) speed = minFocusSpeed();
00070         if (speed > maxFocusSpeed()) speed = maxFocusSpeed();
00071 
00072         // set the ramp mode to linear
00073         ioctlSet(V4L2_CID_FOCUS_AD5820_RAMP_MODE, 0);
00074 
00075         // the value we set is us per tick
00076         // = us per s / ticks per us
00077         int val = ioctlSet(V4L2_CID_FOCUS_AD5820_RAMP_TIME, 1000000.0f/diopterRateToTickRate(speed));
00078 
00079         float focusSpeed = tickRateToDiopterRate(1000000.0f / val);
00080 
00081         // set the focus
00082         if (f < farFocus()) f = farFocus();
00083         if (f > nearFocus()) f = nearFocus();
00084 
00085         int ticks = dioptersToTicks(f);
00086     
00087         int oldTicks = ioctlGet(V4L2_CID_FOCUS_ABSOLUTE);
00088         float oldDiopters = ticksToDiopters(oldTicks);
00089     
00090         int newTicks = ioctlSet(V4L2_CID_FOCUS_ABSOLUTE, ticks);
00091 
00092         Time start = Time::now();    
00093 
00094         f = ticksToDiopters(newTicks);
00095         float dDiopters = f - oldDiopters; 
00096         if (dDiopters < 0) dDiopters = -dDiopters;
00097 
00098         // time taken to focus = diopters to move / (diopters per second)
00099         Time end = start + 1000000 * dDiopters / focusSpeed;
00100 
00101         LensState s;
00102         s.time = start;
00103         s.position = oldDiopters;
00104         lensHistory.push(s);
00105         s.time = end;
00106         s.position = f;
00107         lensHistory.push(s);
00108     }
00109 
00110     float Lens::getFocus() const {
00111         return getFocus(Time::now());
00112     }
00113     
00114     float Lens::getFocus(Time t) const {
00115         if (lensHistory.size() && t > lensHistory[0].time) {
00116             return lensHistory[0].position;
00117         }
00118 
00119         for (size_t i = 0; i < lensHistory.size()-1; i++) {
00120             if (t < lensHistory[i+1].time) continue;
00121             if (t < lensHistory[i].time) {
00122                 // linearly interpolate
00123                 float alpha = float(t - lensHistory[i+1].time)/(lensHistory[i].time - lensHistory[i+1].time);
00124                 return alpha * lensHistory[i].position + (1-alpha) * lensHistory[i+1].position;
00125             }
00126         }
00127 
00128         // uh oh, we ran out of history!
00129         error(Event::LensHistoryError, "Lens position at time %d %d is unknown", t.s(), t.us());
00130         return std::numeric_limits<float>::quiet_NaN(); // unknown
00131     }
00132 
00133     bool Lens::focusChanging() const {
00134         Time t = Time::now();
00135         return (lensHistory.size()) && (lensHistory[0].time > t);
00136     }
00137 
00138     float Lens::minFocusSpeed() const {
00139         // slowest speed is 1 tick / 3200 us
00140         return tickRateToDiopterRate(1/0.003200f);
00141     }
00142     
00143     float Lens::maxFocusSpeed() const {
00144         // fastest speed is 1 tick / 50 us
00145         return tickRateToDiopterRate(1/0.000050f);
00146     }
00147 
00148 
00149     float Lens::ticksToDiopters(int ticks) const {
00150         float d = 0.0315f*(ticks-227);
00151         // the lens hits internal blockers and doesn't actually move
00152         // beyond a certain range
00153         if (d < 0.0f)  d = 0.0f;
00154         if (d > 20.0f) d = 20.0f;
00155         return d;
00156     }
00157 
00158     int Lens::dioptersToTicks(float diopter) const {
00159         return (int)(diopter*31.746f + 227.5f);
00160     }
00161     
00162     
00163     float Lens::tickRateToDiopterRate(int ticks) const {
00164         return ticks * 0.0315f;
00165     }
00166 
00167     int Lens::diopterRateToTickRate(float diopter) const {
00168         return (int)(diopter*31.746f + 0.5f);
00169     }
00170 
00171   
00172     void Lens::tagFrame(FCam::Frame f) {
00173         float initialFocus = getFocus(f.exposureStartTime());
00174         float finalFocus = getFocus(f.exposureEndTime());
00175 
00176         f["lens.initialFocus"] = initialFocus;
00177         f["lens.finalFocus"] = finalFocus;
00178         f["lens.focus"] = (initialFocus + finalFocus)/2;
00179         f["lens.focusSpeed"] = (1000000.0f * (finalFocus - initialFocus)/
00180                                 (f.exposureEndTime() - f.exposureStartTime()));
00181 
00182         float zoom = getZoom();
00183         f["lens.zoom"] = zoom;
00184         f["lens.initialZoom"] = zoom;
00185         f["lens.finalZoom"] = zoom;
00186         f["lens.zoomSpeed"] = 0;
00187 
00188         float aperture = getAperture();
00189         f["lens.aperture"] = aperture;
00190         f["lens.initialAperture"] = aperture;
00191         f["lens.finalAperture"] = aperture;
00192         f["lens.apertureSpeed"] = 0;
00193 
00194         // static properties of the N900's lens. In the future, we may
00195         // just add "lens.*" with a pointer to this lens to the tags,
00196         // and have the tag map lazily evaluate anything starting with
00197         // devicename.* by asking the appropriate device for more
00198         // details. For now there are only four more fields, so we
00199         // don't mind.
00200         f["lens.minZoom"] = minZoom();
00201         f["lens.maxZoom"] = maxZoom();
00202         f["lens.wideApertureMin"] = wideAperture(minZoom());
00203         f["lens.wideApertureMax"] = wideAperture(maxZoom());
00204     }
00205 
00206 }}

Generated on Mon Aug 16 2010 14:25:45 for FCam by  doxygen 1.7.1