F2/Lens.cpp

#include "Lens.h"

#include <stdexcept>
#include <sstream>
#include <vector>
#include <algorithm>

#include <stdio.h>
#include <errno.h>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/time.h>

#define EF232_DEBUG 1

#if defined(EF232_DEBUG) 
#define dprintf(...)         \
  printf("EF232[%s]: ", tty.c_str());       \
  printf(__VA_ARGS__); 

#if EF232_DEBUG > 0
#define ddprintf(...)         \
  printf("EF232[%s]:      ", tty.c_str());      \
  printf(__VA_ARGS__); 
#else
#define ddprintf(...)
#endif

#else
#define dprintf(...) 
#define ddprintf(...)
#endif

#define eprintf(...) \
  fprintf(stderr,"EF232[%s]: ERROR! ", tty.c_str());    \
  fprintf(stderr, __VA_ARGS__);


namespace FCam { namespace F2 {

    // Public methods
    //

    Lens::Lens(): 
    tty("/dev/ttyS2"), 
    state(NOT_INITIALIZED), 
    lensDB(),
    currentLens(NULL)
    {
    init();
    }

    Lens::Lens(const std::string &tty_): tty(tty_), state(NOT_INITIALIZED) {
    init();
    }

    Lens::~Lens() {
    close(serial_fd);
    }

    void Lens::setFocus(float diopters, float speed) {
    if (state != READY) {
        eprintf("setFocus: Lens not ready!\n");
        return;
    }    
    focusDist = cmd_DoFocus(diopToEncoder(diopters));
    }
  
    float Lens::getFocus() {
    if (state != READY) {
        eprintf("getFocus: Lens not ready!\n");
        return -1;
    }    
    return encToDiopter(focusDist);
    }

    float Lens::farFocus() {
    return 0;
    }

    float Lens::nearFocus() {
    if (state != READY) {
        eprintf("maxFocus: Lens not ready!\n");
        return -1;
    }
    return 1000.0/currentLens->focusDistMin;
    }

    bool Lens::focusChanging() {
    return false; 
    }

    int Lens::focusLatency() {
    return 0;  
    }

    float Lens::minFocusSpeed() {
    if (state != READY) {
        eprintf("minFocusSpeed: Lens not ready!\n");
        return -1;
    }    
    return currentLens->focusSpeed;
    }

    float Lens::maxFocusSpeed() {
    if (state != READY) {
        eprintf("maxFocusSpeed: Lens not ready!\n");
        return -1;
    }    
    return currentLens->focusSpeed;
    }
   
    void Lens::setZoom(float focal_length_mm, float speed) {
    eprintf("setZoom: Manual zoom only, not zooming to %f\n", focal_length_mm);
    }

    float Lens::getZoom() {
      if (state != READY) {
      eprintf("getZoom: Lens not ready!\n");
      return -1;
      }
      std::string idStr = cmd_GetID();
      unsigned int fl = cmd_GetFocalLength(idStr);
      if (fl != focalLength) {
      focalLength = fl;
      unsigned int minAperture = cmd_GetMinAperture(idStr);
      if (currentLens->minApertureAt(focalLength) != minAperture) {
          dprintf("Adding entry to min aperture map: %d->%d, currently %d\n", focalLength, minAperture, currentLens->minApertureAt(focalLength));
          // Update mapping
          EF232LensInfo updatedInfo = *currentLens;
          EF232LensInfo::minApertureListIter iter=
          updatedInfo.minApertureList.insert(updatedInfo.minApertureList.begin(),
                             EF232LensInfo::apertureChange(focalLength,
                                           minAperture));
          iter++;
          while (iter != updatedInfo.minApertureList.end()) {
          if (iter->second == minAperture) {
              dprintf("Removing redundant entry %d->%d\n", iter->first, iter->second);
              updatedInfo.minApertureList.erase(iter++);
          } else {
              iter++;
          }
          } 
          currentLens = lensDB.update(updatedInfo);       
      }
      }
      return focalLength;
    }

    float Lens::minZoom() {
    if (state != READY) {
        eprintf("minZoom: Lens not ready!\n");
        return -1;
    }    
    return currentLens->focalLengthMin;
    }
    float Lens::maxZoom() {
    if (state != READY) {
        eprintf("minZoom: Lens not ready!\n");
        return -1;
    }    
    return currentLens->focalLengthMax;
    }

    bool Lens::zoomChanging() {
    return false;
    }

    float Lens::minZoomSpeed() {
    return 0;
    }

    float Lens::maxZoomSpeed() {
    return 0;
    }

    int Lens::zoomLatency() {
    return 0;
    }

    void Lens::setAperture(float f_number, float speed) {
      if (state != READY) {
    eprintf("setAperture: Lens not ready!\n");
    return;
      }    
      aperture = cmd_DoAperture(f_number*10);
    }

