00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00074 int max_max_shutter_width_;
00075 int max_shutter_width_;
00076
00077
00078 int agc_aec_enable_shift_;
00079 int companding_mode_shift_;
00080
00081
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
00131
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
00286
00287
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++)
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)
00323 vblank = 40;
00324 if (vblank > 3000)
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;
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