28 #include <sys/types.h>
30 #include <sys/ioctl.h>
41 #include "ViscaControl.hh"
42 #include <Base/Common/W32Compat.hh>
71 InitPanTiltUnit(std::string port)
74 #ifdef BIAS_HAVE_PTHREADS
75 pthread_mutex_init(&camMutex,NULL);
81 cout<<
"trying to open "<<port<<
" for serial communication...";
82 ret = serialIO_.OpenPort(port);
83 if(!ret){ BIASERR(
"Could not open serial port!")
return false; }
92 bool ViscaControl::ClosePanTiltUnit()
95 bool ret = serialIO_.ClosePort();
97 #ifdef BIAS_HAVE_PTHREADS
98 pthread_mutex_destroy(&camMutex);
105 SendCommand_(
unsigned char* buf,
int length,
109 if(!connected_)
return -1;
110 #ifdef BIAS_HAVE_PTHREADS
111 pthread_mutex_lock(&camMutex);
113 serialIO_.WriteBytes(buf,length);
114 if(manageAnswer)ManageAnswer();
115 #ifdef BIAS_HAVE_PTHREADS
116 pthread_mutex_unlock(&camMutex);
122 void ViscaControl::ContinuousRead(){
129 void ViscaControl::ManageAnswer()
133 unsigned char* receiveBuffer=
new unsigned char[length];
135 for (
int i=0;i<length;i++){ receiveBuffer[i]=0; }
137 cout<<
"Reading answer as reply..";
138 int ret = serialIO_.ReadBytes(receiveBuffer,length,
true);
139 if(ret < 0){BIASERR(
"Could not read answer")}
140 else cout<<
"done, "<<ret<<
" Bytes!"<<endl;
143 for (
int i=0;i<ret;i++){
145 sprintf(temp,
"0x%02X, ", receiveBuffer[i]);
150 if (receiveBuffer[0]==0x90 && receiveBuffer[1]==0x41 && receiveBuffer[2]==0xFF) {
151 cout <<
"ACK!" << endl;
153 if (receiveBuffer[1]==0x61)
155 if(receiveBuffer[2]==0x01){ BIASERR(
"Message length error.")}
156 else if(receiveBuffer[2]==0x02){ BIASERR(
"Syntax error.")}
157 else if(receiveBuffer[2]==0x03){ BIASERR(
"Command buffer full.")}
158 else if(receiveBuffer[2]==0x04){ BIASERR(
"Command cancelled.")}
159 else if(receiveBuffer[2]==0x05){ BIASERR(
"No socket (to be cancelled).")}
160 else if(receiveBuffer[2]==0x41){ BIASERR(
"Command not executable.")}
162 if(receiveBuffer[3]==0x90 && receiveBuffer[4]==0x51 && receiveBuffer[5]==0xFF) {
163 cout <<
"COMMAND COMPLETED!" << endl;
166 cout<<
"Uninterpreted reply:"<<endl;
172 if(receiveBuffer[0]==0x90 && receiveBuffer[1]==0x50){
175 pan_|=(short)receiveBuffer[2]<<12;
176 pan_|=(short)receiveBuffer[3]<<8;
177 pan_|=(short)receiveBuffer[4]<<4;
178 pan_|=(short)receiveBuffer[5];
179 cout <<
"pan_ " << pan_ << endl;
182 tilt_|=(short)receiveBuffer[6]<<12;
183 tilt_|=(short)receiveBuffer[7]<<8;
184 tilt_|=(short)receiveBuffer[8]<<4;
185 tilt_|=(short)receiveBuffer[9];
186 cout <<
"tilt_ " << tilt_ << endl;
192 zoom_|=(int)receiveBuffer[2]<<12;
193 zoom_|=(int)receiveBuffer[3]<<8;
194 zoom_|=(int)receiveBuffer[4]<<4;
195 zoom_|=(int)receiveBuffer[5];
196 cout <<
"zoom int " << zoom_ << endl;
202 delete[] receiveBuffer;
206 void ViscaControl::MoveRelative(
short relPan,
short relTilt)
208 sendBuffer[0] = activeUnit_;
209 sendBuffer[1] = 0x01;
210 sendBuffer[2] = 0x06;
211 sendBuffer[3] = 0x03;
212 sendBuffer[4] = panSpeed_;
213 sendBuffer[5] = tiltSpeed_;
214 sendBuffer[6] = (
unsigned char)((relPan >> 12) & 0x0F);
215 sendBuffer[7] = (
unsigned char)((relPan >> 8) & 0x0F);
216 sendBuffer[8] = (
unsigned char)((relPan >> 4) & 0x0F);
217 sendBuffer[9] = (
unsigned char)(relPan & 0x0F);
218 sendBuffer[10]= (
unsigned char)((relTilt >> 12) & 0x0F);
219 sendBuffer[11]= (
unsigned char)((relTilt >> 8) & 0x0F);
220 sendBuffer[12]= (
unsigned char)((relTilt >> 4) & 0x0F);
221 sendBuffer[13]= (
unsigned char)(relTilt & 0x0F);
222 sendBuffer[14]= 0xFF;
224 this->SendCommand_(sendBuffer,15);
227 void ViscaControl::MoveAbsolute(
short panPos,
short tiltPos)
229 sendBuffer[0] = activeUnit_;
230 sendBuffer[1] = 0x01;
231 sendBuffer[2] = 0x06;
232 sendBuffer[3] = 0x02;
233 sendBuffer[4] = panSpeed_;
234 sendBuffer[5] = tiltSpeed_;
235 sendBuffer[6] = (
unsigned char)((panPos >> 12)& 0x0F);
236 sendBuffer[7] = (
unsigned char)((panPos>> 8)& 0x0F);
237 sendBuffer[8] = (
unsigned char)((panPos>> 4) & 0x0F);
238 sendBuffer[9] = (
unsigned char)(panPos & 0x000F);
239 sendBuffer[10]= (
unsigned char)((tiltPos >> 12)& 0x0F);
240 sendBuffer[11]= (
unsigned char)((tiltPos >> 8)& 0x0F);
241 sendBuffer[12]= (
unsigned char)((tiltPos >> 4)& 0x0F);
242 sendBuffer[13]= (
unsigned char)(tiltPos & 0x0F);
243 sendBuffer[14]= 0xFF;
245 this->SendCommand_(sendBuffer,15);
248 void ViscaControl::MovePhysical(
short panSteps,
short tiltSteps)
250 sendBuffer[0] = activeUnit_;
251 sendBuffer[1] = 0x01;
252 sendBuffer[2] = 0x06;
253 sendBuffer[3] = 0x08;
254 sendBuffer[4] = panSpeed_;
255 sendBuffer[5] = tiltSpeed_;
256 sendBuffer[6] = (
unsigned char)((panSteps >> 12)& 0x0F);
257 sendBuffer[7] = (
unsigned char)((panSteps >> 8)& 0x0F);
258 sendBuffer[8] = (
unsigned char)((panSteps >> 4)& 0x0F);
259 sendBuffer[9] = (
unsigned char)(panSteps & 0x000F);
260 sendBuffer[10]= (
unsigned char)((tiltSteps >> 12)& 0x0F);
261 sendBuffer[11]= (
unsigned char)((tiltSteps >> 8)& 0x0F);
262 sendBuffer[12]= (
unsigned char)((tiltSteps >> 4)& 0x0F);
263 sendBuffer[13]= (
unsigned char)(tiltSteps & 0x0F);
264 sendBuffer[14]= 0xFF;
266 this->SendCommand_(sendBuffer,15);
270 void ViscaControl::LightOn() {
272 sendBuffer[0] = activeUnit_;
273 sendBuffer[1] = 0x01;
274 sendBuffer[2] = 0x04;
275 sendBuffer[3] = 0x02;
276 sendBuffer[4] = 0x03;
277 sendBuffer[5] = 0x02;
278 sendBuffer[6] = 0x01;
279 sendBuffer[7] = 0xFF;
281 this->SendCommand_(sendBuffer, 8);
285 void ViscaControl::LightOff() {
287 sendBuffer[0] = activeUnit_;
288 sendBuffer[1] = 0x01;
289 sendBuffer[2] = 0x04;
290 sendBuffer[3] = 0x02;
291 sendBuffer[4] = 0x03;
292 sendBuffer[5] = 0x03;
293 sendBuffer[6] = 0x01;
294 sendBuffer[7] = 0xFF;
296 this->SendCommand_(sendBuffer, 8);
300 void ViscaControl::Wash() {
302 sendBuffer[0] = activeUnit_;
303 sendBuffer[1] = 0x01;
304 sendBuffer[2] = 0x04;
305 sendBuffer[3] = 0x02;
306 sendBuffer[4] = 0x02;
307 sendBuffer[5] = 0x01;
308 sendBuffer[6] = 0x01;
309 sendBuffer[7] = 0xFF;
311 this->SendCommand_(sendBuffer, 8);
315 void ViscaControl::Wipe() {
317 sendBuffer[0] = activeUnit_;
318 sendBuffer[1] = 0x01;
319 sendBuffer[2] = 0x05;
320 sendBuffer[3] = 0x06;
321 sendBuffer[4] = 0x01;
322 sendBuffer[5] = 0x01;
323 sendBuffer[6] = 0x01;
324 sendBuffer[7] = 0xFF;
326 this->SendCommand_(sendBuffer, 8);
331 void ViscaControl::CamPower(
bool mode)
333 sendBuffer[0]= activeUnit_;
338 if(mode) { sendBuffer[4]=0x02; }
339 else { sendBuffer[4]=0x03; }
343 this->SendCommand_(sendBuffer,6);
346 int ViscaControl::CamInitialize_()
348 sendBuffer[0]= activeUnit_;
355 this->SendCommand_(sendBuffer,6);
359 return SendCommand_(sendBuffer,6);
362 void ViscaControl::PanTiltPosInq()
364 sendBuffer[0]= activeUnit_;
369 this->SendCommand_(sendBuffer,5);
372 void ViscaControl::MoveHome()
374 sendBuffer[0]= activeUnit_;
379 this->SendCommand_(sendBuffer,5);
383 void ViscaControl::AdressSet()
385 sendBuffer[0]= activeUnit_;
390 this->SendCommand_(sendBuffer,4);
393 void ViscaControl::MoveLeft()
395 sendBuffer[0]= activeUnit_;
399 sendBuffer[4]= panSpeed_;
400 sendBuffer[5]= tiltSpeed_;
405 this->SendCommand_(sendBuffer,9);
408 void ViscaControl::MoveRight()
410 sendBuffer[0]= activeUnit_;
414 sendBuffer[4]= panSpeed_;
415 sendBuffer[5]= tiltSpeed_;
420 this->SendCommand_(sendBuffer,9);
422 void ViscaControl::MoveUp()
424 sendBuffer[0]= activeUnit_;
428 sendBuffer[4]= panSpeed_;
429 sendBuffer[5]= tiltSpeed_;
433 this->SendCommand_(sendBuffer,9);
436 void ViscaControl::MoveUpLeft()
438 sendBuffer[0]= activeUnit_;
442 sendBuffer[4]= panSpeed_;
443 sendBuffer[5]= tiltSpeed_;
448 this->SendCommand_(sendBuffer,9);
451 void ViscaControl::MoveUpRight()
453 sendBuffer[0]= activeUnit_;
457 sendBuffer[4]= panSpeed_;
458 sendBuffer[5]= tiltSpeed_;
463 this->SendCommand_(sendBuffer,9);
466 void ViscaControl::MoveDown()
468 sendBuffer[0]= activeUnit_;
472 sendBuffer[4]= panSpeed_;
473 sendBuffer[5]= tiltSpeed_;
478 this->SendCommand_(sendBuffer,9);
481 void ViscaControl::MoveDownLeft()
483 sendBuffer[0]= activeUnit_;
487 sendBuffer[4]= panSpeed_;
488 sendBuffer[5]= tiltSpeed_;
493 this->SendCommand_(sendBuffer,9);
496 void ViscaControl::MoveDownRight()
498 sendBuffer[0]= activeUnit_;
502 sendBuffer[4]= panSpeed_;
503 sendBuffer[5]= tiltSpeed_;
508 this->SendCommand_(sendBuffer,9);
511 void ViscaControl::MoveStop()
513 sendBuffer[0]= activeUnit_;
517 sendBuffer[4]= panSpeed_;
518 sendBuffer[5]= tiltSpeed_;
523 this->SendCommand_(sendBuffer,9);
527 void ViscaControl::ZoomTele()
529 sendBuffer[0]= activeUnit_;
536 this->SendCommand_(sendBuffer,6);
541 void ViscaControl::ZoomWide()
543 sendBuffer[0]= activeUnit_;
550 this->SendCommand_(sendBuffer,6);
553 void ViscaControl::ZoomStop()
555 sendBuffer[0]= activeUnit_;
564 this->SendCommand_(sendBuffer,6);
567 void ViscaControl::ResetPanTilt()
569 sendBuffer[0]= activeUnit_;
575 this->SendCommand_(sendBuffer,5);
578 void ViscaControl::ResetCamCustom()
580 sendBuffer[0]= activeUnit_;
588 this->SendCommand_(sendBuffer,7);
591 void ViscaControl::ZoomInq()
593 sendBuffer[0]= activeUnit_;
601 this->SendCommand_(sendBuffer,5);
605 void ViscaControl::ZoomDirect(
short zoom)
608 zoom=(short)maxZoom_;
614 cout<<
"Zoom:"<<zoom<<endl;
615 sendBuffer[0]= activeUnit_;
620 sendBuffer[4]= (
unsigned char)((zoom >> 12)& 0x0F);
621 sendBuffer[5]= (
unsigned char)((zoom >> 8)& 0x0F);
622 sendBuffer[6]= (
unsigned char)((zoom >> 4)& 0x0F);
623 sendBuffer[7]= (
unsigned char)(zoom & 0x0F);
627 this->SendCommand_(sendBuffer,9);
634 sendBuffer[0]= activeUnit_;
641 if(mode == VISCA_INTERLACED){
644 else if(mode == VISCA_PROGRESSIVE){
649 return this->SendCommand_(sendBuffer,8) != 0;
653 sendBuffer[0]= activeUnit_;
659 this->SendCommand_(sendBuffer,6,
false);
662 unsigned char* receiveBuffer=
new unsigned char[length];
663 for (
int i=0;i<length;i++) receiveBuffer[i]=0;
665 bool ret = serialIO_.ReadBytes(receiveBuffer,length,
true) != 0;
666 if(!ret)BIASERR(
"Could not read answer");
668 if (receiveBuffer[2]==0x00 && receiveBuffer[3]==0x00) {
669 result= VISCA_INTERLACED;
671 else if(receiveBuffer[2]==0x00 && receiveBuffer[3]==0x01) {
672 result= VISCA_PROGRESSIVE;
675 BIASERR(
"Unknown CCD Scanning Mode!");
678 delete[] receiveBuffer;