sonyevid30.cc

机器人仿真软件

    {
      if(temp_reply[i] == 0xFF)
      {
        bufptr = i;
        break;
      }
    }
  }

  temp = numread;
  // if we read extra bytes, keep them around
  if(bufptr == numread-1)
    numread = 0;
  else
  {
    //printf("storing %d bytes\n", numread-(bufptr+1));
    memcpy(buffer,temp_reply+bufptr+1,numread-(bufptr+1));
    numread = numread-(bufptr+1);
  }

  //PrintPacket("Really Received", temp_reply, temp);
  //PrintPacket("Received", temp_reply, bufptr+1);
  
  // strip off leading trash, up to start character 0x90
  for(i = 0;i< bufptr;i++)
  {
    if(temp_reply[i] == 0x90 && temp_reply[i+1] != 0x90)
      break;
  }
  //if(i)
    //printf("SonyEVID30::Receive(): strip off zeros up to: %d\n", i);
  if(i == bufptr)
    return(0);
  memcpy(reply,temp_reply+i,bufptr+1-i);

  // if it's a command completion, record it, then go again
  if((reply[0] == 0x90) && ((reply[1] >> 4) == 0x05) && (reply[2] == 0xFF))
  {
    //puts("got command completion");
    if((reply[1] & 0x0F) == 0x01)
      command_pending1 = false;
    else if((reply[1] & 0x0F) == 0x02)
      command_pending2 = false;
  }

  return(bufptr+1-i);
}

int
SonyEVID30::CancelCommand(char socket)
{
  unsigned char command[MAX_PTZ_MESSAGE_LENGTH];
  unsigned char reply[MAX_PTZ_MESSAGE_LENGTH];
  int reply_len;
  
  //printf("Canceling socket %d\n", socket);

  command[0] = socket;
  command[0] |= 0x20;
  
  if((reply_len = Send(command, 1, reply)) <= 0)
    return(reply_len);
  
  // wait for the response
  while((reply[0] != 0x90) || ((reply[1] >> 4) != 0x06) || 
        !((reply[2] == 0x04) || (reply[2] == 0x05)) || (reply_len != 4))
  {
    if((reply[0] != 0x90) || ((reply[1] >> 4) != 0x05) || (reply[2] != 0xFF))
      PrintPacket("SonyEVID30::CancelCommand(): unexpected response",reply,
                      reply_len);
    //puts("CancelCommand(): calling Receive()");
    if((reply_len = Receive(reply)) <= 0)
      return(reply_len);
  }
  if(socket == 1)
    command_pending1 = false;
  else if(socket == 2)
    command_pending2 = false;
  return(0);
}

int 
SonyEVID30::SendCommand(unsigned char* str, int len, uint8_t camera)
{
  unsigned char reply[MAX_PTZ_PACKET_LENGTH];
  int reply_len;

  if(command_pending1 && command_pending2)
  {
    if((command_pending1 && CancelCommand(1)) || 
       (command_pending2 && CancelCommand(2)))
      return(-1);
  }

  if(command_pending1 && command_pending2)
  {
    puts("2 commands still pending. wait");
    return(-1);
  }

  

  if((reply_len = Send(str, len, reply)) <= 0)
    return(reply_len);
  
  // wait for the ACK
  while((reply[0] != 0x90) || ((reply[1] >> 4) != 0x04) || (reply_len != 3))
  {
    if((reply[0] != 0x90) || ((reply[1] >> 4) != 0x05) || (reply_len != 3))
    {
      PrintPacket("SonyEVID30::SendCommand(): expected ACK, but got", 
                      reply,reply_len);
    }
    //puts("SendCommand(): calling Receive()");
    if((reply_len = Receive(reply)) <= 0)
      return(reply_len);
  }
  
  if((reply[1] & 0x0F) == 0x01)
    command_pending1 = true;
  else if((reply[1] & 0x0F) == 0x02)
    command_pending2 = true;
  else
    fprintf(stderr,"SonyEVID30::SendCommand():got ACK for socket %d\n",
                    reply[1] & 0x0F);

  return(0);
}


