26 #include <core/exceptions/system.h>
27 #include <sys/ioctl.h>
30 #include <sys/types.h>
31 #include <utils/math/angle.h>
52 const char *DirectedPerceptionPTU::DPPTU_PAN_ABSPOS =
"PP";
53 const char *DirectedPerceptionPTU::DPPTU_TILT_ABSPOS =
"TP";
54 const char *DirectedPerceptionPTU::DPPTU_PAN_RELPOS =
"PO";
55 const char *DirectedPerceptionPTU::DPPTU_TILT_RELPOS =
"TO";
56 const char *DirectedPerceptionPTU::DPPTU_PAN_RESOLUTION =
"PR";
57 const char *DirectedPerceptionPTU::DPPTU_TILT_RESOLUTION =
"TR";
58 const char *DirectedPerceptionPTU::DPPTU_PAN_MIN =
"PN";
59 const char *DirectedPerceptionPTU::DPPTU_PAN_MAX =
"PX";
60 const char *DirectedPerceptionPTU::DPPTU_TILT_MIN =
"TN";
61 const char *DirectedPerceptionPTU::DPPTU_TILT_MAX =
"TX";
62 const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_QUERY =
"L";
63 const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_ENABLE =
"LE";
64 const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_DISABLE =
"LD";
65 const char *DirectedPerceptionPTU::DPPTU_IMMEDIATE_EXECUTION =
"I";
66 const char *DirectedPerceptionPTU::DPPTU_SLAVED_EXECUTION =
"S";
67 const char *DirectedPerceptionPTU::DPPTU_AWAIT_COMPLETION =
"A";
68 const char *DirectedPerceptionPTU::DPPTU_HALT_ALL =
"H";
69 const char *DirectedPerceptionPTU::DPPTU_HALT_PAN =
"HP";
70 const char *DirectedPerceptionPTU::DPPTU_HALT_TILT =
"HT";
71 const char *DirectedPerceptionPTU::DPPTU_PAN_SPEED =
"PS";
72 const char *DirectedPerceptionPTU::DPPTU_TILT_SPEED =
"TS";
73 const char *DirectedPerceptionPTU::DPPTU_PAN_ACCEL =
"PA";
74 const char *DirectedPerceptionPTU::DPPTU_TILT_ACCEL =
"TA";
75 const char *DirectedPerceptionPTU::DPPTU_PAN_BASESPEED =
"PB";
76 const char *DirectedPerceptionPTU::DPPTU_TILT_BASESPEED =
"TB";
77 const char *DirectedPerceptionPTU::DPPTU_PAN_UPPER_SPEED_LIMIT =
"PU";
78 const char *DirectedPerceptionPTU::DPPTU_PAN_LOWER_SPEED_LIMIT =
"PL";
79 const char *DirectedPerceptionPTU::DPPTU_TILT_UPPER_SPEED_LIMIT =
"TU";
80 const char *DirectedPerceptionPTU::DPPTU_TILT_LOWER_SPEED_LIMIT =
"TL";
81 const char *DirectedPerceptionPTU::DPPTU_RESET =
"R";
82 const char *DirectedPerceptionPTU::DPPTU_STORE =
"DS";
83 const char *DirectedPerceptionPTU::DPPTU_RESTORE =
"DR";
84 const char *DirectedPerceptionPTU::DPPTU_FACTORY_RESET =
"DF";
85 const char *DirectedPerceptionPTU::DPPTU_ECHO_QUERY =
"E";
86 const char *DirectedPerceptionPTU::DPPTU_ECHO_ENABLE =
"EE";
87 const char *DirectedPerceptionPTU::DPPTU_ECHO_DISABLE =
"ED";
88 const char *DirectedPerceptionPTU::DPPTU_ASCII_VERBOSE =
"FV";
89 const char *DirectedPerceptionPTU::DPPTU_ASCII_TERSE =
"FT";
90 const char *DirectedPerceptionPTU::DPPTU_ASCII_QUERY =
"F";
91 const char *DirectedPerceptionPTU::DPPTU_VERSION =
"V";
99 device_file_ = strdup(device_file);
101 timeout_ms_ = timeout_ms;
114 DirectedPerceptionPTU::open()
119 fd_ = ::open(device_file_, O_RDWR | O_NOCTTY | O_NONBLOCK);
120 if (!fd_ || !isatty(fd_)) {
121 throw Exception(
"Cannot open device or device is not a TTY");
124 struct termios param;
126 if (tcgetattr(fd_, ¶m) != 0) {
128 throw Exception(
"DP PTU: Cannot get parameters");
132 if (cfsetspeed(¶m, B9600) == -1) {
134 throw Exception(
"DP PTU: Cannot set speed");
138 cfsetospeed(¶m, B9600);
139 cfsetispeed(¶m, B9600);
142 param.c_cflag |= (CLOCAL | CREAD);
143 param.c_cflag &= ~CSIZE;
144 param.c_cflag |= CS8;
145 param.c_cflag &= ~PARENB;
146 param.c_cflag &= ~CSTOPB;
149 param.c_iflag &= ~(INPCK | ISTRIP);
150 param.c_iflag &= ~(IXON | IXOFF | IXANY);
152 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
154 param.c_cc[VTIME] = 1;
155 param.c_cc[VMIN] = 0;
157 if (tcsetattr(fd_, TCSANOW, ¶m) != 0) {
159 throw Exception(
"DP PTU: Cannot set parameters");
165 send(DPPTU_ECHO_DISABLE);
166 send(DPPTU_ASCII_TERSE);
170 pan_resolution_ = query_int(DPPTU_PAN_RESOLUTION);
171 tilt_resolution_ = query_int(DPPTU_TILT_RESOLUTION);
173 pan_upper_limit_ = query_int(DPPTU_PAN_MAX);
174 pan_lower_limit_ = query_int(DPPTU_PAN_MIN);
175 tilt_upper_limit_ = query_int(DPPTU_TILT_MAX);
176 tilt_lower_limit_ = query_int(DPPTU_TILT_MIN);
182 DirectedPerceptionPTU::close()
194 send(DPPTU_HALT_ALL);
203 send(DPPTU_PAN_ABSPOS, pan);
212 send(DPPTU_TILT_ABSPOS, tilt);
222 if (pan > pan_upper_limit_)
223 pan = pan_upper_limit_;
224 if (pan < pan_lower_limit_)
225 pan = pan_lower_limit_;
226 if (tilt > tilt_upper_limit_)
227 tilt = tilt_upper_limit_;
228 if (tilt < tilt_lower_limit_)
229 tilt = tilt_lower_limit_;
231 send(DPPTU_PAN_ABSPOS, pan);
232 send(DPPTU_TILT_ABSPOS, tilt);
242 set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
252 pan = query_int(DPPTU_PAN_ABSPOS);
253 tilt = query_int(DPPTU_TILT_ABSPOS);
263 int tpan = 0, ttilt = 0;
265 tpan = query_int(DPPTU_PAN_ABSPOS);
266 ttilt = query_int(DPPTU_TILT_ABSPOS);
268 pan = pan_ticks2rad(tpan);
269 tilt = tilt_ticks2rad(ttilt);
278 return query_int(DPPTU_PAN_ABSPOS);
287 return query_int(DPPTU_TILT_ABSPOS);
296 return pan_upper_limit_;
305 return pan_lower_limit_;
314 return tilt_upper_limit_;
323 return tilt_lower_limit_;
335 pan_min = pan_ticks2rad(pan_lower_limit_);
336 pan_max = pan_ticks2rad(tilt_upper_limit_);
337 tilt_min = tilt_ticks2rad(tilt_lower_limit_);
338 tilt_max = tilt_ticks2rad(tilt_upper_limit_);
349 DirectedPerceptionPTU::send(
const char *command,
int value)
351 snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE,
"%s%i ", command, value);
354 printf(
"Writing with value '%s' to PTU failed\n", obuffer_);
359 DirectedPerceptionPTU::send(
const char *command)
361 snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE,
"%s ", command);
364 printf(
"Writing '%s' to PTU failed\n", obuffer_);
369 DirectedPerceptionPTU::write(
const char *buffer)
371 printf(
"Writing '%s'\n", obuffer_);
373 tcflush(fd_, TCIOFLUSH);
374 unsigned int buffer_size = strlen(buffer);
375 int written = ::write(fd_, buffer, buffer_size);
379 printf(
"Writing '%s' failed: %s\n", buffer, strerror(errno));
380 }
else if ((
unsigned int)written != buffer_size) {
381 printf(
"Writing '%s' failed, only wrote %i of %u bytes\n", buffer, written, buffer_size);
386 DirectedPerceptionPTU::read(
char *buffer,
unsigned int buffer_size)
390 unsigned int diff_msec = 0;
391 gettimeofday(&start, NULL);
394 ioctl(fd_, FIONREAD, &num_bytes);
395 while (((timeout_ms_ == 0) || (diff_msec < timeout_ms_)) && (num_bytes == 0)) {
396 ioctl(fd_, FIONREAD, &num_bytes);
398 gettimeofday(&now, NULL);
399 diff_msec = (now.tv_sec - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
400 usleep(timeout_ms_ * 100);
402 if (num_bytes == 0) {
405 ssize_t bytes_read = ::read(fd_, buffer, buffer_size);
406 if (bytes_read < 0) {
409 return (bytes_read > 0);
414 DirectedPerceptionPTU::result_ok()
416 if (read(ibuffer_, 1)) {
417 if (ibuffer_[0] ==
'*') {
426 DirectedPerceptionPTU::data_available()
429 ioctl(fd_, FIONREAD, &num_bytes);
430 return (num_bytes > 0);
434 DirectedPerceptionPTU::query_int(
const char *query_command)
437 bool ok = read(ibuffer_, DPPTU_MAX_IBUFFER_SIZE);
439 throw Exception(
"DP PTU: failed to query integer");
442 if (sscanf(ibuffer_,
"* %i", &intrv) <= 0) {
443 throw Exception(errno,
"DP PTU: failed to query int");
449 DirectedPerceptionPTU::pan_rad2ticks(
float r)
451 if (pan_resolution_ == 0)
453 return (
int)rint(
rad2deg(r) * 3600 / pan_resolution_);
457 DirectedPerceptionPTU::tilt_rad2ticks(
float r)
459 if (tilt_resolution_ == 0)
461 return (
int)rint(
rad2deg(r) * 3600 / tilt_resolution_);
465 DirectedPerceptionPTU::pan_ticks2rad(
int ticks)
467 if (pan_resolution_ == 0)
469 return deg2rad(ticks * pan_resolution_ / 3600);
473 DirectedPerceptionPTU::tilt_ticks2rad(
int ticks)
475 if (tilt_resolution_ == 0)
477 return deg2rad(ticks * tilt_resolution_ / 3600);