24 #include <fvcams/control/visca.h>
26 #include <sys/ioctl.h>
33 #include <utils/system/console_colors.h>
35 namespace firevision {
47 ViscaControlException::ViscaControlException(
const char *msg)
58 : Exception(msg, _errno)
97 inquire = VISCA_RUNINQ_NONE;
98 this->blocking = blocking;
100 for (
unsigned int i = 0; i < VISCA_NONBLOCKING_NUM; ++i) {
101 nonblocking_sockets[i] = 0;
102 nonblocking_running[i] =
false;
113 struct termios param;
115 dev =
::open(port, O_CREAT | O_RDWR | O_NONBLOCK);
120 if (tcgetattr(dev, ¶m) == -1) {
126 cfsetospeed(¶m, B9600);
127 cfsetispeed(¶m, B9600);
129 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
130 param.c_cflag |= CREAD;
131 param.c_cflag |= CLOCAL;
134 param.c_cc[VMIN] = 1;
135 param.c_cc[VTIME] = 0;
137 param.c_iflag |= IGNBRK;
138 param.c_iflag &= ~PARMRK;
139 param.c_iflag &= ~ISTRIP;
140 param.c_iflag &= ~INLCR;
141 param.c_iflag &= ~IGNCR;
142 param.c_iflag &= ~ICRNL;
143 param.c_iflag &= ~IXON;
144 param.c_iflag &= ~IXOFF;
146 param.c_lflag &= ~ECHO;
149 param.c_lflag |= IEXTEN;
150 param.c_oflag &= ~OPOST;
156 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
158 param.c_cflag |= CS8;
161 param.c_cflag &=~(PARENB & PARODD);
164 param.c_cflag &= ~CSTOPB;
166 if (tcsetattr(dev, TCSANOW, ¶m) != 0) {
174 sender = VISCA_BUS_0;
175 recipient = VISCA_BUS_1;
177 #ifdef TIMETRACKER_VISCA
178 tracker =
new TimeTracker();
179 track_file.open(
"tracker_visca.txt");
180 ttcls_pantilt_get_send = tracker->addClass(
"getPanTilt: send");
181 ttcls_pantilt_get_read = tracker->addClass(
"getPanTilt: read");
182 ttcls_pantilt_get_handle = tracker->addClass(
"getPanTilt: handling responses");
183 ttcls_pantilt_get_interpret = tracker->addClass(
"getPanTilt: interpreting");
207 unsigned char recp_backup = recipient;
208 recipient = VISCA_BUS_BROADCAST;
217 e.
append(
"set_address(%u) failed", num_cameras);
221 recipient = recp_backup;
240 e.
append(
"clear() failed");
254 obuffer[0] |= (sender << 4);
255 obuffer[0] |= recipient;
257 obuffer[++obuffer_length] = VISCA_TERMINATOR;
260 int written = write(dev, obuffer, obuffer_length);
266 if (written < obuffer_length) {
279 ioctl(dev, FIONREAD, &num_bytes);
280 return (num_bytes > 0);
291 recv_packet(max_wait_ms);
293 e.
append(
"Receiving failed, recv_packet() call failed");
298 unsigned char type = ibuffer[1] & 0xF0;
299 while (type == VISCA_RESPONSE_ACK) {
301 recv_packet(max_wait_ms);
303 e.
append(
"Receiving failed, recv_packet() call 2 failed");
306 type = ibuffer[1] & 0xF0;
310 case VISCA_RESPONSE_CLEAR:
311 case VISCA_RESPONSE_ADDRESS:
312 case VISCA_RESPONSE_COMPLETED:
313 case VISCA_RESPONSE_ERROR:
334 unsigned char type = ibuffer[1] & 0xF0;
335 while (type != VISCA_RESPONSE_ACK) {
341 e.
append(
"Handling message of type %u failed", type);
344 type = ibuffer[1] & 0xF0;
348 if (socket != NULL) {
349 *socket = ibuffer[1] & 0x0F;
366 e.
append(
"Non-blocking send failed!");
381 e.
append(
"Sending with reply failed");
391 ViscaControl::recv_packet(
unsigned int max_wait_ms)
395 unsigned int diff_msec = 0;
396 gettimeofday(&start, NULL);
399 ioctl(dev, FIONREAD, &num_bytes);
400 while ( ((max_wait_ms == 0) || (diff_msec < max_wait_ms)) && (num_bytes == 0)) {
401 usleep(max_wait_ms / 100);
402 ioctl(dev, FIONREAD, &num_bytes);
404 gettimeofday(&now, NULL);
405 diff_msec = (now.tv_sec - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
407 if (num_bytes == 0) {
408 throw ViscaControlException(
"recv_packet() failed: no bytes to read");
412 int bytes_read = read(dev, ibuffer, 1);
414 while (ibuffer[pos] != VISCA_TERMINATOR) {
415 bytes_read = read(dev, &ibuffer[++pos], 1);
418 ibuffer_length = pos + 1;
431 ViscaControl::finish_nonblocking(
unsigned int socket )
433 for (
unsigned int i = 0; i < VISCA_NONBLOCKING_NUM; ++i) {
434 if (nonblocking_sockets[i] == socket) {
435 nonblocking_sockets[i] = 0;
436 nonblocking_running[i] =
false;
441 throw ViscaControlException(
"finish_nonblocking() failed: socket not found");
447 ViscaControl::handle_response()
449 unsigned int type = ibuffer[1] & 0xF0;
450 unsigned int socket = ibuffer[1] & 0x0F;
454 throw ViscaControlException(
"handle_response(): Received an inquire response, can't handle");
457 if ( type == VISCA_RESPONSE_COMPLETED ) {
460 finish_nonblocking( ibuffer[1] & 0x0F );
461 }
catch (ViscaControlException &e) {
466 }
else if ( type == VISCA_RESPONSE_ERROR ) {
467 finish_nonblocking( ibuffer[1] & 0x0F );
468 throw ViscaControlException(
"handle_response(): got an error message from camera");
470 ViscaControlException ve(
"Got unknown/unhandled response type");
471 ve.append(
"Received message of type %u", type);
484 unsigned char cancel_socket = socket & 0x0000000F;
486 obuffer[1] = VISCA_CANCEL | cancel_socket;
492 e.
append(
"cancel_command() failed");
496 if ( ((ibuffer[1] & 0xF0) == VISCA_RESPONSE_ERROR) &&
497 ((ibuffer[1] & 0x0F) == cancel_socket) &&
498 ((ibuffer[2] == VISCA_ERROR_CANCELLED)) ) {
511 inquire = VISCA_RUNINQ_NONE;
546 unsigned short int tilt_val = 0 + tilt;
547 unsigned short int pan_val = 0 + pan;
549 obuffer[1] = VISCA_COMMAND;
550 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
551 obuffer[3] = VISCA_PT_ABSOLUTE_POSITION;
558 obuffer[6] = (pan_val & 0xf000) >> 12;
559 obuffer[7] = (pan_val & 0x0f00) >> 8;
560 obuffer[8] = (pan_val & 0x00f0) >> 4;
561 obuffer[9] = (pan_val & 0x000f);
563 obuffer[10] = (tilt_val & 0xf000) >> 12;
564 obuffer[11] = (tilt_val & 0x0f00) >> 8;
565 obuffer[12] = (tilt_val & 0x00f0) >> 4;
566 obuffer[13] = (tilt_val & 0x000f);
572 nonblocking_running[ VISCA_NONBLOCKING_PANTILT ] =
true;
578 e.
append(
"setPanTilt() failed");
591 inquire = VISCA_RUNINQ_PANTILT;
593 obuffer[1] = VISCA_INQUIRY;
594 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
595 obuffer[3] = VISCA_PT_POSITION_INQ;
601 e.
append(
"startGetPanTilt() failed");
619 if ( inquire != VISCA_RUNINQ_PANTILT ) {
622 #ifdef TIMETRACKER_VISCA
623 tracker->pingStart( ttcls_pantilt_get_read );
630 #ifdef TIMETRACKER_VISCA
631 tracker->pingEnd( ttcls_pantilt_get_read );
636 obuffer[1] = VISCA_INQUIRY;
637 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
638 obuffer[3] = VISCA_PT_POSITION_INQ;
642 #ifdef TIMETRACKER_VISCA
643 tracker->pingStart( ttcls_pantilt_get_send );
645 tracker->pingEnd( ttcls_pantilt_get_send );
646 tracker->pingStart( ttcls_pantilt_get_read );
648 tracker->pingEnd( ttcls_pantilt_get_read );
657 #ifdef TIMETRACKER_VISCA
658 tracker->pingStart( ttcls_pantilt_get_handle );
661 while (ibuffer[1] != VISCA_RESPONSE_COMPLETED) {
672 #ifdef TIMETRACKER_VISCA
673 tracker->pingEnd( ttcls_pantilt_get_handle );
674 tracker->pingStart( ttcls_pantilt_get_interpret );
679 if ( ibuffer[1] == VISCA_RESPONSE_COMPLETED ) {
680 unsigned short int pan_val = 0;
681 unsigned short int tilt_val = 0;
683 pan_val |= (ibuffer[2] & 0x0F) << 12;
684 pan_val |= (ibuffer[3] & 0x0F) << 8;
685 pan_val |= (ibuffer[4] & 0x0F) << 4;
686 pan_val |= (ibuffer[5] & 0x0F);
688 tilt_val |= (ibuffer[6] & 0x0F) << 12;
689 tilt_val |= (ibuffer[7] & 0x0F) << 8;
690 tilt_val |= (ibuffer[8] & 0x0F) << 4;
691 tilt_val |= (ibuffer[9] & 0x0F);
693 if (pan_val < 0x8000) {
698 *pan = pan_val - 0xFFFF;
701 if (tilt_val < 0x8000) {
706 *tilt = tilt_val - 0xFFFF;
712 #ifdef TIMETRACKER_VISCA
713 tracker->pingEnd( ttcls_pantilt_get_interpret );
714 tracker->printToStream( track_file );
717 inquire = VISCA_RUNINQ_NONE;
725 obuffer[1] = VISCA_COMMAND;
726 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
727 obuffer[3] = VISCA_PT_LIMITSET;
728 obuffer[3] = VISCA_PT_LIMITSET_CLEAR;
729 obuffer[4] = VISCA_PT_LIMITSET_SET_UR;
743 obuffer[4] = VISCA_PT_LIMITSET_SET_DL;
747 e.
append(
"resetPanTiltLimit() failed");
763 obuffer[1] = VISCA_COMMAND;
764 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
765 obuffer[3] = VISCA_PT_LIMITSET;
766 obuffer[3] = VISCA_PT_LIMITSET_SET;
767 obuffer[4] = VISCA_PT_LIMITSET_SET_UR;
769 obuffer[5] = (pan_right & 0xf000) >> 12;
770 obuffer[6] = (pan_right & 0x0f00) >> 8;
771 obuffer[7] = (pan_right & 0x00f0) >> 4;
772 obuffer[8] = (pan_right & 0x000f);
774 obuffer[9] = (tilt_up & 0xf000) >> 12;
775 obuffer[10] = (tilt_up & 0x0f00) >> 8;
776 obuffer[11] = (tilt_up & 0x00f0) >> 4;
777 obuffer[12] = (tilt_up & 0x000f);
783 obuffer[4] = VISCA_PT_LIMITSET_SET_DL;
785 obuffer[5] = (pan_left & 0xf000) >> 12;
786 obuffer[6] = (pan_left & 0x0f00) >> 8;
787 obuffer[7] = (pan_left & 0x00f0) >> 4;
788 obuffer[8] = (pan_left & 0x000f);
790 obuffer[9] = (tilt_down & 0xf000) >> 12;
791 obuffer[10] = (tilt_down & 0x0f00) >> 8;
792 obuffer[11] = (tilt_down & 0x00f0) >> 4;
793 obuffer[12] = (tilt_down & 0x000f);
797 e.
append(
"setPanTiltLimit() failed");
807 obuffer[1] = VISCA_COMMAND;
808 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
809 obuffer[3] = VISCA_PT_HOME;
815 e.
append(
"resetPanTilt() failed");
825 obuffer[1] = VISCA_COMMAND;
826 obuffer[2] = VISCA_CATEGORY_CAMERA1;
827 obuffer[3] = VISCA_ZOOM;
828 obuffer[4] = VISCA_ZOOM_STOP;
834 e.
append(
"resetZoom() failed");
846 obuffer[1] = VISCA_COMMAND;
847 obuffer[2] = VISCA_CATEGORY_CAMERA1;
848 obuffer[3] = VISCA_ZOOM;
849 obuffer[4] = VISCA_ZOOM_TELE_SPEED;
851 obuffer[5] = (speed & 0x000f) | 0x0020;
857 e.
append(
"setZoomSpeedTele() failed");
869 obuffer[1] = VISCA_COMMAND;
870 obuffer[2] = VISCA_CATEGORY_CAMERA1;
871 obuffer[3] = VISCA_ZOOM;
872 obuffer[4] = VISCA_ZOOM_WIDE_SPEED;
874 obuffer[5] = (speed & 0x000f) | 0x0020;
880 e.
append(
"setZoomSpeedWide() failed");
892 obuffer[1] = VISCA_COMMAND;
893 obuffer[2] = VISCA_CATEGORY_CAMERA1;
894 obuffer[3] = VISCA_ZOOM_VALUE;
896 obuffer[4] = (zoom & 0xf000) >> 12;
897 obuffer[5] = (zoom & 0x0f00) >> 8;
898 obuffer[6] = (zoom & 0x00f0) >> 4;
899 obuffer[7] = (zoom & 0x000f);
906 e.
append(
"setZoom() failed");
918 obuffer[1] = VISCA_INQUIRY;
919 obuffer[2] = VISCA_CATEGORY_CAMERA1;
920 obuffer[3] = VISCA_ZOOM_VALUE;
926 e.
append(
"getZoom() failed");
931 if ( ibuffer[1] == VISCA_RESPONSE_COMPLETED ) {
932 unsigned short int zoom_val = 0;
934 zoom_val |= (ibuffer[2] & 0x0F) << 12;
935 zoom_val |= (ibuffer[3] & 0x0F) << 8;
936 zoom_val |= (ibuffer[4] & 0x0F) << 4;
937 zoom_val |= (ibuffer[5] & 0x0F);
941 throw ViscaControlException(
"getZoom(): zoom inquiry failed, response code not VISCA_RESPONSE_COMPLETED");
953 obuffer[1] = VISCA_COMMAND;
954 obuffer[2] = VISCA_CATEGORY_CAMERA1;
955 obuffer[3] = VISCA_DZOOM;
957 obuffer[4] = VISCA_DZOOM_ON;
959 obuffer[4] = VISCA_DZOOM_OFF;
966 e.
append(
"setZoomDigitalEnabled() failed");
978 obuffer[1] = VISCA_COMMAND;
979 obuffer[2] = VISCA_CATEGORY_CAMERA1;
980 obuffer[3] = VISCA_PICTURE_EFFECT;
987 e.
append(
"applyEffect() failed");
1000 e.
append(
"resetEffect() failed");
1013 e.
append(
"applyEffectPastel() failed");
1026 e.
append(
"applyEffectNegArt() failed");
1039 e.
append(
"applyEffectSepia() failed");
1052 e.
append(
"applyEffectBnW() failed");
1065 e.
append(
"applyEffectSolarize() failed");
1078 e.
append(
"applyEffectMosaic() failed");
1091 e.
append(
"applyEffectSlim() failed");
1104 e.
append(
"applyEffectStretch() failed");
1116 obuffer[1] = VISCA_INQUIRY;
1117 obuffer[2] = VISCA_CATEGORY_CAMERA1;
1118 obuffer[3] = VISCA_WB;
1124 e.
append(
"getWhiteBalanceMode() failed");
1128 while (ibuffer[1] != VISCA_RESPONSE_COMPLETED) {
1135 e.
append(
"getWhiteBalanceMode() failed");
1141 if ( ibuffer[1] == VISCA_RESPONSE_COMPLETED ) {