00001
00002 /***************************************************************************
00003  *  sony_evid100p_control.cpp - Controller for Sony EVI-D100P
00004  *
00005  *  Created: Tue Jun 07 19:27:20 2005
00006  *  Copyright  2005-2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023
00024 #include <cams/control/sony_evid100p.h>
00025 #include <cams/control/visca.h>
00026 #include <fvutils/system/camargp.h>
00027
00028 #include <utils/math/angle.h>
00029
00030 #include <termios.h>
00031 #include <cstring>
00032 #include <cstdlib>
00033
00034 using namespace std;
00035 using namespace fawkes;
00036 
00037 /** Maximum pan. */
00038 const int   SonyEviD100PControl::MAX_PAN       =  1440;
00039 /** Minimum pan. */
00040 const int   SonyEviD100PControl::MIN_PAN       = -1439;
00041 /** Max Tilt. */
00042 const int   SonyEviD100PControl::MAX_TILT      =   360;
00043 /** Min tilt .*/
00044 const int   SonyEviD100PControl::MIN_TILT      = - 359;
00045 
00046 /** Max pan in degrees. */
00047 const float SonyEviD100PControl::MAX_PAN_DEG   =  100.f;
00048 /** Min pan in degrees. */
00049 const float SonyEviD100PControl::MIN_PAN_DEG   = -100.f;
00050 /** Max tilt in degrees. */
00051 const float SonyEviD100PControl::MAX_TILT_DEG  =   25.f;
00052 /** Min tilt in degrees. */
00053 const float SonyEviD100PControl::MIN_TILT_DEG  = - 25.f;
00054 
00055 /** Max pan in rad. */
00056 const float SonyEviD100PControl::MAX_PAN_RAD   = deg2rad(MAX_PAN_DEG);
00057 /** Min pan in rad. */
00058 const float SonyEviD100PControl::MIN_PAN_RAD   = deg2rad(MIN_PAN_DEG);
00059 /** Max tilt in rad. */
00060 const float SonyEviD100PControl::MAX_TILT_RAD  = deg2rad(MAX_TILT_DEG);
00061 /** Min tilt in rad. */
00062 const float SonyEviD100PControl::MIN_TILT_RAD  = deg2rad(MIN_TILT_DEG);
00063 
00064 /** Pan steps per degree */
00065 const float SonyEviD100PControl::PAN_STEPS_PER_DEG  = MAX_PAN  / MAX_PAN_DEG;
00066 /** Tilt steps per degree */
00067 const float SonyEviD100PControl::TILT_STEPS_PER_DEG = MAX_TILT / MAX_TILT_DEG;
00068 
00069 /** Pan steps per rad */
00070 const float SonyEviD100PControl::PAN_STEPS_PER_RAD  = MAX_PAN  / MAX_PAN_RAD;
00071 /** Tilt steps per rad */
00072 const float SonyEviD100PControl::TILT_STEPS_PER_RAD = MAX_TILT / MAX_TILT_RAD;
00073 
00074 /** Pastel effect. */
00075 const unsigned int SonyEviD100PControl::EFFECT_PASTEL   = 1;
00076 /** Negative effect. */
00077 const unsigned int SonyEviD100PControl::EFFECT_NEGATIVE = 2;
00078 /** Sepia effect. */
00079 const unsigned int SonyEviD100PControl::EFFECT_SEPIA    = 3;
00080 /** B/W effect. */
00081 const unsigned int SonyEviD100PControl::EFFECT_BW       = 4;
00082 /** Solarize effect. */
00083 const unsigned int SonyEviD100PControl::EFFECT_SOLARIZE = 5;
00084 /** Mosaic effect. */
00085 const unsigned int SonyEviD100PControl::EFFECT_MOSAIC   = 6;
00086 /** Slim effect. */
00087 const unsigned int SonyEviD100PControl::EFFECT_SLIM     = 7;
00088 /** Stretch effect. */
00089 const unsigned int SonyEviD100PControl::EFFECT_STRETCH  = 8;
00090
00091 
00092 /** @class SonyEviD100PControl <cams/control/sony_evid100p.h>
00093  * Sony Evi D100P pan/tilt control.
00094  * Internally uses Visca.
00095  * @author Tim Niemueller
00096  */
00097
00098 
00099 /** Constructor.
00100  * @param tty_port serial port (e.g. /dev/ttyS0)
00101  */
00102 SonyEviD100PControl::SonyEviD100PControl(const char *tty_port)
00103 {
00104   this->tty_port = strdup(tty_port);
00105   visca = new ViscaControl( /* non-blocking */ false );
00106   opened = false;
00107   pan_target = 0;
00108   tilt_target = 0;
00109   _effect = EFFECT_NONE;
00110
00111   open();
00112 }
00113
00114 
00115 /** Constructor.
00116  * Uses camera argument parser to gather arguments. The ID that the camera argument
00117  * parser returns is used as the serial port (like /dev/ttyS0).
00118  * @param cap camera argument parser
00119  */
00120 SonyEviD100PControl::SonyEviD100PControl(const CameraArgumentParser *cap)
00121 {
00122   tty_port = strdup(cap->cam_id().c_str());
00123
00124   visca = new ViscaControl( /* non-blocking */ false );
00125   opened = false;
00126   pan_target = 0;
00127   tilt_target = 0;
00128   _effect = EFFECT_NONE;
00129
00130   open();
00131 }
00132
00133 
00134 /** Destructor. */
00135 SonyEviD100PControl::~SonyEviD100PControl()
00136 {
00137   close();
00138   delete visca;
00139   free(tty_port);
00140 }
00141
00142 
00143 /** Open visca device.
00144  * @return true on success
00145  */
00146 void
00147 SonyEviD100PControl::open()
00148 {
00149   if (opened) return;
00150
00151   try {
00152     visca->open(tty_port);
00153     visca->set_address(1);
00154     visca->clear();
00155   } catch (ViscaControlException &e) {
00156     visca->close();
00157     e.append("Sony EviD100PControl failed");
00158     throw;
00159   }
00160
00161   opened = true;
00162 }
00163
00164 
00165 /** Close Visca device.
00166  */
00167 void
00168 SonyEviD100PControl::close()
00169 {
00170   if ( ! opened ) return;
00171   visca->close();
00172 }
00173
00174
00175 void
00176 SonyEviD100PControl::process_pantilt()
00177 {
00178   visca->process();
00179 }
00180
00181
00182 bool
00183 SonyEviD100PControl::supports_pan()
00184 {
00185   return true;
00186 }
00187
00188
00189 bool
00190 SonyEviD100PControl::supports_tilt()
00191 {
00192   return true;
00193 }
00194
00195
00196 void
00197 SonyEviD100PControl::set_pan(int pan)
00198 {
00199   pan_target = pan;
00200   visca->setPanTilt(pan, tilt_target);
00201 }
00202
00203
00204 void
00205 SonyEviD100PControl::set_tilt(int tilt)
00206 {
00207   tilt_target = tilt;
00208   visca->setPanTilt(pan_target, tilt);
00209 }
00210
00211
00212 void
00213 SonyEviD100PControl::set_pan_tilt(int pan, int tilt)
00214 {
00215   pan_target  = pan;
00216   tilt_target = tilt;
00217   visca->setPanTilt(pan, tilt);
00218 }
00219
00220
00221 void
00222 SonyEviD100PControl::set_pan_tilt_rad(float pan, float tilt)
00223 {
00224   int tpan = 0, ttilt = 0;
00225
00226   tpan = (int)rint(  pan  * PAN_STEPS_PER_RAD  );
00227   ttilt = (int)rint( tilt * TILT_STEPS_PER_RAD );
00228
00229   set_pan_tilt(tpan, ttilt);
00230 }
00231
00232
00233 void
00234 SonyEviD100PControl::start_get_pan_tilt()
00235 {
00236   visca->startGetPanTilt();
00237 }
00238
00239
00240 void
00241 SonyEviD100PControl::pan_tilt(int &pan, int &tilt)
00242 {
00243   int tpan, ttilt;
00244   visca->getPanTilt(&tpan, &ttilt);
00245   pan  = tpan;
00246   tilt = ttilt;
00247 }
00248
00249
00250 void
00251 SonyEviD100PControl::pan_tilt_rad(float &pan, float &tilt)
00252 {
00253   int tpan = 0, ttilt = 0;
00254   visca->getPanTilt(&tpan, &ttilt);
00255
00256   pan  = tpan  / PAN_STEPS_PER_RAD;
00257   tilt = ttilt / PAN_STEPS_PER_RAD;
00258 }
00259
00260
00261 int
00262 SonyEviD100PControl::pan()
00263 {
00264   int pan = 0, tilt = 0;
00265   visca->getPanTilt(&pan, &tilt);
00266   return pan;
00267 }
00268
00269
00270 int
00271 SonyEviD100PControl::tilt()
00272 {
00273   int pan = 0, tilt = 0;
00274   visca->getPanTilt(&pan, &tilt);
00275   return tilt;
00276 }
00277
00278
00279 int
00280 SonyEviD100PControl::max_pan()
00281 {
00282   return MAX_PAN;
00283 }
00284
00285
00286 int
00287 SonyEviD100PControl::min_pan()
00288 {
00289   return MIN_PAN;
00290 }
00291
00292
00293 int
00294 SonyEviD100PControl::max_tilt()
00295 {
00296   return MAX_TILT;
00297 }
00298
00299
00300 int
00301 SonyEviD100PControl::min_tilt()
00302 {
00303   return MIN_TILT;
00304 }
00305
00306
00307 void
00308 SonyEviD100PControl::reset_pan_tilt()
00309 {
00310   visca->resetPanTilt();
00311 }
00312
00313
00314 void
00315 SonyEviD100PControl::set_pan_tilt_limit(int pan_left, int pan_right,
00316                                      int tilt_up, int tilt_down)
00317 {
00318   visca->setPanTiltLimit(pan_left, pan_right, tilt_up, tilt_down);
00319 }
00320
00321
00322 void
00323 SonyEviD100PControl::reset_pan_tilt_limit()
00324 {
00325   visca->resetPanTiltLimit();
00326 }
00327
00328
00329 void
00330 SonyEviD100PControl::reset_zoom()
00331 {
00332   visca->resetZoom();
00333 }
00334
00335
00336 void
00337 SonyEviD100PControl::set_zoom(unsigned int zoom)
00338 {
00339   visca->setZoom(zoom);
00340 }
00341
00342
00343 unsigned int
00344 SonyEviD100PControl::zoom()
00345 {
00346   unsigned int zoom;
00347   visca->getZoom(&zoom);
00348   return zoom;
00349 }
00350
00351
00352 unsigned int
00353 SonyEviD100PControl::zoom_min()
00354 {
00355   return 0;
00356 }
00357
00358
00359 unsigned int
00360 SonyEviD100PControl::zoom_max()
00361 {
00362   return 0x4000;
00363 }
00364
00365
00366 void
00367 SonyEviD100PControl::set_zoom_speed_tele(unsigned int speed)
00368 {
00369   visca->setZoomSpeedTele(speed);
00370 }
00371
00372
00373 void
00374 SonyEviD100PControl::set_zoom_speed_wide(unsigned int speed)
00375 {
00376   visca->setZoomSpeedWide(speed);
00377 }
00378
00379
00380 void
00381 SonyEviD100PControl::set_zoom_digital_enabled(bool enabled)
00382 {
00383   visca->setZoomDigitalEnabled(enabled);
00384 }
00385
00386
00387 bool
00388 SonyEviD100PControl::supports_effect(unsigned int __effect)
00389 {
00390   if ( __effect == EFFECT_NONE ) {
00391     return true;
00392   }
00393
00394   switch (__effect) {
00395   case EFFECT_PASTEL:
00396   case EFFECT_NEGATIVE:
00397   case EFFECT_SEPIA:
00398   case EFFECT_BW:
00399   case EFFECT_SOLARIZE:
00400   case EFFECT_MOSAIC:
00401   case EFFECT_SLIM:
00402   case EFFECT_STRETCH:
00403     return true;
00404     break;
00405   default:
00406     return false;
00407   }
00408 }
00409
00410
00411 void
00412 SonyEviD100PControl::set_effect(unsigned int __effect)
00413 {
00414   this->_effect = __effect;
00415   if ( __effect == EFFECT_NONE ) {
00416     visca->resetEffect();
00417   }
00418   switch (__effect) {
00419   case EFFECT_PASTEL:
00420     visca->applyEffectPastel();
00421     break;
00422   case EFFECT_NEGATIVE:
00423     visca->applyEffectNegArt();
00424     break;
00425   case EFFECT_SEPIA:
00426     visca->applyEffectSepia();
00427     break;
00428   case EFFECT_BW:
00429     visca->applyEffectBnW();
00430     break;
00431   case EFFECT_SOLARIZE:
00432     visca->applyEffectSolarize();
00433     break;
00434   case EFFECT_MOSAIC:
00435     visca->applyEffectMosaic();
00436     break;
00437   case EFFECT_SLIM:
00438     visca->applyEffectSlim();
00439     break;
00440   case EFFECT_STRETCH:
00441     visca->applyEffectStretch();
00442     break;
00443   default:
00444     break;
00445   }
00446
00447 }
00448
00449
00450 unsigned int
00451 SonyEviD100PControl::effect()
00452 {
00453   return _effect;
00454 }
00455
00456
00457 void
00458 SonyEviD100PControl::reset_effect()
00459 {
00460   visca->resetEffect();
00461 }
00462
00463 
00464 /** Get current white balance mode.
00465  * @return white balance mode
00466  */
00467 unsigned int
00468 SonyEviD100PControl::white_balance_mode()
00469 {
00470   return visca->getWhiteBalanceMode();
00471 }