    float Lens::getAperture() {
      if (state != READY) {
    eprintf("getAperture: Lens not ready!\n");
    return -1;
      }    
      return aperture/10.0;
    }

    float Lens::wideAperture(float focal_length_mm) {
    if (state != READY) {
        eprintf("minAperture: Lens not ready!\n");
        return -1;
    }    
    
    return currentLens->minApertureAt(focal_length_mm)/10.0;
    }


    float Lens::narrowAperture(float focal_length_mm) {
    if (state != READY) {
        eprintf("maxAperture: Lens not ready!\n");
        return -1;
    }    
    return currentLens->apertureMax/10.0;
    }

    bool Lens::apertureChanging() {
    return false;
    }

    int Lens::apertureLatency() {
    return 0; 
    }

    float Lens::minApertureSpeed() {
    return 0; 
    }
       
    float Lens::maxApertureSpeed() {
    return 0; 
    }
    
    
    void Lens::reset() {
      if (state == READY || state == BUSY) {
    close(serial_fd);
      }
      state = NOT_INITIALIZED;
      init();
    }

    void Lens::tagFrame(FCam::Frame *) {

    }


    // Private methods
    //

  
    // Diopter->encoder conversions for focus distance
    float Lens::encToDiopter(unsigned int encoder) {
      return diopScaleFactor * (focusEncoderMax - encoder);
    }

    unsigned int Lens::diopToEncoder(float diopters) {
      return focusEncoderMax - (diopters/diopScaleFactor);
    }

    // Needed forward declaration of template specialization
    template<>
    void Lens::read(std::string &val);

    // Primary initialization for the lens

    void Lens::init() {
      // Open serial port device
      serial_fd = open (tty.c_str(), 
            O_RDWR | O_NOCTTY | O_NDELAY);

      if (serial_fd < 0) {
    eprintf("Unable to open serial device!");
    return;
      }
    
      // Configure port communication parameters

      // reads will block
      fcntl(serial_fd, F_SETFL, 0);

      struct termios opts;
      tcgetattr(serial_fd, &opts);
      // set speed to 19200 8n1
      opts.c_cflag = B19200 | CS8 | CLOCAL | CREAD;
      opts.c_lflag = 0;
      opts.c_iflag = 0;
      opts.c_oflag = 0;
      // set the read timeout to 0.1 seconds
      opts.c_cc[VMIN] = 0;
      opts.c_cc[VTIME] = 1;
      // write the opts struct
      tcflush(serial_fd, TCIFLUSH);
      tcsetattr(serial_fd, TCSANOW, &opts);

      // Set lens starting parameters

      try {
    cmd_DoInitialize();
    cmd_DoApertureOpen();
     
    state = NO_LENS;
      } catch (std::runtime_error err) {
    eprintf("Unable to initialize the lens controller!\n");
    close(serial_fd);
    state = NOT_INITIALIZED;
    return;
      }        

      calibrateLens();
    }

    // Detect and measure lens parameters

