00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <p2os_driver/p2os_ptz.hpp>
00023 #include <p2os_driver/p2os.hpp>
00024
00025
00026
00027
00028 const int P2OSPtz::MAX_COMMAND_LENGTH = 19;
00029 const int P2OSPtz::MAX_REQUEST_LENGTH = 17;
00030 const int P2OSPtz::COMMAND_RESPONSE_BYTES = 6;
00031 const int P2OSPtz::PACKET_TIMEOUT = 300;
00032 const int P2OSPtz::SLEEP_TIME_USEC = 300000;
00033 const int P2OSPtz::PAN_THRESH = 1;
00034 const int P2OSPtz::TILT_THRESH = 1;
00035 const int P2OSPtz::ZOOM_THRESH = 1;
00036
00037
00038
00039
00040 P2OSPtz::P2OSPtz(P2OSNode * p2os, bool bidirectional_com)
00041 : p2os_(p2os), max_zoom_(MAX_ZOOM_OPTIC), pan_(0), tilt_(0), zoom_(0),
00042 is_on_(false), error_code_(CAM_ERROR_NONE),
00043 bidirectional_com_(bidirectional_com)
00044 {
00045 current_state_.pan = 0;
00046 current_state_.zoom = 0;
00047 current_state_.tilt = 0;
00048 current_state_.relative = false;
00049 }
00050
00051
00052
00053
00054
00055 int P2OSPtz::setup()
00056 {
00057 int err = 0;
00058 int num_inits = 7;
00059 is_on_ = true;
00060 for (int i = 1; i < num_inits; i++) {
00061 switch (i) {
00062
00063
00064
00065
00066
00067
00068
00069 case 1:
00070 do {
00071 ROS_DEBUG("Waiting for camera to power on.");
00072 err = setPower(POWER_ON);
00073 } while (error_code_ == CAM_ERROR_BUSY);
00074 break;
00075 case 2:
00076 do {
00077 ROS_DEBUG("Waiting for camera mode to set");
00078 err = setControlMode();
00079 } while (error_code_ == CAM_ERROR_BUSY);
00080 break;
00081 case 3:
00082 do {
00083 ROS_DEBUG("Waiting for camera to initialize");
00084 err = sendInit();
00085 } while (error_code_ == CAM_ERROR_BUSY);
00086 break;
00087 case 4:
00088 do {
00089 for (int i = 0; i < 3; i++) {
00090 ROS_DEBUG("Waiting for camera to set default tilt");
00091 err = setDefaultTiltRange();
00092 }
00093 } while (error_code_ == CAM_ERROR_BUSY);
00094 break;
00095 case 5:
00096 do {
00097 ROS_DEBUG("Waiting for camera to set initial pan and tilt");
00098 err = sendAbsPanTilt(0, 0);
00099 } while (error_code_ == CAM_ERROR_BUSY);
00100 break;
00101 case 6:
00102 do {
00103 ROS_DEBUG("Waiting for camera to set initial zoom");
00104 err = sendAbsZoom(0);
00105 } while (error_code_ == CAM_ERROR_BUSY);
00106 break;
00107 default:
00108 err = -7;
00109 break;
00110 }
00111
00112
00113 if (err) {
00114 ROS_ERROR("Error initiliazing PTZ at stage %i", i);
00115 switch (error_code_) {
00116 case CAM_ERROR_BUSY:
00117 ROS_ERROR("Error: CAM_ERROR_BUSY");
00118 break;
00119 case CAM_ERROR_PARAM:
00120 ROS_ERROR("Error: CAM_ERROR_PARAM");
00121 break;
00122 case CAM_ERROR_MODE:
00123 ROS_ERROR("Error: CAM_ERROR_MODE");
00124 break;
00125 default:
00126 ROS_ERROR("Error: Unknown error response from camera.");
00127 break;
00128 }
00129 return -1;
00130 } else {
00131 ROS_DEBUG("Passed stage %i of PTZ initialization.", i);
00132 }
00133 }
00134 ROS_DEBUG("Finished initialization of the PTZ.");
00135 return 0;
00136 }
00137
00138 void P2OSPtz::shutdown()
00139 {
00140 sendAbsPanTilt(0, 0);
00141 usleep(SLEEP_TIME_USEC);
00142 sendAbsZoom(0);
00143 usleep(SLEEP_TIME_USEC);
00144 setPower(POWER_OFF);
00145 usleep(SLEEP_TIME_USEC);
00146 ROS_INFO("PTZ camera has been shutdown");
00147 }
00148
00149 void P2OSPtz::callback(const p2os_msgs::PTZStateConstPtr & cmd)
00150 {
00151 p2os_msgs::PTZState to_send;
00152 bool change_pan_tilt = false;
00153 bool change_zoom = false;
00154 to_send.pan = pan_;
00155 to_send.tilt = tilt_;
00156 to_send.zoom = zoom_;
00157
00158
00159 if (cmd->relative) {
00160 if (abs(cmd->pan) > PAN_THRESH) {
00161 to_send.pan = cmd->pan + pan_;
00162 change_pan_tilt = true;
00163 }
00164 if (abs(cmd->tilt) > TILT_THRESH) {
00165 to_send.tilt = cmd->tilt + tilt_;
00166 change_pan_tilt = true;
00167 }
00168 if (abs(cmd->zoom) > ZOOM_THRESH) {
00169 to_send.zoom = cmd->zoom + zoom_;
00170 change_zoom = true;
00171 }
00172 } else {
00173 if (abs(cmd->pan - pan_) > PAN_THRESH) {
00174 to_send.pan = cmd->pan;
00175 change_pan_tilt = true;
00176 }
00177 if (abs(cmd->tilt - tilt_) > TILT_THRESH) {
00178 to_send.tilt = cmd->tilt;
00179 change_pan_tilt = true;
00180 }
00181 if (abs(cmd->zoom - zoom_) > ZOOM_THRESH) {
00182 to_send.zoom = cmd->zoom;
00183 change_zoom = true;
00184 }
00185 }
00186
00187 if (change_pan_tilt) {
00188 sendAbsPanTilt(to_send.pan, to_send.tilt);
00189 }
00190 if (change_zoom) {
00191 sendAbsZoom(to_send.zoom);
00192 }
00193
00194 current_state_.pan = pan_;
00195 current_state_.zoom = zoom_;
00196 current_state_.tilt = tilt_;
00197 }
00198
00199
00200
00201
00202 int P2OSPtz::sendCommand(unsigned char * str, int len)
00203 {
00204 P2OSPacket ptz_packet;
00205 P2OSPacket request_pkt;
00206 unsigned char request[4];
00207
00208
00209 request[0] = GETAUX;
00210 request[1] = ARGINT;
00211 request[2] = 0;
00212 request[3] = 0;
00213 request_pkt.Build(request, 4);
00214 p2os_->SendReceive(&request_pkt, false);
00215
00216 if (len > MAX_COMMAND_LENGTH) {
00217 ROS_ERROR("Command message is too large to send");
00218 return -1;
00219 }
00220
00221
00222
00223 unsigned char mybuf[MAX_COMMAND_LENGTH + 3];
00224 mybuf[0] = TTY2;
00225 mybuf[1] = ARGSTR;
00226 mybuf[2] = len;
00227
00228 memcpy(&mybuf[3], str, len);
00229 ptz_packet.Build(mybuf, len + 3);
00230
00231
00232 p2os_->SendReceive(&ptz_packet, false);
00233
00234 return 0;
00235 }
00236
00237 int P2OSPtz::sendRequest(unsigned char * str, int len, unsigned char * reply)
00238 {
00239 static_cast<void>(reply);
00240 P2OSPacket ptz_packet;
00241 P2OSPacket request_pkt;
00242 unsigned char request[4];
00243
00244
00245 request[0] = GETAUX;
00246 request[1] = ARGINT;
00247 request[2] = 0;
00248 request[3] = 0;
00249 request_pkt.Build(request, 4);
00250 p2os_->SendReceive(&request_pkt, false);
00251
00252 if (len > MAX_REQUEST_LENGTH) {
00253 ROS_ERROR("Request message is too large to send.");
00254 return -1;
00255 }
00256
00257
00258
00259 unsigned char mybuf[MAX_REQUEST_LENGTH];
00260 mybuf[0] = TTY2;
00261 mybuf[1] = ARGSTR;
00262 mybuf[2] = len;
00263
00264 memcpy(&mybuf[3], str, len);
00265 ptz_packet.Build(mybuf, len + 3);
00266
00267
00268 p2os_->SendReceive(&ptz_packet, false);
00269
00270
00271 return 0;
00272 }
00273
00274 int P2OSPtz::receiveCommandAnswer(int asize)
00275 {
00276 int num;
00277 unsigned char reply[MAX_REQUEST_LENGTH];
00278 int len = 0;
00279 unsigned char byte;
00280 int t;
00281 memset(reply, 0, COMMAND_RESPONSE_BYTES);
00282
00283 getPtzPacket(asize);
00284
00285 for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++) {
00286
00287
00288 t = cb_.getFromBuf();
00289 if (t < 0) {
00290
00291 ROS_ERROR("circbuf error!");
00292 return -1;
00293 } else {
00294 byte = (unsigned char)t;
00295 }
00296 if (byte == (unsigned char)RESPONSE) {
00297 reply[0] = byte;
00298 len++;
00299 break;
00300 }
00301 }
00302
00303 if (len == 0) {
00304 ROS_ERROR("Length is 0 on received packet.");
00305 return -1;
00306 }
00307
00308
00309 for (num = 1; num <= MAX_REQUEST_LENGTH; num++) {
00310 t = cb_.getFromBuf();
00311 if (t < 0) {
00312
00313 if (reply[len - 1] != (unsigned char)FOOTER) {
00314 ROS_ERROR("canonvcc4::receiveCommandAnswer: Discarding bad packet.");
00315 return -1;
00316 } else {
00317 break;
00318 }
00319 } else {
00320
00321 reply[len] = (unsigned char)t;
00322 len++;
00323 }
00324 }
00325
00326
00327 if (len != COMMAND_RESPONSE_BYTES) {
00328 ROS_ERROR("Answer does not equal command response bytes");
00329 return -1;
00330 }
00331
00332
00333 if (reply[0] != (unsigned char)RESPONSE || reply[5] != (unsigned char)FOOTER) {
00334 ROS_ERROR("Header or Footer is wrong on received packet");
00335 return -1;
00336 }
00337
00338
00339 error_code_ = reply[3];
00340 if (error_code_ == CAM_ERROR_NONE) {
00341 return 0;
00342 }
00343
00344 switch (error_code_) {
00345 case CAM_ERROR_BUSY:
00346 ROS_ERROR("Error: CAM_ERROR_BUSY");
00347 break;
00348 case CAM_ERROR_PARAM:
00349 ROS_ERROR("Error: CAM_ERROR_PARAM");
00350 break;
00351 case CAM_ERROR_MODE:
00352 ROS_ERROR("Error: CAM_ERROR_MODE");
00353 break;
00354 default:
00355 ROS_ERROR("Error: Unknown error response from camera.");
00356 break;
00357 }
00358 return -1;
00359 }
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372 int P2OSPtz::receiveRequestAnswer(unsigned char * data, int s1, int s2)
00373 {
00374 int num;
00375 unsigned char reply[MAX_REQUEST_LENGTH];
00376 int len = 0;
00377 unsigned char byte;
00378 int t;
00379
00380 memset(reply, 0, MAX_REQUEST_LENGTH);
00381 getPtzPacket(s1, s2);
00382
00383 for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++) {
00384
00385
00386 t = cb_.getFromBuf();
00387 if (t < 0) {
00388 ROS_ERROR("circbuf error!\n");
00389 return -1;
00390 } else {
00391 byte = (unsigned char)t;
00392 }
00393 if (byte == (unsigned char)RESPONSE) {
00394 reply[0] = byte;
00395 len++;
00396 break;
00397 }
00398 }
00399 if (len == 0) {
00400 ROS_ERROR("Received Request Answer has length 0");
00401 return -1;
00402 }
00403
00404 for (num = 1; num <= MAX_REQUEST_LENGTH; num++) {
00405 t = cb_.getFromBuf();
00406 if (t < 0) {
00407
00408 if (reply[len - 1] != (unsigned char)FOOTER) {
00409 ROS_ERROR("Last Byte was not the footer!");
00410 return -1;
00411 } else {
00412 break;
00413 }
00414 } else {
00415
00416 reply[len] = (unsigned char)t;
00417 len++;
00418 }
00419 }
00420
00421 if (len != COMMAND_RESPONSE_BYTES && len != 8 && len != 10 && len != 14) {
00422 ROS_ERROR("Response Length was incorrect at %i.", len);
00423 return -1;
00424 }
00425
00426 if (reply[0] != (unsigned char)RESPONSE ||
00427 reply[len - 1] != (unsigned char)FOOTER)
00428 {
00429 ROS_ERROR("Header or Footer is wrong on received packet");
00430 return -1;
00431 }
00432
00433
00434 error_code_ = reply[3];
00435
00436 if (error_code_ == CAM_ERROR_NONE) {
00437 memcpy(data, reply, len);
00438 return len;
00439 }
00440 switch (error_code_) {
00441 case CAM_ERROR_BUSY:
00442 ROS_ERROR("Error: CAM_ERROR_BUSY");
00443 break;
00444 case CAM_ERROR_PARAM:
00445 ROS_ERROR("Error: CAM_ERROR_PARAM");
00446 break;
00447 case CAM_ERROR_MODE:
00448 ROS_ERROR("Error: CAM_ERROR_MODE");
00449 break;
00450 default:
00451 ROS_ERROR("Error: Unknown error response from camera.");
00452 break;
00453 }
00454 return -1;
00455 }
00456
00457 void P2OSPtz::getPtzPacket(int s1, int s2)
00458 {
00459 int packetCount = 0;
00460 unsigned char request[4];
00461 P2OSPacket request_pkt;
00462 bool secondSent = false;
00463
00464 request[0] = GETAUX;
00465 request[1] = ARGINT;
00466 request[2] = s1;
00467 request[3] = 0;
00468
00469
00470 cb_.reset();
00471
00472
00473 request_pkt.Build(request, 4);
00474 p2os_->SendReceive(&request_pkt, false);
00475
00476 while (!cb_.gotPacket() ) {
00477 if (packetCount++ > PACKET_TIMEOUT) {
00478
00479 ROS_ERROR("Waiting for packet timed out.");
00480 return;
00481 }
00482 if (cb_.size() == s1 && !secondSent) {
00483 if (s2 > s1) {
00484
00485 int newsize = s2 - s1;
00486 ROS_ERROR("Requesting Second Packet of size %i.", newsize);
00487 request[2] = newsize;
00488 request_pkt.Build(request, 4);
00489 secondSent = true;
00490 p2os_->SendReceive(&request_pkt, false);
00491 } else {
00492
00493 ROS_ERROR("Got reply from AUX1 But don't have a full packet.");
00494 break;
00495 }
00496 }
00497
00498
00499 p2os_->SendReceive(NULL, false);
00500 }
00501 }
00502
00503
00504
00505
00506 int P2OSPtz::setPower(Power on)
00507 {
00508 unsigned char command[MAX_COMMAND_LENGTH];
00509
00510 command[0] = HEADER;
00511 command[1] = DEVICEID;
00512 command[2] = DEVICEID;
00513 command[3] = DELIM;
00514 command[4] = POWER;
00515 if (on) {
00516 command[5] = DEVICEID + 1;
00517 } else {
00518 command[5] = DEVICEID;
00519 }
00520 command[6] = FOOTER;
00521
00522 if (sendCommand(command, 7)) {
00523 return -1;
00524 }
00525 if (bidirectional_com_) {
00526 return receiveCommandAnswer(COMMAND_RESPONSE_BYTES);
00527 } else {
00528 usleep(SLEEP_TIME_USEC);
00529 return 0;
00530 }
00531 }
00532
00533 int P2OSPtz::setControlMode()
00534 {
00535 unsigned char command[MAX_COMMAND_LENGTH];
00536
00537 command[0] = HEADER;
00538 command[1] = DEVICEID;
00539 command[2] = DEVICEID;
00540 command[3] = DELIM;
00541 command[4] = CONTROL;
00542 command[5] = DEVICEID;
00543 command[6] = FOOTER;
00544
00545 if (sendCommand(command, 7)) {
00546 return -1;
00547 }
00548 if (bidirectional_com_) {
00549 return receiveCommandAnswer(COMMAND_RESPONSE_BYTES);
00550 } else {
00551 usleep(SLEEP_TIME_USEC);
00552 return 0;
00553 }
00554 }
00555
00556 int P2OSPtz::sendInit()
00557 {
00558 unsigned char command[MAX_COMMAND_LENGTH];
00559
00560 command[0] = HEADER;
00561 command[1] = DEVICEID;
00562 command[2] = DEVICEID;
00563 command[3] = DELIM;
00564 command[4] = INIT;
00565 command[5] = DEVICEID;
00566 command[6] = FOOTER;
00567
00568 if (sendCommand(command, 7)) {
00569 return -1;
00570 }
00571 if (bidirectional_com_) {
00572 return receiveCommandAnswer(COMMAND_RESPONSE_BYTES);
00573 } else {
00574 usleep(SLEEP_TIME_USEC);
00575 return 0;
00576 }
00577 }
00578
00579 int P2OSPtz::getMaxZoom(int * maxzoom)
00580 {
00581 unsigned char command[MAX_COMMAND_LENGTH];
00582 unsigned char reply[MAX_REQUEST_LENGTH];
00583 int reply_len;
00584 char byte;
00585 unsigned char buf[4];
00586 unsigned int u_zoom;
00587 int i;
00588
00589 command[0] = HEADER;
00590 command[1] = DEVICEID;
00591 command[2] = DEVICEID;
00592 command[3] = DELIM;
00593 command[4] = ZOOMREQ;
00594 command[5] = DEVICEID;
00595 command[6] = FOOTER;
00596
00597 if (sendCommand(command, 7)) {
00598 return -1;
00599 }
00600
00601 if (bidirectional_com_) {
00602 reply_len = receiveRequestAnswer(reply, 10, 0);
00603 } else {
00604 return 0;
00605 }
00606
00607 if (reply_len == COMMAND_RESPONSE_BYTES) {
00608 return -1;
00609 }
00610
00611
00612 for (i = 0; i < 4; i++) {
00613 byte = reply[i + 5];
00614 if (byte < 0x40) {
00615 byte = byte - 0x30;
00616 } else {
00617 byte = byte - 'A' + 10;
00618 }
00619 buf[i] = byte;
00620 }
00621
00622
00623 u_zoom = 0;
00624 for (i = 0; i < 4; i++) {
00625 u_zoom += buf[i] * static_cast<unsigned int>(pow(16.0, static_cast<double>(3 - i)));
00626 }
00627 *maxzoom = u_zoom;
00628
00629 return 0;
00630 }
00631 int P2OSPtz::getAbsZoom(int * zoom)
00632 {
00633 unsigned char command[MAX_COMMAND_LENGTH];
00634 unsigned char reply[MAX_REQUEST_LENGTH];
00635 int reply_len;
00636 char byte;
00637 unsigned char buf[4];
00638 unsigned int u_zoom;
00639 int i;
00640
00641 command[0] = HEADER;
00642 command[1] = DEVICEID;
00643 command[2] = DEVICEID;
00644 command[3] = DELIM;
00645 command[4] = ZOOMREQ;
00646 command[5] = DEVICEID;
00647 command[6] = FOOTER;
00648
00649 if (sendRequest(command, 6, reply)) {
00650 return -1;
00651 }
00652 if (bidirectional_com_) {
00653 reply_len = receiveRequestAnswer(reply, 10, 0);
00654 } else {
00655 return 0;
00656 }
00657
00658 if (reply_len == COMMAND_RESPONSE_BYTES) {
00659 return -1;
00660 }
00661
00662
00663 for (i = 0; i < 4; i++) {
00664 byte = reply[i + 5];
00665 if (byte < 0x40) {
00666 byte = byte - 0x30;
00667 } else {
00668 byte = byte - 'A' + 10;
00669 }
00670 buf[i] = byte;
00671 }
00672
00673
00674 u_zoom = 0;
00675 for (i = 0; i < 4; i++) {
00676 u_zoom += buf[i] * static_cast<unsigned int>(pow(16.0, static_cast<double>(3 - i)));
00677 }
00678 *zoom = u_zoom;
00679 return 0;
00680 }
00681
00682 int P2OSPtz::sendAbsZoom(int zoom)
00683 {
00684 unsigned char command[MAX_COMMAND_LENGTH];
00685 unsigned char buf[5];
00686 int i;
00687
00688 if (zoom < 0) {
00689 zoom = 0;
00690 } else if (zoom > max_zoom_) {
00691 zoom = max_zoom_;
00692 }
00693
00694 command[0] = HEADER;
00695 command[1] = DEVICEID;
00696 command[2] = DEVICEID;
00697 command[3] = DELIM;
00698 command[4] = ZOOM;
00699
00700 snprintf(reinterpret_cast<char *>(buf), sizeof(buf), "%4X", zoom);
00701
00702 for (i = 0; i < 3; i++) {
00703 if (buf[i] == ' ') {
00704 buf[i] = '0';
00705 }
00706 }
00707
00708
00709 command[5] = buf[0];
00710 command[6] = buf[1];
00711 command[7] = buf[2];
00712 command[8] = buf[3];
00713 command[9] = FOOTER;
00714
00715 zoom_ = zoom;
00716
00717 if (sendCommand(command, 10)) {
00718 return -1;
00719 }
00720 if (bidirectional_com_) {
00721 return receiveCommandAnswer(COMMAND_RESPONSE_BYTES);
00722 } else {
00723 usleep(SLEEP_TIME_USEC);
00724 return 0;
00725 }
00726
00727 }
00728
00729 int P2OSPtz::setDefaultTiltRange()
00730 {
00731 unsigned char command[MAX_COMMAND_LENGTH];
00732 unsigned char buf[5];
00733 int maxtilt, mintilt;
00734
00735 command[0] = HEADER;
00736 command[1] = DEVICEID;
00737 command[2] = DEVICEID;
00738 command[3] = DELIM;
00739 command[4] = SETRANGE;
00740 command[5] = DEVICEID + 1;
00741
00742 mintilt = static_cast<int>(floor(MIN_TILT / .1125) + 0x8000);
00743 snprintf(reinterpret_cast<char *>(buf), sizeof(buf), "%X", mintilt);
00744 command[6] = buf[0];
00745 command[7] = buf[1];
00746 command[8] = buf[2];
00747 command[9] = buf[3];
00748 maxtilt = static_cast<int>(floor(MAX_TILT / .1125) + 0x8000);
00749 snprintf(reinterpret_cast<char *>(buf), sizeof(buf), "%X", maxtilt);
00750
00751 command[10] = buf[0];
00752 command[11] = buf[1];
00753 command[12] = buf[2];
00754 command[13] = buf[3];
00755 command[14] = FOOTER;
00756
00757 if (sendCommand(command, 15)) {
00758 return -1;
00759 }
00760 if (bidirectional_com_) {
00761 return receiveCommandAnswer(COMMAND_RESPONSE_BYTES);
00762 } else {
00763 usleep(SLEEP_TIME_USEC);
00764 return 0;
00765 }
00766
00767
00768 }
00769
00770 int P2OSPtz::getAbsPanTilt(int * pan, int * tilt)
00771 {
00772 unsigned char command[MAX_COMMAND_LENGTH];
00773 unsigned char reply[MAX_REQUEST_LENGTH];
00774 int reply_len;
00775 unsigned char buf[4];
00776 char byte;
00777 unsigned int u_val;
00778 int val;
00779 int i;
00780
00781 command[0] = HEADER;
00782 command[1] = DEVICEID;
00783 command[2] = DEVICEID;
00784 command[3] = DELIM;
00785 command[4] = PANTILTREQ;
00786 command[5] = FOOTER;
00787
00788 if (sendRequest(command, 6, reply)) {
00789 return -1;
00790 }
00791 if (bidirectional_com_) {
00792 reply_len = receiveRequestAnswer(reply, 14, 0);
00793 } else {
00794 return 0;
00795 }
00796
00797 if (reply_len != 14) {
00798 ROS_ERROR("Reply Len = %i; should equal 14", reply_len);
00799 return -1;
00800 }
00801
00802
00803 for (i = 0; i < 4; i++) {
00804 byte = reply[i + 5];
00805 if (byte < 0x40) {
00806 byte = byte - 0x30;
00807 } else {
00808 byte = byte - 'A' + 10;
00809 }
00810 buf[i] = byte;
00811 }
00812
00813
00814 u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
00815
00816
00817 val = static_cast<int>((static_cast<int>(u_val) - 0x8000) * 0.1125);
00818
00819
00820 *pan = val;
00821
00822
00823 for (i = 0; i < 4; i++) {
00824 byte = reply[i + 9];
00825 if (byte < 0x40) {
00826 byte = byte - 0x30;
00827 } else {
00828 byte = byte - 'A' + 10;
00829 }
00830 buf[i] = byte;
00831 }
00832 u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
00833 val = static_cast<int>((static_cast<int>(u_val) - 0x8000) * 0.1125);
00834 *tilt = val;
00835
00836 return 0;
00837 }
00838
00839 int P2OSPtz::sendAbsPanTilt(int pan, int tilt)
00840 {
00841 unsigned char command[MAX_COMMAND_LENGTH];
00842 int convpan, convtilt;
00843 unsigned char buf[5];
00844 int ppan, ttilt;
00845
00846 ppan = pan; ttilt = tilt;
00847 if (pan < MIN_PAN) {
00848 ppan = static_cast<int>(MIN_PAN);
00849 } else if (pan > MAX_PAN) {
00850 ppan = static_cast<int>(MAX_PAN);
00851 }
00852
00853 if (tilt > MAX_TILT) {
00854 ttilt = static_cast<int>(MAX_TILT);
00855 } else if (tilt < MIN_TILT) {
00856 ttilt = static_cast<int>(MIN_TILT);
00857 }
00858
00859
00860
00861
00862 convpan = static_cast<int>(floor(ppan / .1125)) + 0x8000;
00863 convtilt = static_cast<int>(floor(ttilt / .1125)) + 0x8000;
00864
00865
00866 command[0] = HEADER;
00867 command[1] = DEVICEID;
00868 command[2] = DEVICEID;
00869 command[3] = DELIM;
00870 command[4] = PANTILT;
00871
00872
00873 snprintf(reinterpret_cast<char *>(buf), sizeof(buf), "%X", convpan);
00874
00875 command[5] = buf[0];
00876 command[6] = buf[1];
00877 command[7] = buf[2];
00878 command[8] = buf[3];
00879
00880 snprintf(reinterpret_cast<char *>(buf), sizeof(buf), "%X", convtilt);
00881 command[9] = buf[0];
00882 command[10] = buf[1];
00883 command[11] = buf[2];
00884 command[12] = buf[3];
00885 command[13] = (unsigned char) FOOTER;
00886 if (sendCommand(command, 14)) {
00887 return -1;
00888 }
00889
00890 tilt_ = ttilt;
00891 pan_ = ppan;
00892
00893 if (bidirectional_com_) {
00894 return receiveCommandAnswer(COMMAND_RESPONSE_BYTES);
00895 } else {
00896 usleep(SLEEP_TIME_USEC);
00897 return 0;
00898 }
00899
00900
00901 }
00902
00903
00904
00905
00906 circbuf::circbuf(int size)
00907 : start(0), end(0), mysize(size), gotPack(false)
00908 {
00909 this->buf = new unsigned char[size];
00910 }
00911
00912 void circbuf::printBuf()
00913 {
00914 int i = start;
00915 printf("circbuf: ");
00916 while (i != end) {
00917 printf("0x%x ", buf[i]);
00918 i = (i + 1) % mysize;
00919 }
00920 printf("\n");
00921 }
00922
00923
00924 void circbuf::putOnBuf(unsigned char c)
00925 {
00926 buf[end] = c;
00927 end = (end + 1) % mysize;
00928 if (end == start) {
00929
00930 start = (start + 1) % mysize;
00931 }
00932
00933
00934 if (c == P2OSPtz::FOOTER) {
00935 gotPack = true;
00936 }
00937 }
00938
00939 bool circbuf::haveData()
00940 {
00941 return !(this->start == this->end);
00942 }
00943
00944 int circbuf::getFromBuf()
00945 {
00946 if (start != end) {
00947 unsigned char ret = buf[start];
00948 start = (start + 1) % mysize;
00949 return static_cast<int>(ret);
00950 } else {
00951 return -1;
00952 }
00953 }
00954
00955 int circbuf::size()
00956 {
00957 if (end > start) {
00958 return end - start;
00959 } else if (start > end) {
00960 return mysize - start - end - 1;
00961 } else {
00962 return 0;
00963 }
00964 }
00965
00966 bool circbuf::gotPacket()
00967 {
00968 return gotPack;
00969 }
00970
00971 void circbuf::reset()
00972 {
00973 memset(buf, 0, mysize);
00974 gotPack = false;
00975 start = end = 0;
00976 }