CanonDriver.cpp
Go to the documentation of this file.
00001 
00002 #include <stdlib.h>
00003 #include <stdio.h>
00004 #include <string.h>
00005 #include <assert.h>
00006 #include <math.h>
00007 #include "canon_vbc50i/libCanon/CanonDriver.h"
00008 
00009 CanonDriver::CanonDriver(const char * initseq_filename,
00010                 const char * hostname) : cm(initseq_filename)
00011 {
00012         host = strdup(hostname);
00013         connected = false;
00014         verbose = 0;
00015 }
00016 
00017 CanonDriver::~CanonDriver() 
00018 {
00019         disconnect();
00020         stopVideoReception();
00021         free(host); host = NULL;
00022 }
00023 
00024 
00025 
00026 bool CanonDriver::connect()
00027 {
00028         cm.setVerboseLevel(verbose);
00029         if (!cm.open(host,CONTROL_PORT)) {
00030                 printf("%s: Can't open CommManager '%s:%d'\n",__FUNCTION__,
00031                                 host,CONTROL_PORT);
00032                 return false;
00033         }
00034         connected = true;
00035         return requestCurrentPos();     
00036 }
00037 
00038 bool CanonDriver::startVideoReception(VideoManager::SignalF f,
00039                 void * farg)
00040 {
00041         return startVideoReception(NULL,f,farg);
00042 }
00043 
00044 bool CanonDriver::startVideoReception(unsigned char * dst, 
00045                 VideoManager::SignalF f, void * farg)
00046 {
00047         vm.setDestination(dst);
00048         vm.setRecordingBasename(host);
00049         vm.setSignalFunction(f,farg);
00050 
00051         printf("Opening video reception\n");
00052         if (!vm.open(host,VIDEO_PORT)) {
00053                 printf("%s: Can't open VideoManager '%s:%d'\n",__FUNCTION__,
00054                                 host,VIDEO_PORT);
00055                 return false;
00056         }
00057         vm.setVerboseLevel(verbose);
00058         return true;
00059 }
00060 
00061 bool CanonDriver::stopVideoReception()
00062 {
00063         return vm.close();
00064 }
00065 
00066 bool CanonDriver::setVideoReceptionPauseStatus(bool pause)
00067 {
00068         if (vm.isPaused() == pause) 
00069                 return true;
00070         if (vm.isPaused()) 
00071                 return vm.resume();
00072         else
00073                 return vm.pause();
00074 }
00075 
00076 bool CanonDriver::getVideoReceptionPauseStatus()
00077 {
00078         return vm.isPaused();
00079 }
00080 
00081 bool CanonDriver::pauseVideoReception()
00082 {
00083         return vm.pause();
00084 }
00085 
00086 bool CanonDriver::resumeVideoReception()
00087 {
00088         return vm.resume();
00089 }
00090 
00091 bool CanonDriver::disconnect()
00092 {
00093         //printf("Sending 0x21\n");
00094         if (connected) {
00095                 if (!sendAndWait(0x21,0x0000)) {
00096                         if (verbose) 
00097                                 fprintf(stderr,"Failed to send 0x21\n");
00098                 }
00099         }
00100         //printf("Sent 0x21\n");
00101         cm.close();
00102         //printf("Close comm manager\n");
00103         connected = false;
00104 
00105         stopVideoReception();
00106         //printf("Close video manager\n");
00107 
00108         return true;
00109 }
00110 
00111 
00112 bool CanonDriver::moveTo(double pan, double tilt, double zoom)
00113 {
00114         unsigned char data[6];
00115         signed short span,stilt,szoom;
00116         //printf("P %f T %f Z %f\n",pan,tilt,zoom);
00117         if (pan < -170) pan = -170;
00118         if (pan > +170) pan = +170;
00119         if (tilt < -90) tilt = -90;
00120         if (tilt > 10) tilt = 10;
00121         if (zoom < 1.97) zoom = 1.97;
00122         if (zoom > 41.26) zoom = 41.26;
00123         //printf("P %f T %f Z %f\n",pan,tilt,zoom);
00124 
00125         span = (signed short)round(pan*100.0);
00126         stilt = (signed short)round(tilt*100.0);
00127         szoom = (signed short)round(zoom*100.0);
00128 
00129         unsigned short upan,utilt,uzoom;
00130         upan = (unsigned short)span;
00131         utilt = (unsigned short)stilt;
00132         uzoom = (unsigned short)szoom;
00133         //printf("P %02X T %02X Z %02X\n",upan,utilt,uzoom);
00134 
00135         data[0] = upan >> 8;
00136         data[1] = (upan & 0xFF);
00137         data[2] = utilt >> 8;
00138         data[3] = (utilt & 0xFF);
00139         data[4] = uzoom >> 8;
00140         data[5] = (uzoom & 0xFF);
00141 
00142         bool res = sendAndWait(0x33,0x00e0,data,6);
00143         if (!res && (verbose > 0)) printf("Move To Failed\n");
00144         return res;
00145 }
00146 
00147 bool CanonDriver::panTo(double pan)
00148 {
00149         requestCurrentPos();
00150         return moveTo(pan,ctilt,czoom);
00151 }
00152 
00153 bool CanonDriver::tiltTo(double tilt)
00154 {
00155         requestCurrentPos();
00156         return moveTo(cpan,tilt,czoom);
00157 }
00158 
00159 bool CanonDriver::zoomTo(double zoom)
00160 {
00161         requestCurrentPos();
00162         return moveTo(cpan,ctilt,zoom);
00163 }
00164 
00165 bool CanonDriver::center()
00166 {
00167         requestCurrentPos();
00168         return moveTo(0,0,czoom);
00169 }
00170 
00171 bool CanonDriver::startMoving(Direction dir) 
00172 {
00173         unsigned char data = dir;
00174         return sendAndWait(0x3C,0x0000,&data,1);
00175 }
00176 
00177 
00178 bool CanonDriver::stop() 
00179 {
00180         return sendAndWait33(0x3C,0x0000);
00181 }
00182 
00183 bool CanonDriver::requestCurrentPos()
00184 {
00185         Datagram dgm;
00186         dgm.reset();
00187         dgm.setId(0x33);
00188         dgm.setStatus(0x8000);
00189 
00190         cm.addToReceptionField(0x33);
00191         if (!cm.send(dgm)) {
00192                 if (verbose) {
00193                         printf("%s: Can't send datagram: ",__FUNCTION__);
00194                         dgm.print(stdout);
00195                         printf("\n");
00196                 }
00197                 goto cleanup;
00198         }
00199 
00200         if (!cm.wait(0x33,dgm)) goto cleanup;
00201         cm.removeFromReceptionField(0x33);
00202         return interpreteDatagram33(dgm);
00203 
00204 cleanup:
00205         if (verbose)
00206                 printf("requestCurrentPos failed\n");
00207         cm.removeFromReceptionField(0x33);
00208         return false;
00209 }
00210 
00211 void CanonDriver::getCurrentPos(double * pan, double * tilt, double * zoom)
00212 {
00213         requestCurrentPos();
00214         if (pan != NULL) *pan = cpan ;
00215         if (tilt != NULL) *tilt = ctilt ;
00216         if (zoom != NULL) *zoom = czoom ;
00217         //printf("At position: %f %f %f\n", cpan,ctilt,czoom);
00218 }
00219 
00220 bool CanonDriver::startZooming() 
00221 {
00222         unsigned char data = 0x01;
00223         return sendAndWait(0x3D,0x0000,&data,1);
00224 }
00225 
00226 bool CanonDriver::stopZooming() 
00227 {
00228         return sendAndWait33(0x3D,0x0000);
00229 }
00230 
00231 bool CanonDriver::startDeZooming() 
00232 {
00233         unsigned char data = 0x02;
00234         return sendAndWait(0x3D,0x0000,&data,1);
00235 }
00236 
00237 
00238 bool CanonDriver::setPanSpeed(unsigned short speed) 
00239 {
00240         unsigned char data[8] = {
00241                 0x00, 0x00, 0x00, 0x00,
00242                 0x00, 0x00, 0x80, 0x00
00243         };
00244         data[0] = speed >> 8;
00245         data[1] = speed & 0xFF;
00246         return sendAndWait(0x3B,0x0000,data,8);
00247 }
00248 
00249 bool CanonDriver::setTiltSpeed(unsigned short speed) 
00250 {
00251         unsigned char data[8] = {
00252                 0x00, 0x00, 0x00, 0x00,
00253                 0x00, 0x00, 0x40, 0x00
00254         };
00255         data[2] = speed >> 8;
00256         data[3] = speed & 0xFF;
00257         return sendAndWait(0x3B,0x0000,data,8);
00258 }
00259 
00260 bool CanonDriver::setZoomSpeed(unsigned short speed) 
00261 {
00262         unsigned char data[8] = {
00263                 0x00, 0x00, 0x00, 0x00,
00264                 0x00, 0x00, 0x20, 0x00
00265         };
00266         data[4] = speed >> 8;
00267         data[5] = speed & 0xFF;
00268         return sendAndWait(0x3B,0x0000,data,8);
00269 }
00270 
00271 bool CanonDriver::setMaxSpeed()
00272 {
00273         unsigned char data[8] = {
00274                 0x08, 0x84, 0x06, 0x63,
00275                 0x00, 0x07, 0xE0, 0x00
00276         };
00277         return sendAndWait(0x3B,0x0000,data,8);
00278 }
00279 
00280 bool CanonDriver::setMinSpeed()
00281 {
00282         unsigned char data[8] = {
00283                 0x02, 0x21, 0x01, 0x98,
00284                 0x00, 0x00, 0xE0, 0x00
00285         };
00286         return sendAndWait(0x3B,0x0000,data,8);
00287 }
00288 
00289 bool CanonDriver::getSpeeds(unsigned short * pan, 
00290                 unsigned short * tilt, unsigned short * zoom)
00291 {
00292         Datagram dgm;
00293         if (!sendRequestAndWait(0x3B,0x8000,dgm))
00294                 return false;
00295         const unsigned char * data = dgm.bdata();
00296         *pan = data[0];
00297         *pan = (*pan << 8) | data[1];
00298         *tilt = data[2];
00299         *tilt = (*tilt << 8) | data[3];
00300         *zoom = data[4];
00301         *zoom = (*zoom << 8) | data[5];
00302         return true;
00303 }
00304 
00305 bool CanonDriver::setFocusMode(FocusMode fm, ManualFocusMode mfm)
00306 {
00307         unsigned char data = fm;
00308         if (!sendAndWait(0x40,0x0000,&data,1)) return false;
00309 
00310         if ((fm == FMManual) && (mfm != FMUndef)) {
00311                 // This sequence used in the win app
00312                 data = 0;
00313                 if (!sendAndWait(0x43,0x0000,&data,1)) return false;
00314                 data = mfm;
00315                 if (!sendAndWait(0x43,0x0000,&data,1)) return false;
00316                 data = 0;
00317                 if (!sendAndWait(0x43,0x0000,&data,1)) return false;
00318         }
00319 
00320         return true;
00321 
00322 }
00323 
00324 bool CanonDriver::getFocusMode(FocusMode * fm, ManualFocusMode * mfm)
00325 {
00326         Datagram dgm;
00327         if (!sendRequestAndWait(0x40,0x8000,dgm))
00328                 return false;
00329         *fm = (FocusMode)(dgm.bdata()[0]);
00330         if (*fm == FMManual) {
00331                 if (!sendRequestAndWait(0x43,0x8000,dgm))
00332                         return false;
00333                 *mfm = (ManualFocusMode)(dgm.bdata()[0]);
00334         }
00335         return true;
00336 }
00337 
00338 bool CanonDriver::getAutoExposure(bool * autoexp, AutoExposureMode * aem)
00339 {
00340         Datagram dgm;
00341         if (!sendRequestAndWait(0x82,0x8000,dgm))
00342                 return false;
00343         //dgm.print();
00344         *autoexp = (dgm.bdata()[0] == 0x00);
00345         if (!*autoexp) {
00346                 *aem = AXPundef;
00347                 return true;
00348         }
00349         if (!sendRequestAndWait(0x87,0x8000,dgm))
00350                 return false;
00351         unsigned short us = dgm.bdata()[0];
00352         us = (us << 8) | dgm.bdata()[1];
00353         *aem = (AutoExposureMode)(us);
00354         //dgm.print();
00355         return true;
00356 }
00357 
00358 
00359 bool CanonDriver::setAutoExposure(AutoExposureMode aem)
00360 {
00361         if (!sendAndWait(0x82,0x0000)) return false;
00362         if (aem != AXPundef) {
00363                 unsigned char data[2];
00364                 data[0] = ((unsigned int)aem) >> 8;
00365                 data[1] = ((unsigned int)aem) & 0xFF;
00366                 if (!sendAndWait(0x87,0x0000,data,2)) return false;
00367         }
00368         return true;
00369 }
00370 
00371 bool CanonDriver::setManualExposure()
00372 {
00373         return setExposureParameters(0x07,0x64,0x80);
00374 }
00375 
00376 bool CanonDriver::setExposureParameters(unsigned int aperture, 
00377                 unsigned int inv_shutter, unsigned int gain)
00378 {
00379         unsigned char data[16] = {
00380                 0xE0, 0x00, 0x00, 0x00,
00381                 0x00, 0x00, 0x00, 0x00,
00382                 0x00, 0x00, 0x00, 0x00,
00383                 0x00, 0x00, 0x00, 0x00
00384         };
00385         if ((aperture<1) || (aperture >15)) return false;
00386         if ((gain<1) || (gain >256)) return false;
00387         if ((inv_shutter<1) || (inv_shutter >250)) return false;
00388         data[7] = aperture;
00389         data[11] = inv_shutter;
00390         data[14] = gain >> 8;
00391         data[15] = gain & 0xFF;
00392         bool r = sendAndWait(0x82,0x0000,data,16);
00393         return r;
00394 }
00395 
00396 bool CanonDriver::getExposureParameters(unsigned int * aperture, 
00397                 unsigned int * inv_shutter, unsigned int * gain)
00398 {
00399         unsigned char c = 0xE0;
00400         Datagram dgm;
00401         if (!sendRequestAndWait(0x82,0x8000, dgm,&c,1)) return false;
00402         const unsigned char * data = dgm.bdata();
00403         //dgm.print();
00404         *aperture = data[7];
00405         *inv_shutter = data[11];
00406         *gain = data[14];
00407         *gain = (*gain << 8) | data[15];
00408         //printf("a %d is %d g %d\n",*aperture,*inv_shutter,*gain);
00409         return true;
00410 }
00411 
00412 bool CanonDriver::setDigitalZoom(unsigned char zoom)
00413 {
00414         return sendAndWait(0x88,0x0000,&zoom,1);
00415 }
00416 
00417 bool CanonDriver::setNightMode(bool activated)
00418 {
00419         unsigned char data = (activated)?0x01:0x00;
00420         return sendAndWait(0x85,0x0000,&data,1);
00421 }
00422 
00423 bool CanonDriver::setInfraRed(bool activated)
00424 {
00425         unsigned char data = (activated)?0x01:0x00;
00426         return sendAndWait(0x86,0x0000,&data,1);
00427 }
00428 
00429 
00430 unsigned char CanonDriver::getDigitalZoom()
00431 {
00432         Datagram dgm;
00433         if (!sendRequestAndWait(0x88,0x8000,dgm)) return 0;
00434         //dgm.print();
00435         return dgm.bdata()[1];
00436 }
00437 
00438 bool CanonDriver::getNightMode()
00439 {
00440         Datagram dgm;
00441         if (!sendRequestAndWait(0x85,0x8000,dgm)) return false;
00442         //dgm.print(stdout);
00443         return dgm.bdata()[0] == 0x01;
00444 }
00445 
00446 bool CanonDriver::getInfraRed()
00447 {
00448         Datagram dgm;
00449         if (!sendRequestAndWait(0x86,0x8000,dgm)) return false;
00450         return dgm.bdata()[0] == 0x01;
00451 }
00452 
00453 bool CanonDriver::setImageSize(unsigned int width, unsigned int height)
00454 {
00455         return vm.setImageSize(width,height);
00456 }
00457 
00458 bool CanonDriver::getImageSize(unsigned int *width, unsigned int *height)
00459 {
00460         return vm.getImageSize(width,height);
00461 }
00462 
00463 bool CanonDriver::sendRequestAndWait(unsigned char id, 
00464                 unsigned short status, Datagram & dgm,
00465                 unsigned char * data, unsigned int datasize)
00466 {
00467         Datagram dgmreq;
00468         dgmreq.reset();
00469         dgmreq.setId(id);
00470         dgmreq.setStatus(status);
00471         if (data != NULL) {
00472                 dgmreq.setData(data,datasize);
00473         }
00474 
00475         //printf("Sending: ");dgmreq.print(stdout);
00476         cm.addToReceptionField(id);
00477         if (!cm.send(dgmreq)) {
00478                 if (verbose) {
00479                         printf("%s: Can't send datagram: ",__FUNCTION__);
00480                         dgm.print(stdout);
00481                         printf("\n");
00482                 }
00483                 goto cleanup;
00484         }
00485         dgm.reset();
00486         if (!cm.wait(id,dgm,1.0)) {
00487                 if (verbose) printf("Failed to receive reply dgm %02X\n",id);
00488                 goto cleanup;
00489         }
00490         cm.removeFromReceptionField(id);
00491         return true;
00492 cleanup:
00493         cm.removeFromReceptionField(id);
00494         return false;
00495 }
00496 
00497 bool CanonDriver::sendAndWait(unsigned char id, unsigned short status, 
00498                 unsigned char * data, unsigned int datasize)
00499 {
00500         Datagram dgm;
00501         dgm.reset();
00502         dgm.setId(id);
00503         dgm.setStatus(status);
00504         if (data != NULL) {
00505                 dgm.setData(data,datasize);
00506         }
00507 
00508 
00509         cm.addToReceptionField(id);
00510         if (!cm.send(dgm)) {
00511                 if (verbose) {
00512                         printf("%s: Can't send datagram: ",__FUNCTION__);
00513                         dgm.print(stdout);
00514                         printf("\n");
00515                 }
00516                 goto cleanup;
00517         }
00518         if (!cm.wait(id,dgm,1.0)) {
00519                 if (verbose > 1)
00520                         printf("SaW: Failed to receive ack dgm %02X\n",id);
00521                 goto cleanup;
00522         }
00523         if (verbose) {printf("Received dgm %02X\n",id); dgm.print();}
00524         cm.removeFromReceptionField(id);
00525         return true;
00526 cleanup:
00527         cm.removeFromReceptionField(id);
00528         return false;
00529 }
00530 
00531 bool CanonDriver::sendAndWait33(unsigned char id, unsigned short status, 
00532                 unsigned char * data, unsigned int datasize)
00533 {
00534         Datagram dgm;
00535         dgm.reset();
00536         dgm.setId(id);
00537         dgm.setStatus(status);
00538         if (data != NULL) {
00539                 dgm.setData(data,datasize);
00540         }
00541 
00542 
00543         cm.addToReceptionField(id);
00544         cm.addToReceptionField(0x33);
00545         if (!cm.send(dgm)) {
00546                 if (verbose) {
00547                         printf("%s: Can't send datagram: ",__FUNCTION__);
00548                         dgm.print(stdout);
00549                         printf("\n");
00550                 }
00551                 goto cleanup;
00552         }
00553         if (!cm.wait(id,dgm,1.0)) {
00554                 if (verbose > 1)
00555                         printf("SaW33-1: Failed to receive ack dgm %02X\n",id);
00556                 goto cleanup;
00557         }
00558         if (!cm.wait(0x33,dgm,1.0)) {
00559                 if (verbose > 1)
00560                         printf("SaW33-2: Failed to receive ack dgm %02X\n",0x33);
00561                 goto cleanup;
00562         }
00563         cm.removeFromReceptionField(id);
00564         cm.removeFromReceptionField(0x33);
00565 
00566         return interpreteDatagram33(dgm);
00567 cleanup:
00568         cm.removeFromReceptionField(id);
00569         cm.removeFromReceptionField(0x33);
00570         return false;
00571 }
00572 
00573 bool CanonDriver::interpreteDatagram33(const Datagram & dgm)
00574 {
00575         if (dgm.getId() != 0x33) return false;
00576         const unsigned char * rdata = dgm.bdata();
00577         cpan = (signed short)(((unsigned short)(rdata[0]) << 8) | rdata[1]);
00578         cpan /= 100.0;
00579         ctilt = (signed short)(((unsigned short)(rdata[2]) << 8) | rdata[3]);
00580         ctilt /= 100.0;
00581         czoom = (signed short)(((unsigned short)(rdata[4]) << 8) | rdata[5]);
00582         czoom /= 100.0;
00583         return true;
00584 }
00585 
00586 bool CanonDriver::sendDatagram(const std::vector<unsigned char> & dgm)
00587 {
00588         Datagram dgram;
00589         dgram.read(dgm);
00590         dgram.print();
00591         unsigned int id = dgram.getId();
00592         cm.setVerboseLevel(15);
00593         cm.addToReceptionField(id);
00594         if (!cm.send(dgram)) {
00595                 if (verbose) {
00596                         printf("%s: Can't send datagram: ",__FUNCTION__);
00597                         dgram.print(stdout);
00598                         printf("\n");
00599                 }
00600                 goto cleanup;
00601         }
00602         if (!cm.wait(id,dgram,1.0)) {
00603                 if (verbose > 1)
00604                         printf("SDgm: Failed to receive ack dgm %02X\n",id);
00605                 goto cleanup;
00606         }
00607         cm.removeFromReceptionField(id);
00608         cm.setVerboseLevel(verbose);
00609         return true;
00610 cleanup:
00611         cm.removeFromReceptionField(id);
00612         cm.setVerboseLevel(verbose);
00613         return false;
00614 }
00615 
00616 void CanonDriver::setMaxFrameRate()
00617 {
00618         vm.setMaxFrameRate();
00619 }
00620 
00621 void CanonDriver::setFrameRate(double fps)
00622 {
00623         vm.setFrameRate(fps);
00624 }
00625 
00626 double CanonDriver::getRequiredFrameRate()
00627 {
00628         return vm.getRequiredFrameRate();
00629 }
00630 
00631 double CanonDriver::getObservedFrameRate()
00632 {
00633         return vm.getObservedFrameRate();
00634 }
00635 
00636 void CanonDriver::setVideoOutputColorSpace(JpegReader::ColorSpace cspace)
00637 {
00638         vm.setVideoOutputColorSpace(cspace);
00639 }
00640 
00641 void CanonDriver::startVideoRecording()
00642 {
00643         vm.startRecording();
00644 }
00645 
00646 void CanonDriver::stopVideoRecording()
00647 {
00648         vm.stopRecording();
00649 }
00650 
00651 bool CanonDriver::getVideoRecordingStatus() const
00652 {
00653         return vm.isRecording();
00654 }
00655 
00656 void CanonDriver::setVideoRecordingMode(bool mode)
00657 {
00658         if (mode)
00659                 vm.startRecording();
00660         else 
00661                 vm.stopRecording();
00662 }
00663 
00664 void CanonDriver::setRecordingDestination(const char * dirname)
00665 {
00666         vm.setRecordingDestination(dirname);
00667 }
00668 
00669 
00670 
00671 
00672 
00673 
00674 
00675 
00676 
00677 
00678 
00679 
00680 
00681 
00682 
00683 
00684 


canon_vbc50i
Author(s): Cedric Pradalier
autogenerated on Mon Jan 6 2014 11:18:27