mt9v.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009-2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <wge100_camera/mt9v.h>
00036 #include <ros/console.h>
00037 #include <math.h>
00038 #include <values.h>
00039 
00040 #define VMODEDEF(width, height, fps, hblank, vblank) { #width"x"#height"x"#fps, width, height, fps, hblank, vblank }  
00041 const struct MT9VMode MT9VModes[MT9V_NUM_MODES] = {
00042   VMODEDEF(752,480,15, 974, 138),
00043   VMODEDEF(752,480,12.5, 848, 320),
00044   VMODEDEF(640,480,30, 372, 47),
00045   VMODEDEF(640,480,25, 543, 61),
00046   VMODEDEF(640,480,15, 873, 225),
00047   VMODEDEF(640,480,12.5, 960, 320),
00048   VMODEDEF(320,240,60, 106, 73),
00049   VMODEDEF(320,240,50, 180, 80),
00050   VMODEDEF(320,240,30, 332, 169),
00051   VMODEDEF(320,240,25, 180, 400)
00052 };
00053 
00054 class MT9VImagerImpl : public MT9VImager
00055 {
00056 protected:
00057   IpCamList &camera_;
00058 
00059   uint8_t reg_column_start_;
00060   uint8_t reg_row_start_;
00061   uint8_t reg_window_width_;
00062   uint8_t reg_window_height_;
00063   uint8_t reg_hblank_;
00064   uint8_t reg_vblank_;
00065   uint8_t reg_shutter_width_;
00066   uint8_t reg_max_shutter_width_;
00067   uint8_t reg_analog_gain_;
00068   uint8_t reg_agc_aec_enable_;
00069   uint8_t reg_read_mode_;
00070   uint8_t reg_companding_mode_; 
00071   uint8_t reg_desired_bin_;
00072 
00073 // Imager dependent bounds
00074   int max_max_shutter_width_;
00075   int max_shutter_width_;
00076     
00077 // Some bits are not always at the same position
00078   int agc_aec_enable_shift_;
00079   int companding_mode_shift_;
00080 
00081 // Cached register values
00082   uint16_t read_mode_cache_;
00083   uint16_t *agc_aec_mode_cache_;
00084   uint16_t agc_aec_mode_cache_backing_;;
00085   uint16_t *companding_mode_cache_;
00086   uint16_t companding_mode_cache_backing_;;
00087 
00088   uint16_t imager_version_;
00089   double line_time_;
00090  
00091   std::string model_;
00092 
00093   MT9VImagerPtr alternate_;
00094 
00095 public:
00096   MT9VImagerImpl(IpCamList &cam) : camera_(cam)
00097   {
00098     reg_column_start_ = 0x01;
00099     reg_row_start_ = 0x02;
00100     reg_window_width_ = 0x04;
00101     reg_window_height_ = 0x03;
00102     reg_hblank_ = 0x05;
00103     reg_vblank_ = 0x06;
00104     reg_shutter_width_ = 0x0B;
00105     reg_max_shutter_width_ = 0xBD;
00106     reg_analog_gain_ = 0x35;
00107     reg_agc_aec_enable_ = 0xAF;
00108     reg_read_mode_ = 0x0D;
00109     reg_companding_mode_ = 0x1C;
00110     reg_desired_bin_ = 0xA5;
00111 
00112     agc_aec_enable_shift_ = 0;
00113     companding_mode_shift_ = 0;
00114 
00115     max_max_shutter_width_ = 2047;
00116     max_shutter_width_ = 32767;
00117 
00118     read_mode_cache_ = 0x300;
00119     agc_aec_mode_cache_ = &agc_aec_mode_cache_backing_;
00120     *agc_aec_mode_cache_ = 3;
00121     companding_mode_cache_ = &companding_mode_cache_backing_;
00122     *companding_mode_cache_ = 2;
00123 
00124     line_time_ = 0;
00125 
00126     if (wge100ReliableSensorRead(&cam, 0x00, &imager_version_, NULL))
00127     {
00128       ROS_WARN("MT9VImager::getInstance Unable to read imager version.");
00129     }
00130     //ROS_FATAL("Black level correction is forced off for debugging. This shouldn't get checked in.");
00131     //setBlackLevel(true, 64, 0, 0);
00132   }
00133 
00134   virtual ~MT9VImagerImpl()
00135   {
00136   }
00137 
00138   virtual bool setAgcAec(bool agc, bool aec)
00139   {
00140     uint16_t mask = 3 << agc_aec_enable_shift_;
00141     uint16_t val = (agc ? 2 : 0) | (aec ? 1 : 0);
00142     val <<= agc_aec_enable_shift_;
00143 
00144     *agc_aec_mode_cache_ = (*agc_aec_mode_cache_ & ~mask) | val;
00145 
00146     if (wge100ReliableSensorWrite(&camera_, reg_agc_aec_enable_, *agc_aec_mode_cache_, NULL) != 0)
00147     {
00148       ROS_WARN("Error setting AGC/AEC mode. Exposure and gain may be incorrect.");
00149       return true;
00150     }
00151     return false;
00152   }
00153 
00154   virtual bool setBrightness(int brightness)
00155   {
00156     if (wge100ReliableSensorWrite(&camera_, reg_desired_bin_, brightness, NULL) != 0)
00157     {
00158       ROS_WARN("Error setting brightness.");
00159       return true;
00160     }
00161     return false;
00162   }
00163 
00164   virtual bool setGain(int gain)
00165   {
00166     if (wge100ReliableSensorWrite(&camera_, reg_analog_gain_, 0x8000 | gain, NULL) != 0)
00167     {
00168       ROS_WARN("Error setting analog gain.");
00169       return true;
00170     }
00171     return false;
00172   }
00173 
00174   virtual bool setExposure(double exp)
00175   {
00176     if (line_time_ == 0)
00177     {
00178       ROS_ERROR("setExposure called before setMode in class MT9VImager. This is a bug.");
00179       return true;
00180     }
00181     int explines = exp / line_time_;
00182     if (explines < 1) 
00183     {
00184       ROS_WARN("Requested exposure too short, setting to %f s", line_time_);
00185       explines = 1;
00186     }
00187     if (explines > max_shutter_width_) 
00188     {
00189       explines = max_shutter_width_;
00190       ROS_WARN("Requested exposure too long, setting to %f s", explines * line_time_);
00191     }
00192     ROS_DEBUG("Setting exposure lines to %i (%f s).", explines, explines * line_time_);
00193     if ( wge100ReliableSensorWrite( &camera_, reg_shutter_width_, explines, NULL) != 0)
00194     {
00195       ROS_WARN("Error setting exposure.");
00196       return true;
00197     }
00198     return false;
00199   }
00200   
00201   virtual bool setMaxExposure(double exp)
00202   {
00203     if (line_time_ == 0)
00204     {
00205       ROS_ERROR("setMaxExposure called before setMode in class MT9VImager. This is a bug.");
00206       return true;
00207     }
00208     int explines = exp / line_time_;
00209     if (explines < 1) 
00210     {
00211       ROS_WARN("Requested max exposure too short, setting to %f s", line_time_);
00212       explines = 1;
00213     }
00214     if (explines > max_max_shutter_width_) 
00215     {
00216       explines = max_max_shutter_width_;
00217       ROS_WARN("Requested max exposure too long, setting to %f s", explines * line_time_);
00218     }
00219     ROS_DEBUG("Setting max exposure lines to %i (%f s).", explines, explines * line_time_);
00220     if ( wge100ReliableSensorWrite( &camera_, reg_max_shutter_width_, explines, NULL) != 0)
00221     {
00222       ROS_WARN("Error setting max exposure.");
00223       return true;
00224     }
00225     return false;
00226   }
00227 
00228   virtual bool setMirror(bool mirrorx, bool mirrory)
00229   {
00230     uint16_t mask = 0x30;
00231     uint16_t val = (mirrory ? 0x10 : 0) | (mirrorx ? 0x20 : 0);
00232 
00233     read_mode_cache_ = (read_mode_cache_ & ~mask) | val;
00234 
00235     if (wge100ReliableSensorWrite(&camera_, reg_read_mode_, read_mode_cache_, NULL) != 0)
00236     {
00237       ROS_WARN("Error setting mirror mode. Read mode register settings may have been corrupted.");
00238       return true;
00239     }
00240     return false;
00241   }
00242   
00243   virtual bool setBlackLevel(bool manual_override, int calibration_value, int step_size, int filter_length)
00244   {
00245     if (wge100ReliableSensorWrite(&camera_, 0x47, ((filter_length & 7) << 5) | manual_override, NULL) != 0 ||
00246         wge100ReliableSensorWrite(&camera_, 0x48, calibration_value & 0xFF, NULL) != 0 ||
00247         wge100ReliableSensorWrite(&camera_, 0x4C, step_size & 0x1F, NULL) != 0)
00248     {
00249       ROS_WARN("Error setting black level correction mode.");
00250       return true;
00251     }
00252 
00253     return false;
00254   }
00255 
00256   virtual bool setMode(int x, int y, int binx, int biny, double rate, int xoffset, int yoffset)
00257   {
00258     if (binx < 0 || binx > 2)
00259     {
00260       ROS_ERROR("x binning should 0, 1 or 2, not %i", binx);
00261       return true;
00262     }
00263 
00264     if (biny < 0 || biny > 2)
00265     {
00266       ROS_ERROR("y binning should 0, 1 or 2, not %i", biny);
00267       return true;
00268     }
00269 
00270     x <<= binx;
00271     y <<= biny;
00272     
00273     if (x < 1 || x > 752)
00274     {
00275       ROS_ERROR("Requested x resolution (includes binning) %i is not between 1 and 752.", x);
00276       return true;
00277     }
00278 
00279     if (y < 1 || y > 480)
00280     {
00281       ROS_ERROR("Requested y resolution (includes binning) %i is not between 1 and 480.", y);
00282       return true;
00283     }
00284     
00285     // XXX -old center-based calculation left here for reference #4670
00286     // startx = (752 - x) / 2 + xoffset + 1;
00287     // starty = (480 - y) / 2 + yoffset + 4;
00288 
00289     int startx, starty;
00290     startx = 57 + xoffset;
00291     starty = 4 + yoffset;
00292 
00293     if (startx < 1 || startx + x > 752 + 1)
00294     {
00295       ROS_ERROR("Image x offset exceeds imager bounds.");
00296       return true;
00297     }
00298 
00299     if (starty < 4 || starty + y > 480 + 4)
00300     {
00301       ROS_ERROR("Image y offset exceeds imager bounds.");
00302       return true;
00303     }
00304 
00305     int bestvblank = 0;
00306     int besthblank = 0;
00307     
00308     double clock = 16e6;
00309     int target_tics = clock / rate;
00310     int besterror = INT_MAX;
00311     int packet_size = 70 + (x >> binx);
00312     double max_peak_data_rate = 90e6;
00313     double best_peak_data_rate = 0;
00314     int best_tics = 0;
00315     ROS_DEBUG("Target tics %i", target_tics);
00316 
00317     for (int hblank = 100; hblank < 1023; hblank++) // Smaller min values are possible depending on binning. Who cares...
00318     {
00319       int htics = x + hblank;
00320       int extra_tics = 4;
00321       int vblank = round(((double) target_tics - extra_tics) / htics) - y;
00322       if (vblank < 40) // Could probably use 4 here
00323         vblank = 40;
00324       if (vblank > 3000) // Some imagers can do more
00325         vblank = 3000;
00326       int vtics = vblank + y;
00327       
00328       int tics = htics * vtics + extra_tics;
00329       int err = abs(tics - target_tics);
00330       double peak_data_rate = 8 * packet_size * clock / htics;
00331 
00332       if (peak_data_rate <= max_peak_data_rate && err < besterror)
00333       {
00334         besterror = err;
00335         best_tics = tics;
00336         bestvblank = vblank;
00337         besthblank = hblank;
00338         best_peak_data_rate = peak_data_rate;
00339       }
00340     }
00341 
00342     if (besterror == INT_MAX)
00343     {
00344       ROS_ERROR("Could not find suitable vblank and hblank for %ix%i mode.\n", x, y);
00345       return true;
00346     }
00347 
00348     line_time_ = (besthblank + x) / clock;
00349 
00350     ROS_DEBUG("Selected vblank=%i hblank=%i data_rate=%f err=%f period=%f", bestvblank, besthblank, best_peak_data_rate, besterror / clock, best_tics / clock);
00351     
00352     if (wge100ReliableSensorWrite(&camera_, reg_vblank_, bestvblank, NULL) != 0)
00353     {
00354       ROS_ERROR("Failed to set vertical blanking. Stream will probably be corrupt.");
00355       return true;
00356     }
00357     
00358     if (wge100ReliableSensorWrite(&camera_, reg_hblank_, besthblank, NULL) != 0)
00359     {
00360       ROS_ERROR("Failed to set horizontal blanking. Stream will probably be corrupt.");
00361       return true;
00362     }
00363     
00364     if (wge100ReliableSensorWrite(&camera_, reg_column_start_, startx, NULL) != 0)
00365     {
00366       ROS_ERROR("Failed to set start column. Stream will probably be corrupt.");
00367       return true;
00368     }
00369     
00370     if (wge100ReliableSensorWrite(&camera_, reg_row_start_, starty, NULL) != 0)
00371     {
00372       ROS_ERROR("Failed to set start row. Stream will probably be corrupt.");
00373       return true;
00374     }
00375     
00376     if (wge100ReliableSensorWrite(&camera_, reg_window_width_, x, NULL) != 0)
00377     {
00378       ROS_ERROR("Failed to set image width. Stream will probably be corrupt.");
00379       return true;
00380     }
00381     
00382     if (wge100ReliableSensorWrite(&camera_, reg_window_height_, y, NULL) != 0)
00383     {
00384       ROS_ERROR("Failed to set image height. Stream will probably be corrupt.");
00385       return true;
00386     }
00387 
00388     uint16_t mask = 0x0F;
00389     uint16_t val = (binx << 2) | biny;
00390 
00391     read_mode_cache_ = (read_mode_cache_ & ~mask) | val;
00392     
00393     if (wge100ReliableSensorWrite(&camera_, reg_read_mode_, read_mode_cache_, NULL) != 0)
00394     {
00395       ROS_ERROR("Failed to set binning modes. Read mode may be corrupted.");
00396       return true;
00397     }
00398 
00399     return false;
00400   }
00401   
00402   virtual bool setCompanding(bool activated)
00403   {
00404     uint16_t mask = 3 << companding_mode_shift_;
00405     uint16_t val = (activated ? 3 : 2) << companding_mode_shift_;
00406     *companding_mode_cache_ = (*companding_mode_cache_ & ~mask) | val;
00407 
00408     if (wge100ReliableSensorWrite(&camera_, reg_companding_mode_, *companding_mode_cache_, NULL) != 0)
00409     {
00410       ROS_WARN("Error setting companding mode.");
00411       return true;
00412     }
00413     return false;
00414   }
00415 
00416   virtual MT9VImagerPtr getAlternateContext()
00417   {
00418     return alternate_;
00419   }
00420 
00421   virtual uint16_t getVersion()
00422   {
00423     return imager_version_;
00424   }
00425 
00426   virtual std::string getModel()
00427   {
00428     return model_;
00429   }
00430 };
00431 
00432 class MT9V034 : public MT9VImagerImpl
00433 {
00434 public:
00435   class MT9V034Alternate : public MT9VImagerImpl
00436   {
00437   public:
00438     ~MT9V034Alternate()
00439     {
00440     }
00441     
00442     MT9V034Alternate(IpCamList &cam, MT9V034 *parent) : MT9VImagerImpl(cam)
00443     {
00444       model_ = "MT9V034";
00445       agc_aec_mode_cache_ = &parent->agc_aec_mode_cache_backing_;
00446       companding_mode_cache_ = &parent->companding_mode_cache_backing_;
00447       agc_aec_enable_shift_ = 8;
00448       companding_mode_shift_ = 8;
00449       reg_column_start_ = 0xC9;
00450       reg_row_start_ = 0x0CA;
00451       reg_window_width_ = 0xCC;
00452       reg_window_height_ = 0xCB;
00453       reg_hblank_ = 0xCD;
00454       reg_vblank_ = 0xCE;
00455       reg_shutter_width_ = 0xD2;
00456       reg_max_shutter_width_ = 0xAD; // FIXME Yes, they went and used the same max frame rate for both contexts! Why???
00457       reg_analog_gain_ = 0x36;
00458       reg_agc_aec_enable_ = 0xAF;
00459       reg_read_mode_ = 0x0E;
00460     }
00461   };
00462 
00463   MT9V034(IpCamList &cam) : MT9VImagerImpl(cam)
00464   {
00465     ROS_DEBUG("Found MT9V034 imager.");
00466     model_ = "MT9V034";
00467     max_max_shutter_width_ = 32765;
00468     max_shutter_width_ = 32765;
00469     reg_max_shutter_width_ = 0xAD;
00470     *agc_aec_mode_cache_ = 0x303;
00471     *companding_mode_cache_ = 0x302;
00472     alternate_ = MT9VImagerPtr(new MT9V034Alternate(cam, this));
00473 
00474     if (wge100ReliableSensorWrite(&camera_, 0x18, 0x3e3a, NULL) || 
00475         wge100ReliableSensorWrite(&camera_, 0x15, 0x7f32, NULL) ||
00476         wge100ReliableSensorWrite(&camera_, 0x20, 0x01c1, NULL) ||
00477         wge100ReliableSensorWrite(&camera_, 0x21, 0x0018, NULL))
00478     {
00479       ROS_WARN("Error setting magic sensor register.");
00480     }
00481     if (wge100ReliableSensorWrite(&camera_, 0x70, 0x000, NULL) != 0)
00482     {
00483       ROS_WARN("Error turning off row-noise correction.");
00484     }
00485     if (wge100ReliableSensorWrite(&camera_, 0x3A, 0x001A, NULL))
00486     {
00487       ROS_WARN("Error setting V2 Voltge level for context B.");
00488     }
00489     if (wge100ReliableSensorWrite(&camera_, 0x0F, 0x0000, NULL))
00490     {
00491       ROS_WARN("Error turning off high dynamic range for alternate configuration.");
00492     }
00493     if (wge100ReliableSensorWrite(&camera_, 0xD1, 0x0164, NULL))
00494     {
00495       ROS_WARN("Error turning on exposure knee auto-adjust for alternate configuration.");
00496     }
00497   }  
00498 
00499   virtual ~MT9V034()
00500   {
00501   }
00502 };
00503 
00504 class MT9V032 : public MT9VImagerImpl
00505 {
00506 public:
00507   MT9V032(IpCamList &cam) : MT9VImagerImpl(cam)
00508   {
00509     ROS_DEBUG("Found MT9V032 imager.");
00510     model_ = "MT9V032";
00511     if (wge100ReliableSensorWrite(&camera_, 0x18, 0x3e3a, NULL) || 
00512         wge100ReliableSensorWrite(&camera_, 0x15, 0x7f32, NULL) ||
00513         wge100ReliableSensorWrite(&camera_, 0x20, 0x01c1, NULL) ||
00514         wge100ReliableSensorWrite(&camera_, 0x21, 0x0018, NULL))
00515     {
00516       ROS_WARN("Error setting magic sensor register.");
00517     }
00518     if (wge100ReliableSensorWrite(&camera_, 0x70, 0x14, NULL) != 0)
00519     {
00520       ROS_WARN("Error turning off row-noise correction");
00521     }
00522   }
00523 
00524   virtual ~MT9V032()
00525   {
00526   }
00527 };
00528 
00529 MT9VImagerPtr MT9VImager::getInstance(IpCamList &cam)
00530 {
00531   uint16_t imager_version;
00532   if (wge100ReliableSensorRead(&cam, 0x00, &imager_version, NULL))
00533   {
00534     ROS_ERROR("MT9VImager::getInstance Unable to read imager version.");
00535     return MT9VImagerPtr();
00536   }
00537 
00538   switch (imager_version)
00539   {
00540     case 0x1324:
00541       return MT9VImagerPtr(new MT9V034(cam));
00542 
00543     default:
00544       ROS_ERROR("MT9VImager::getInstance Unknown imager version 0x%04x. Assuming it is compatible with MT9V032.", imager_version);
00545       return MT9VImagerPtr();
00546     
00547     case 0x1311:
00548     case 0x1313:
00549       return MT9VImagerPtr(new MT9V032(cam));
00550   }
00551 }
00552 
00553 


wge100_camera
Author(s): Blaise Gassend, Patrick Mihelich, Eric MacIntosh, David Palchak
autogenerated on Sat Jun 8 2019 20:51:24