int 
SonyEVID30::SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera)
{
  int reply_len;

  if((reply_len = Send(str, len, reply, camera)) <= 0)
    return(reply_len);
  
  // check that it's an information return
  while((reply[0] != 0x90) || (reply[1] != 0x50))
  {
    if((reply[0] != 0x90) || ((reply[1] >> 4) != 0x05) || (reply_len != 3))
    {
      PrintPacket("SonyEVID30::SendCommand(): expected information return, but got", 
                    reply,reply_len);
    }
    //puts("SendRequest(): calling Receive()");
    if((reply_len = Receive(reply)) <= 0)
      return(reply_len);
  }

  return(reply_len);
}

int
SonyEVID30::SendAbsPanTilt(short pan, short tilt)
{
  unsigned char command[MAX_PTZ_MESSAGE_LENGTH];
  short convpan,convtilt;

  printf("Send abs pan/tilt: %d, %d\n", pan, tilt);

  if (abs(pan)>(short)PTZ_PAN_MAX) 
  {
    if (pan < (short) -PTZ_PAN_MAX)
      pan = (short)-PTZ_PAN_MAX;
    else if (pan > (short) PTZ_PAN_MAX)
      pan = (short)PTZ_PAN_MAX;
    puts("Camera pan angle thresholded");
  }

  if(abs(tilt)>(short)PTZ_TILT_MAX) 
  {
    if(tilt<(short)-PTZ_TILT_MAX)
      tilt=(short)-PTZ_TILT_MAX;
    else if(tilt>(short)PTZ_TILT_MAX)
      tilt=(short)PTZ_TILT_MAX;
    puts("Camera tilt angle thresholded");
  }

  convpan = (short)(pan*ptz_pan_conv_factor);
  convtilt = (short)(tilt*ptz_tilt_conv_factor);

  printf("[Conv] Send abs pan/tilt: %d, %d\n", convpan, convtilt);

  command[0] = 0x01;  // absolute position command
  command[1] = 0x06;  // absolute position command
  command[2] = 0x02;  // absolute position command
  command[3] = PTZ_MAX_PAN_SPEED;  // MAX pan speed
  command[4] = PTZ_MAX_TILT_SPEED;  // MAX tilt speed
  // pan position
  command[5] =  (unsigned char)((convpan & 0xF000) >> 12); 
  command[6] = (unsigned char)((convpan & 0x0F00) >> 8);
  command[7] = (unsigned char)((convpan & 0x00F0) >> 4);
  command[8] = (unsigned char)(convpan & 0x000F); 
  // tilt position
  command[9] = (unsigned char)((convtilt & 0xF000) >> 12); 
  command[10] = (unsigned char)((convtilt & 0x0F00) >> 8);
  command[11] = (unsigned char)((convtilt & 0x00F0) >> 4);
  command[12] = (unsigned char)(convtilt & 0x000F); 

  return(SendCommand(command, 13));
}

int
SonyEVID30::SendStepPan(int dir)
{
  unsigned char cmd[MAX_PTZ_MESSAGE_LENGTH];
  
  cmd[0] = 0x01;
  cmd[1] = 0x06;
  cmd[2] = 0x01;
  cmd[3] = PTZ_MAX_PAN_SPEED;
  cmd[4] = PTZ_MAX_TILT_SPEED;
  // if dir >= 0 then pan left, else right
  cmd[5] = dir >= 0 ? 0x01 : 0x02;
  cmd[6] = 0x03;

  printf("step pan\n");

  return (SendCommand(cmd, 7));
}

int
SonyEVID30::SendStepTilt(int dir)
{
  unsigned char cmd[MAX_PTZ_MESSAGE_LENGTH];
  
  cmd[0] = 0x01;
  cmd[1] = 0x06;
  cmd[2] = 0x01;
  cmd[3] = PTZ_MAX_PAN_SPEED;
  cmd[4] = PTZ_MAX_TILT_SPEED;
  cmd[5] = 0x03;
  // if dir >= 0 then tilt up, else down
  cmd[6] = dir >= 0 ? 0x01 : 0x02;

  return (SendCommand(cmd, 7));
}