    void Lens::calibrateLens() {
      try {
    unsigned int focalMin, focalMax;

    focalLength = cmd_GetFocalLength();

    cmd_GetZoomRange(focalMin, focalMax);

    // Find lens in database, or create a new entry for it 
    currentLens = lensDB.find(focalMin, focalMax);

    // Focus near, and reset lens focus distance encoder 
    cmd_DoFocusAtZero();
    cmd_SetFocusEncoder(0);
      
    // Find minimum focus distance if not known
    if (currentLens->focusDistMin == 0) {
      dprintf("Measuring minimum focus distance\n");
      unsigned int newFDMin, newFDMax;

      cmd_GetFocusBracket(newFDMin, newFDMax);
      if (newFDMin == 0) {
        // Can't find out what the minimum focus distance for this lens is
        // And we don't have it in the DB, either.
        // This results in a lot of buggy math down the line, FIXME
        eprintf("Unknown lens minimum focus distance, assuming 0mm!\n");
      } else {
        EF232LensInfo updatedInfo = *currentLens;
        updatedInfo.focusDistMin = newFDMin;
        currentLens = lensDB.update(updatedInfo);
        dprintf("Updated focusDistMin to %d\n", currentLens->focusDistMin);
      }
    }

    // Find maximum focus encoder count, assume that's infinity
    // Note - that assumption is a bit dangerous, should really calibrate better
    cmd_DoFocusAtInf();

    focusEncoderMax = cmd_GetFocusEncoder();
    diopScaleFactor = 1000.0/(currentLens->focusDistMin
                  * focusEncoderMax);

    state = READY;

    if (currentLens->apertureMax == 0) {
      dprintf("Measuring max aperture...\n");
      unsigned int newApertureMax = cmd_DoApertureClose();
      EF232LensInfo updatedInfo = *currentLens;
      updatedInfo.apertureMax = newApertureMax;
      currentLens = lensDB.update(updatedInfo); 
    }

    if (currentLens->focusSpeed == 0) {
      dprintf("Measuring focus speed...\n");
      // Roughly measure focusing speed
      std::vector<float> focusSpeeds;
      float maxDiop = 1000.0/currentLens->focusDistMin;
      for (unsigned int percent = 10; percent <= 100; percent+=20) {
        timeval startFocus, midFocus, endFocus;
        float destDiop = maxDiop*percent/100;
        gettimeofday(&startFocus, NULL);
        setFocus(destDiop);
        gettimeofday(&midFocus, NULL);
        setFocus(0);
        gettimeofday(&endFocus, NULL);
      
        unsigned int infToZeroT = (midFocus.tv_sec - startFocus.tv_sec)*1000000 
          + (midFocus.tv_usec - startFocus.tv_usec);
        unsigned int zeroToInfT = (endFocus.tv_sec - midFocus.tv_sec)*1000000 
          + (endFocus.tv_usec - midFocus.tv_usec);
  
        float infToZeroDPS = (destDiop/(float)infToZeroT)*1e6;
        float zeroToInfDPS = (destDiop/(float)zeroToInfT)*1e6;
        dprintf("Focus Speed calib for 0 to %f diop: %f diop/sec inf-to-zero, %f diop/sec zero-to-inf\n", destDiop, infToZeroDPS, zeroToInfDPS);
        focusSpeeds.push_back(infToZeroDPS);
        focusSpeeds.push_back(zeroToInfDPS);
      }

      // Get median speed, more or less
      std::sort(focusSpeeds.begin(), focusSpeeds.end());
      float newFocusSpeed = focusSpeeds[(focusSpeeds.size()+1)/2];
      dprintf("Setting focus speed to median: %f DPS\n", newFocusSpeed);
    
      EF232LensInfo updatedInfo = *currentLens;
      updatedInfo.focusSpeed = newFocusSpeed;
      currentLens = lensDB.update(updatedInfo);
    
    }
      
    if (currentLens->minApertureList.size() == 0) {
      dprintf("Establishing minimal focal length->min aperture map\n");
      // Minimal entry for min aperture/focal length mapping
      unsigned int newFocalLength = cmd_GetFocalLength();
      unsigned int newMinAperture = cmd_GetMinAperture();
      EF232LensInfo updatedInfo = *currentLens;
      updatedInfo.minApertureList[newFocalLength] = newMinAperture;
      currentLens = lensDB.update(updatedInfo);
    }

    aperture = cmd_DoApertureOpen();
           
      } catch (std::runtime_error err) {
    eprintf("Unable to configure lens!\n");
    state = NO_LENS;
      }
    }

    

    // Individual command executers

    std::string Lens::cmd_GetID() {
      send("id");
      expect("OK");
      read(buf); // Expecting a string of form "NNmm,fNN"
      dprintf("* ID response: %s\n", buf.c_str());
      return buf;
    }

    unsigned int Lens::cmd_GetFocalLength(std::string idStr) {
      std::stringstream parsebuf;
      if (idStr == "") idStr = cmd_GetID();
      parsebuf.str(idStr);
      unsigned int val;
      parsebuf >> val;
      return val;
    }

    unsigned int Lens::cmd_GetMinAperture(std::string idStr) {
      std::stringstream parsebuf;
      if (idStr == "") idStr = cmd_GetID();
      parsebuf.str(idStr);
      unsigned int val;
      parsebuf.ignore(10,'f');
      parsebuf >> val;
      return val;
    }

    void Lens::cmd_GetZoomRange(unsigned int &min,
                     unsigned int &max) {
      std::stringstream parsebuf;
      send("dz"); // Get lens zoom range
      expect("OK");
      read(buf); // Expecting a string of form "NNmm,NNmm"
      parsebuf.str(buf);
      parsebuf >> min;
      parsebuf.ignore(10,',');
      parsebuf >> max;
      dprintf("* DZ response: %s\n",buf.c_str());
    }

    void Lens::cmd_GetFocusBracket(unsigned int &min,
                    unsigned int &max) {
      std::stringstream parsebuf;
      send("fd"); // See if we can get a focus distance bracket here at zero focus
      expect("OK");
      read(buf); // Expecting NNcm,NNcm
      parsebuf.str(buf);
      parsebuf >> min;
      parsebuf.ignore(10,',');
      parsebuf >> max;
      min*=10;
      max*=10;    
      dprintf("* FD response: %s\n", buf.c_str());
    }

    void Lens::cmd_DoInitialize() {
      send("in");  
      expect("OK");
      expect("DONE");
      dprintf("* IN successful\n");
    }

