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) {
static const unsigned int VISCA_WHITEBALANCE_OUTDOOR
Outdoor white balance preset.
bool data_available()
Check data availability.
static const unsigned int VISCA_WHITEBALANCE_ATW
ATW white balance preset.
void resetPanTiltLimit()
Reset pan/tilt limit.
void applyEffectStretch()
Apply stretch effect.
void setPanTilt(int pan, int tilt)
Set pan tilt.
static const unsigned int VISCA_WHITEBALANCE_ONE_PUSH
One push white balance preset.
void applyEffectMosaic()
Apply mosaic effect.
void send_with_reply()
Send and wait for reply, blocking.
ViscaControlException(const char *msg)
Constructor.
void set_address(unsigned int num_cameras)
Set addresses of cameras.
Visca inquire running exception.
ViscaControl(bool blocking=true)
Constructor.
void applyEffectPastel()
Apply pastel effect.
void applyEffectSolarize()
Apply solarize effect.
void getZoom(unsigned int *zoom)
Get zoom.
unsigned int getWhiteBalanceMode()
Get white balance mode.
void applyEffectSlim()
Apply slim effect.
void send()
Send outbound queue.
void resetZoom()
Reset zoom.
void setPanTiltLimit(int pan_left, int pan_right, int tilt_up, int tilt_down)
Set pan tilt limit.
void startGetPanTilt()
Query for pan/tilt but do not wait until finished This will send an inquire to the camera that asks f...
void setZoomSpeedTele(unsigned int speed)
Set zoom speed in tele.
void resetEffect()
Reset effects.
void setZoomSpeedWide(unsigned int speed)
Set zoom speed in wide angle.
static const unsigned int VISCA_WHITEBALANCE_INDOOR
Indoor white balance preset.
void recv_ack(unsigned int *socket=NULL)
Receive ACK packet.
void open(const char *port)
Open serial port.
void resetPanTilt()
Reset pan/tilt.
void applyEffectBnW()
Apply B/W effect.
ViscaControlInquiryRunningException()
Constructor.
void setZoomDigitalEnabled(bool enabled)
Enable or disable digital zoome.
void process()
Process incoming data.
void setZoom(unsigned int zoom)
Set zoom.
void recv(unsigned int max_wait_ms=10)
Receive data.
void applyEffectSepia()
Apply sepia effect.
void getPanTilt(int *pan, int *tilt)
Get pan and tilt values.
void cancel_command(unsigned int socket)
Cancel a running command.
void send_nonblocking(unsigned int *socket=NULL)
Send non-blocking.
static const unsigned int VISCA_WHITEBLANCE_AUTO
Automatic white balance.
static const unsigned int VISCA_WHITEBALANCE_MANUAL
Manual white balance.
void applyEffect(unsigned char effect)
Apply effect.
void append(const char *format,...)
Append messages to the message list.
void applyEffectNegArt()
Apply negative art effect.