int
SonyEVID30::GetAbsPanTilt(short* pan, short* tilt)
{
  unsigned char command[MAX_PTZ_MESSAGE_LENGTH];
  unsigned char reply[MAX_PTZ_PACKET_LENGTH];
  int reply_len;
  short convpan, convtilt;

  command[0] = 0x09;
  command[1] = 0x06;
  command[2] = 0x12;
  
  if((reply_len = SendRequest(command,3,reply)) <= 0)
    return(reply_len);

  // first two bytes are header (0x90 0x50)
  
  // next 4 are pan
  convpan = reply[5];
  convpan |= (reply[4] << 4);
  convpan |= (reply[3] << 8);
  convpan |= (reply[2] << 12);

  *pan = (short)(convpan / ptz_pan_conv_factor);
  
  // next 4 are tilt
  convtilt = reply[9];
  convtilt |= (reply[8] << 4);
  convtilt |= (reply[7] << 8);
  convtilt |= (reply[6] << 12);

  *tilt = (short)(convtilt / ptz_tilt_conv_factor);

  return(0);
}

int
SonyEVID30::GetAbsZoom(short* zoom)
{
  unsigned char command[MAX_PTZ_MESSAGE_LENGTH];
  unsigned char reply[MAX_PTZ_PACKET_LENGTH];
  int reply_len;

  command[0] = 0x09;
  command[1] = 0x04;
  command[2] = 0x47;

  if((reply_len = SendRequest(command,3,reply)) <= 0)
    return(reply_len);

  // first two bytes are header (0x90 0x50)
  // next 4 are zoom
  *zoom = reply[5];
  *zoom |= (reply[4] << 4);
  *zoom |= (reply[3] << 8);
  *zoom |= (reply[2] << 12);

  return(0);
}

/*
* Get the device type and version information from the camera.  Query packet is
* in the format: 8x 09 00 02 FF, return packet in the format: y0 50 gg gg hh hh
* jj jj kk FF where
* 	gggg: Vendor ID
*   hhhh: Model ID
*   jjjj: ROM Version
*   kk: socket number (2)
*/
int 
SonyEVID30::GetCameraType(int *model)
{
	unsigned char command[MAX_VER_MESSAGE_LENGTH];
	unsigned char reply[MAX_VER_REPLY_LENGTH];
	int reply_len;

	command[0] = 0x09;
	command[1] = 0x00;
	command[2] = 0x02;

	if ((reply_len = SendRequest(command, 3, reply)) <= 0) {
		return reply_len;
	} else {
		*model = (reply[4] << 8) + reply[5];
	}
	return 0;
}

int
SonyEVID30::SendAbsZoom(short zoom)
{
  unsigned char command[MAX_PTZ_MESSAGE_LENGTH];

  if(zoom<0) {
    zoom=0;
    //puts("Camera zoom thresholded");
  }
  else if(zoom>1023){
    zoom=1023;
    //puts("Camera zoom thresholded");
  }

  command[0] = 0x01;  // absolute position command
  command[1] = 0x04;  // absolute position command
  command[2] = 0x47;  // absolute position command
  
  // zoom position
  command[3] =  (unsigned char)((zoom & 0xF000) >> 12); 
  command[4] = (unsigned char)((zoom & 0x0F00) >> 8);
  command[5] = (unsigned char)((zoom & 0x00F0) >> 4);
  command[6] = (unsigned char)(zoom & 0x000F); 

  return(SendCommand(command, 7));
}