    unsigned int Lens::cmd_DoApertureOpen() {
      std::stringstream parsebuf;
      unsigned int val;
      send("mo");
      expect("OK");
      expect("DONE", buf); // Expecting DONE<steps>,f<f_number*10>
      dprintf("* MO response: %s\n", buf.c_str());
      parsebuf.str(buf);
      parsebuf.ignore(10,'f');
      parsebuf >> val;
      return val;
    }

    unsigned int Lens::cmd_DoAperture(unsigned int val) {
      std::stringstream parsebuf;
      unsigned int minAperture = currentLens->minApertureAt(focalLength);
      unsigned int newVal;

      // Aperture position encoded as 1/4-stops from fully-open
      unsigned int aperture_enc = (val-minAperture)*4/10;
      parsebuf << "ma" << aperture_enc;
      dprintf("* MA send: %s\n", parsebuf.str().c_str());
      send(parsebuf.str());
      expect("OK");
      expect("DONE", buf); // Expecting DONE<step>,f<f_number*10>

      dprintf("* MA response: %s\n", buf.c_str());
      parsebuf.str(buf);
      parsebuf.ignore(10,'f');
      parsebuf >> newVal;
      return newVal;
    }

    unsigned int Lens::cmd_DoApertureClose() {
      std::stringstream parsebuf;
      unsigned int val;
      send("mc");
      expect("OK");
      expect("DONE", buf); // Expecting DONE<steps>,f<f_number*10>
      dprintf("* MC response: %s\n", buf.c_str());
      parsebuf.str(buf);
      parsebuf.ignore(10,'f');
      parsebuf >> val;
      return val;
    }

    void Lens::cmd_DoFocusAtZero() {
      send("mz"); // Set lens focus distance to nearest
      expect("OK");
      expect("DONE", buf);
      dprintf("* MZ response: %s\n", buf.c_str());
    }

    unsigned int Lens::cmd_DoFocus(unsigned int val) {
      std::stringstream parsebuf, parsebuf2;
      unsigned int encVal;
      parsebuf << "fa" << val;
      send(parsebuf.str());
      expect("OK");
      expect("DONE", buf);
      parsebuf2.str(buf);
      parsebuf2 >> encVal;
      dprintf("* FA response: %s (%d)\n", buf.c_str(), encVal);
      return encVal;
    }

    void Lens::cmd_DoFocusAtInf() {
      send("mi"); // Set lens focus distance to infinity
      expect("OK");
      expect("DONE", buf);
      dprintf("* MI response: %s\n", buf.c_str());    
    }

    void Lens::cmd_SetFocusEncoder(unsigned int val) {
      std::stringstream parsebuf;
      parsebuf << "sf" << val;
      send(parsebuf.str());
      expect("OK");
      dprintf("* SF successful\n");
    }

    unsigned int Lens::cmd_GetFocusEncoder() {
      std::stringstream parsebuf;
      int val;
      send("pf"); // Read encoder value at focus position
      expect("OK");
      read(buf);
      dprintf("* PF response: %s\n", buf.c_str());
      parsebuf.str(buf);
      parsebuf >> val;
      return val;
    }

    // Basic serial communication primitives

    void Lens::send(const std::string &str) {
      ddprintf("Sending: %s\n", str.c_str());
      char c = 13;
      usleep(1000);
      write(serial_fd, str.c_str(), str.size());
      write(serial_fd, &c, 1);

      expect(str);
    }

    template<>
    void Lens::read(std::string &str) {   
      char nextC;
      int timeoutCounter = 0;
      str.clear();
      do {
    int ret = ::read(serial_fd, &nextC, 1);
    if (ret > 0 && nextC != 13) {
      str += nextC; 
    } else if (ret < 0) {
      eprintf("Read returned %i\n", ret);
      return;
    } else {
      timeoutCounter++;
      if (timeoutCounter == 30) {
        eprintf("Lost comm with lens controller\n");
        throw std::runtime_error("timeout communicating with controller");
      }
    }
      } while (nextC != 13);
      ddprintf("Received: '%s'\n", str.c_str());
    }

    template<typename T>
    void Lens::read(T &val) {
      std::stringstream str;

      read(str.str());
      str >> val;
    }

    void Lens::expect(const std::string &desired) {
      std::string buf;
      read(buf);
      ddprintf("Expect: Got '%s'\n", buf.c_str());
      if (buf.substr(0, desired.size()) == desired) return;

      eprintf("Expected: '%s' Got: '%s'\n", desired.c_str(), buf.c_str());
      throw std::runtime_error("expect mismatch");
    }

    void Lens::expect(const std::string &desired, std::string &remainder) {
      std::string buf;
      read(buf);
      if (buf.substr(0, desired.size()) == desired) {
    remainder = buf.substr(desired.size());
    return;
      }
    
      eprintf("Expected: %s\n Got: %s\n", desired.c_str(), buf.c_str());
      throw std::runtime_error("expect mismatch");
    }
  }
}

---