24 #include <fvcams/control/visca.h>
25 #include <sys/ioctl.h>
27 #include <utils/system/console_colors.h>
34 namespace firevision {
52 : Exception(msg, _errno)
90 inquire = VISCA_RUNINQ_NONE;
91 this->blocking = blocking;
93 for (
unsigned int i = 0; i < VISCA_NONBLOCKING_NUM; ++i) {
94 nonblocking_sockets[i] = 0;
95 nonblocking_running[i] =
false;
105 struct termios param;
107 dev =
::open(port, O_CREAT | O_RDWR | O_NONBLOCK);
112 if (tcgetattr(dev, ¶m) == -1) {
118 cfsetospeed(¶m, B9600);
119 cfsetispeed(¶m, B9600);
121 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
122 param.c_cflag |= CREAD;
123 param.c_cflag |= CLOCAL;
126 param.c_cc[VMIN] = 1;
127 param.c_cc[VTIME] = 0;
129 param.c_iflag |= IGNBRK;
130 param.c_iflag &= ~PARMRK;
131 param.c_iflag &= ~ISTRIP;
132 param.c_iflag &= ~INLCR;
133 param.c_iflag &= ~IGNCR;
134 param.c_iflag &= ~ICRNL;
135 param.c_iflag &= ~IXON;
136 param.c_iflag &= ~IXOFF;
138 param.c_lflag &= ~ECHO;
141 param.c_lflag |= IEXTEN;
142 param.c_oflag &= ~OPOST;
148 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
150 param.c_cflag |= CS8;
153 param.c_cflag &= ~(PARENB & PARODD);
156 param.c_cflag &= ~CSTOPB;
158 if (tcsetattr(dev, TCSANOW, ¶m) != 0) {
166 sender = VISCA_BUS_0;
167 recipient = VISCA_BUS_1;
169 #ifdef TIMETRACKER_VISCA
170 tracker =
new TimeTracker();
171 track_file.open(
"tracker_visca.txt");
172 ttcls_pantilt_get_send = tracker->addClass(
"getPanTilt: send");
173 ttcls_pantilt_get_read = tracker->addClass(
"getPanTilt: read");
174 ttcls_pantilt_get_handle = tracker->addClass(
"getPanTilt: handling responses");
175 ttcls_pantilt_get_interpret = tracker->addClass(
"getPanTilt: interpreting");
197 unsigned char recp_backup = recipient;
198 recipient = VISCA_BUS_BROADCAST;
207 e.
append(
"set_address(%u) failed", num_cameras);
211 recipient = recp_backup;
230 e.
append(
"clear() failed");
244 obuffer[0] |= (sender << 4);
245 obuffer[0] |= recipient;
247 obuffer[++obuffer_length] = VISCA_TERMINATOR;
250 int written = write(dev, obuffer, obuffer_length);
256 if (written < obuffer_length) {
268 ioctl(dev, FIONREAD, &num_bytes);
269 return (num_bytes > 0);
279 recv_packet(max_wait_ms);
281 e.
append(
"Receiving failed, recv_packet() call failed");
286 unsigned char type = ibuffer[1] & 0xF0;
287 while (type == VISCA_RESPONSE_ACK) {
289 recv_packet(max_wait_ms);
291 e.
append(
"Receiving failed, recv_packet() call 2 failed");
294 type = ibuffer[1] & 0xF0;
298 case VISCA_RESPONSE_CLEAR:
299 case VISCA_RESPONSE_ADDRESS:
300 case VISCA_RESPONSE_COMPLETED:
301 case VISCA_RESPONSE_ERROR:
break;
319 unsigned char type = ibuffer[1] & 0xF0;
320 while (type != VISCA_RESPONSE_ACK) {
325 e.
append(
"Handling message of type %u failed", type);
328 type = ibuffer[1] & 0xF0;
332 if (socket != NULL) {
333 *socket = ibuffer[1] & 0x0F;
348 e.
append(
"Non-blocking send failed!");
362 e.
append(
"Sending with reply failed");
371 ViscaControl::recv_packet(
unsigned int max_wait_ms)
375 unsigned int diff_msec = 0;
376 gettimeofday(&start, NULL);
379 ioctl(dev, FIONREAD, &num_bytes);
380 while (((max_wait_ms == 0) || (diff_msec < max_wait_ms)) && (num_bytes == 0)) {
381 usleep(max_wait_ms / 100);
382 ioctl(dev, FIONREAD, &num_bytes);
384 gettimeofday(&now, NULL);
385 diff_msec = (now.tv_sec - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
387 if (num_bytes == 0) {
388 throw ViscaControlException(
"recv_packet() failed: no bytes to read");
392 int bytes_read = read(dev, ibuffer, 1);
394 while (ibuffer[pos] != VISCA_TERMINATOR) {
395 bytes_read = read(dev, &ibuffer[++pos], 1);
398 ibuffer_length = pos + 1;
410 ViscaControl::finish_nonblocking(
unsigned int socket)
412 for (
unsigned int i = 0; i < VISCA_NONBLOCKING_NUM; ++i) {
413 if (nonblocking_sockets[i] == socket) {
414 nonblocking_sockets[i] = 0;
415 nonblocking_running[i] =
false;
420 throw ViscaControlException(
"finish_nonblocking() failed: socket not found");
425 ViscaControl::handle_response()
427 unsigned int type = ibuffer[1] & 0xF0;
428 unsigned int socket = ibuffer[1] & 0x0F;
432 throw ViscaControlException(
"handle_response(): Received an inquire response, can't handle");
435 if (type == VISCA_RESPONSE_COMPLETED) {
438 finish_nonblocking(ibuffer[1] & 0x0F);
439 }
catch (ViscaControlException &e) {
444 }
else if (type == VISCA_RESPONSE_ERROR) {
445 finish_nonblocking(ibuffer[1] & 0x0F);
446 throw ViscaControlException(
"handle_response(): got an error message from camera");
448 ViscaControlException ve(
"Got unknown/unhandled response type");
449 ve.append(
"Received message of type %u", type);
460 unsigned char cancel_socket = socket & 0x0000000F;
462 obuffer[1] = VISCA_CANCEL | cancel_socket;
468 e.
append(
"cancel_command() failed");
472 if (((ibuffer[1] & 0xF0) == VISCA_RESPONSE_ERROR) && ((ibuffer[1] & 0x0F) == cancel_socket)
473 && ((ibuffer[2] == VISCA_ERROR_CANCELLED))) {
484 inquire = VISCA_RUNINQ_NONE;
517 unsigned short int tilt_val = 0 + tilt;
518 unsigned short int pan_val = 0 + pan;
520 obuffer[1] = VISCA_COMMAND;
521 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
522 obuffer[3] = VISCA_PT_ABSOLUTE_POSITION;
529 obuffer[6] = (pan_val & 0xf000) >> 12;
530 obuffer[7] = (pan_val & 0x0f00) >> 8;
531 obuffer[8] = (pan_val & 0x00f0) >> 4;
532 obuffer[9] = (pan_val & 0x000f);
534 obuffer[10] = (tilt_val & 0xf000) >> 12;
535 obuffer[11] = (tilt_val & 0x0f00) >> 8;
536 obuffer[12] = (tilt_val & 0x00f0) >> 4;
537 obuffer[13] = (tilt_val & 0x000f);
543 nonblocking_running[VISCA_NONBLOCKING_PANTILT] =
true;
549 e.
append(
"setPanTilt() failed");
561 inquire = VISCA_RUNINQ_PANTILT;
563 obuffer[1] = VISCA_INQUIRY;
564 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
565 obuffer[3] = VISCA_PT_POSITION_INQ;
571 e.
append(
"startGetPanTilt() failed");
587 if (inquire != VISCA_RUNINQ_PANTILT) {
590 #ifdef TIMETRACKER_VISCA
591 tracker->pingStart(ttcls_pantilt_get_read);
598 #ifdef TIMETRACKER_VISCA
599 tracker->pingEnd(ttcls_pantilt_get_read);
603 obuffer[1] = VISCA_INQUIRY;
604 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
605 obuffer[3] = VISCA_PT_POSITION_INQ;
609 #ifdef TIMETRACKER_VISCA
610 tracker->pingStart(ttcls_pantilt_get_send);
612 tracker->pingEnd(ttcls_pantilt_get_send);
613 tracker->pingStart(ttcls_pantilt_get_read);
615 tracker->pingEnd(ttcls_pantilt_get_read);
624 #ifdef TIMETRACKER_VISCA
625 tracker->pingStart(ttcls_pantilt_get_handle);
628 while (ibuffer[1] != VISCA_RESPONSE_COMPLETED) {
639 #ifdef TIMETRACKER_VISCA
640 tracker->pingEnd(ttcls_pantilt_get_handle);
641 tracker->pingStart(ttcls_pantilt_get_interpret);
645 if (ibuffer[1] == VISCA_RESPONSE_COMPLETED) {
646 unsigned short int pan_val = 0;
647 unsigned short int tilt_val = 0;
649 pan_val |= (ibuffer[2] & 0x0F) << 12;
650 pan_val |= (ibuffer[3] & 0x0F) << 8;
651 pan_val |= (ibuffer[4] & 0x0F) << 4;
652 pan_val |= (ibuffer[5] & 0x0F);
654 tilt_val |= (ibuffer[6] & 0x0F) << 12;
655 tilt_val |= (ibuffer[7] & 0x0F) << 8;
656 tilt_val |= (ibuffer[8] & 0x0F) << 4;
657 tilt_val |= (ibuffer[9] & 0x0F);
659 if (pan_val < 0x8000) {
664 *pan = pan_val - 0xFFFF;
667 if (tilt_val < 0x8000) {
672 *tilt = tilt_val - 0xFFFF;
678 #ifdef TIMETRACKER_VISCA
679 tracker->pingEnd(ttcls_pantilt_get_interpret);
680 tracker->printToStream(track_file);
683 inquire = VISCA_RUNINQ_NONE;
690 obuffer[1] = VISCA_COMMAND;
691 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
692 obuffer[3] = VISCA_PT_LIMITSET;
693 obuffer[3] = VISCA_PT_LIMITSET_CLEAR;
694 obuffer[4] = VISCA_PT_LIMITSET_SET_UR;
708 obuffer[4] = VISCA_PT_LIMITSET_SET_DL;
712 e.
append(
"resetPanTiltLimit() failed");
727 obuffer[1] = VISCA_COMMAND;
728 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
729 obuffer[3] = VISCA_PT_LIMITSET;
730 obuffer[3] = VISCA_PT_LIMITSET_SET;
731 obuffer[4] = VISCA_PT_LIMITSET_SET_UR;
733 obuffer[5] = (pan_right & 0xf000) >> 12;
734 obuffer[6] = (pan_right & 0x0f00) >> 8;
735 obuffer[7] = (pan_right & 0x00f0) >> 4;
736 obuffer[8] = (pan_right & 0x000f);
738 obuffer[9] = (tilt_up & 0xf000) >> 12;
739 obuffer[10] = (tilt_up & 0x0f00) >> 8;
740 obuffer[11] = (tilt_up & 0x00f0) >> 4;
741 obuffer[12] = (tilt_up & 0x000f);
747 obuffer[4] = VISCA_PT_LIMITSET_SET_DL;
749 obuffer[5] = (pan_left & 0xf000) >> 12;
750 obuffer[6] = (pan_left & 0x0f00) >> 8;
751 obuffer[7] = (pan_left & 0x00f0) >> 4;
752 obuffer[8] = (pan_left & 0x000f);
754 obuffer[9] = (tilt_down & 0xf000) >> 12;
755 obuffer[10] = (tilt_down & 0x0f00) >> 8;
756 obuffer[11] = (tilt_down & 0x00f0) >> 4;
757 obuffer[12] = (tilt_down & 0x000f);
761 e.
append(
"setPanTiltLimit() failed");
770 obuffer[1] = VISCA_COMMAND;
771 obuffer[2] = VISCA_CATEGORY_PAN_TILTER;
772 obuffer[3] = VISCA_PT_HOME;
778 e.
append(
"resetPanTilt() failed");
787 obuffer[1] = VISCA_COMMAND;
788 obuffer[2] = VISCA_CATEGORY_CAMERA1;
789 obuffer[3] = VISCA_ZOOM;
790 obuffer[4] = VISCA_ZOOM_STOP;
796 e.
append(
"resetZoom() failed");
807 obuffer[1] = VISCA_COMMAND;
808 obuffer[2] = VISCA_CATEGORY_CAMERA1;
809 obuffer[3] = VISCA_ZOOM;
810 obuffer[4] = VISCA_ZOOM_TELE_SPEED;
812 obuffer[5] = (speed & 0x000f) | 0x0020;
818 e.
append(
"setZoomSpeedTele() failed");
829 obuffer[1] = VISCA_COMMAND;
830 obuffer[2] = VISCA_CATEGORY_CAMERA1;
831 obuffer[3] = VISCA_ZOOM;
832 obuffer[4] = VISCA_ZOOM_WIDE_SPEED;
834 obuffer[5] = (speed & 0x000f) | 0x0020;
840 e.
append(
"setZoomSpeedWide() failed");
851 obuffer[1] = VISCA_COMMAND;
852 obuffer[2] = VISCA_CATEGORY_CAMERA1;
853 obuffer[3] = VISCA_ZOOM_VALUE;
855 obuffer[4] = (zoom & 0xf000) >> 12;
856 obuffer[5] = (zoom & 0x0f00) >> 8;
857 obuffer[6] = (zoom & 0x00f0) >> 4;
858 obuffer[7] = (zoom & 0x000f);
865 e.
append(
"setZoom() failed");
876 obuffer[1] = VISCA_INQUIRY;
877 obuffer[2] = VISCA_CATEGORY_CAMERA1;
878 obuffer[3] = VISCA_ZOOM_VALUE;
884 e.
append(
"getZoom() failed");
889 if (ibuffer[1] == VISCA_RESPONSE_COMPLETED) {
890 unsigned short int zoom_val = 0;
892 zoom_val |= (ibuffer[2] & 0x0F) << 12;
893 zoom_val |= (ibuffer[3] & 0x0F) << 8;
894 zoom_val |= (ibuffer[4] & 0x0F) << 4;
895 zoom_val |= (ibuffer[5] & 0x0F);
900 "getZoom(): zoom inquiry failed, response code not VISCA_RESPONSE_COMPLETED");
910 obuffer[1] = VISCA_COMMAND;
911 obuffer[2] = VISCA_CATEGORY_CAMERA1;
912 obuffer[3] = VISCA_DZOOM;
914 obuffer[4] = VISCA_DZOOM_ON;
916 obuffer[4] = VISCA_DZOOM_OFF;
923 e.
append(
"setZoomDigitalEnabled() failed");
934 obuffer[1] = VISCA_COMMAND;
935 obuffer[2] = VISCA_CATEGORY_CAMERA1;
936 obuffer[3] = VISCA_PICTURE_EFFECT;
943 e.
append(
"applyEffect() failed");
955 e.
append(
"resetEffect() failed");
967 e.
append(
"applyEffectPastel() failed");
979 e.
append(
"applyEffectNegArt() failed");
991 e.
append(
"applyEffectSepia() failed");
1003 e.
append(
"applyEffectBnW() failed");
1015 e.
append(
"applyEffectSolarize() failed");
1027 e.
append(
"applyEffectMosaic() failed");
1039 e.
append(
"applyEffectSlim() failed");
1051 e.
append(
"applyEffectStretch() failed");
1062 obuffer[1] = VISCA_INQUIRY;
1063 obuffer[2] = VISCA_CATEGORY_CAMERA1;
1064 obuffer[3] = VISCA_WB;
1070 e.
append(
"getWhiteBalanceMode() failed");
1074 while (ibuffer[1] != VISCA_RESPONSE_COMPLETED) {
1081 e.
append(
"getWhiteBalanceMode() failed");
1087 if (ibuffer[1] == VISCA_RESPONSE_COMPLETED) {
void append(const char *format,...)
Append messages to the message list.
ViscaControlException(const char *msg)
Constructor.
Visca inquire running exception.
ViscaControlInquiryRunningException()
Constructor.
ViscaControl(bool blocking=true)
Constructor.
void cancel_command(unsigned int socket)
Cancel a running command.
void setZoomSpeedTele(unsigned int speed)
Set zoom speed in tele.
void setPanTiltLimit(int pan_left, int pan_right, int tilt_up, int tilt_down)
Set pan tilt limit.
void process()
Process incoming data.
void send_nonblocking(unsigned int *socket=NULL)
Send non-blocking.
void setPanTilt(int pan, int tilt)
Set pan tilt.
void getPanTilt(int *pan, int *tilt)
Get pan and tilt values.
void applyEffectNegArt()
Apply negative art effect.
static const unsigned int VISCA_WHITEBALANCE_MANUAL
Manual white balance.
static const unsigned int VISCA_WHITEBLANCE_AUTO
Automatic white balance.
unsigned int getWhiteBalanceMode()
Get white balance mode.
void applyEffectSolarize()
Apply solarize effect.
bool data_available()
Check data availability.
void resetPanTilt()
Reset pan/tilt.
void applyEffectSepia()
Apply sepia effect.
void applyEffect(unsigned char effect)
Apply effect.
void set_address(unsigned int num_cameras)
Set addresses of cameras.
void recv(unsigned int max_wait_ms=10)
Receive data.
void resetZoom()
Reset zoom.
void applyEffectPastel()
Apply pastel effect.
void setZoomSpeedWide(unsigned int speed)
Set zoom speed in wide angle.
void recv_ack(unsigned int *socket=NULL)
Receive ACK packet.
static const unsigned int VISCA_WHITEBALANCE_INDOOR
Indoor white balance preset.
void applyEffectSlim()
Apply slim effect.
void resetPanTiltLimit()
Reset pan/tilt limit.
void open(const char *port)
Open serial port.
void applyEffectStretch()
Apply stretch effect.
void applyEffectBnW()
Apply B/W effect.
void startGetPanTilt()
Query for pan/tilt but do not wait until finished This will send an inquire to the camera that asks f...
void setZoom(unsigned int zoom)
Set zoom.
void getZoom(unsigned int *zoom)
Get zoom.
void resetEffect()
Reset effects.
void applyEffectMosaic()
Apply mosaic effect.
static const unsigned int VISCA_WHITEBALANCE_ONE_PUSH
One push white balance preset.
void send_with_reply()
Send and wait for reply, blocking.
static const unsigned int VISCA_WHITEBALANCE_ATW
ATW white balance preset.
void setZoomDigitalEnabled(bool enabled)
Enable or disable digital zoome.
static const unsigned int VISCA_WHITEBALANCE_OUTDOOR
Outdoor white balance preset.
void send()
Send outbound queue.