int SonyEVID30::ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr, void * data)
{
  assert(hdr);
  assert(data);

  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_PTZ_REQ_GENERIC, device_addr))
  {
    assert(hdr->size == sizeof(player_ptz_req_generic_t));

    player_ptz_req_generic_t *cfg = (player_ptz_req_generic_t *)data;

    // check whether command or inquiry...
    if (cfg->config[0] == VISCA_COMMAND_CODE) 
    {
      if (SendCommand((uint8_t *)cfg->config, cfg->config_count) < 0) 
        Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, hdr->subtype);
      else
        Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
      return 0;
    } 
    else 
    {
      // this is an inquiry, so we have to send data back
      cfg->config_count = SendRequest((uint8_t*)cfg->config, cfg->config_count, (uint8_t*)cfg->config);
      Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
    }
  }

  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_PTZ_CMD_STATE, device_addr))
  {
    short zoomdemand=0;
    bool newpantilt=true, newzoom=true;
  	
  	assert (hdr->size == sizeof (player_ptz_cmd_t));
  	player_ptz_cmd_t command = *reinterpret_cast<player_ptz_cmd_t *> (data);
    if(pandemand != (int)rint(RTOD(command.pan)))
    {
      pandemand = (int)rint(RTOD(command.pan));
      newpantilt = true;
    }
    if(tiltdemand != (int)rint(RTOD(command.tilt)))
    {
      tiltdemand = (int)rint(RTOD(command.tilt));
      newpantilt = true;
    }
    
    zoomdemand = (1024 * (zoomdemand - this->maxfov)) / (this->minfov - this->maxfov);
    
    if(newzoom)
    {
	  if(SendAbsZoom(zoomdemand))
	  {
	    fputs("SonyEVID30:Main():SendAbsZoom() errored. bailing.\n", stderr);
	    pthread_exit(NULL);
	  }
    }
    
    if(newpantilt)
    {
	  if (!movement_mode) 
	  {
	    pandemand = -pandemand;
	    if(SendAbsPanTilt(pandemand,tiltdemand))
	    {
	      fputs("SonyEVID30:Main():SendAbsPanTilt() errored. bailing.\n", stderr);
	      pthread_exit(NULL);
	    }
	  }
    } 
	return 0;
  }

  return -1;
}


void
SonyEVID30::PrintPacket(char* str, unsigned char* cmd, int len)
{
  printf("%s: ", str);
  for(int i=0;i<len;i++)
    printf(" %.2x", cmd[i]);
  puts("");
}

// this function will be run in a separate thread
void 
SonyEVID30::Main()
{
  player_ptz_data_t data;
//  char buffer[256];
//  size_t buffer_len;
//  void *client;
  short pan,tilt,zoom;
  
  while(1) 
  {
    pthread_testcancel();
    ProcessMessages();
    pthread_testcancel();
    
    
    /* get current state */
    if(GetAbsPanTilt(&pan,&tilt))
    {
	  fputs("SonyEVID30:Main():GetAbsPanTilt() errored. bailing.\n", stderr);
	  pthread_exit(NULL);
    }
    /* get current state */
    if(GetAbsZoom(&zoom))
    {
	  fputs("SonyEVID30:Main():GetAbsZoom() errored. bailing.\n", stderr);
	  pthread_exit(NULL);
    }
    
    // Do the necessary coordinate conversions.  Camera's natural pan
    // coordinates increase clockwise; we want them the other way, so
    // we negate pan here.  Zoom values are converted from arbitrary
    // units to a field of view (in degrees).
    pan = -pan;
    zoom = this->maxfov + (zoom * (this->minfov - this->maxfov)) / 1024; 
    
    if (movement_mode) 
	{
	  if (pandemand-pan) 
	  {
	    SendStepPan(pandemand-pan);
	  }
	
	  if (tiltdemand - tilt) 
	  {
	    SendStepTilt(tiltdemand - tilt);
	  }
	}
  
    // Copy the data.
    data.pan = DTOR(pan);
    data.tilt = DTOR(tilt);
    data.zoom = DTOR(zoom);
    
    /* test if we are supposed to cancel */
    pthread_testcancel();
    Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, &data,sizeof(player_ptz_data_t),NULL);
       
    usleep(PTZ_SLEEP_TIME_USEC);
    }
}