canonvcc4.cc

机器人仿真软件

    {
      byte = reply[i+5];
      if (byte < 0x40)
	byte = byte - 0x30;
      else
	byte = byte - 'A' + 10;
      buf[i] = byte;
    }

  // convert the 4-bytes into a number
  u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];

  // convert the number to a value that's meaningful, based on camera specs
  val = (int)(((int)u_val - (int)0x8000) * 0.1125);

  // now set myPan to the response received for where the camera thinks it is
  *pan = val;

  // repeat the steps for the tilt value
  for (i = 0; i < 4; i++)
    {
      byte = reply[i+9];
      if (byte < 0x40)
	byte = byte - 0x30;
      else
	byte = byte - 'A' + 10;
      buf[i] = byte;
    }
  u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
  val =(int)(((int)u_val  - (int)0x8000) * 0.1125);
  *tilt = val;

  return(0);
}

/************************************************************************/
int
canonvcc4::GetAbsZoom(int* zoom)
{
  unsigned char command[MAX_PTZ_COMMAND_LENGTH];
  unsigned char reply[MAX_PTZ_REQUEST_LENGTH];
  int reply_len;
  char byte;
  unsigned char buf[4];
  unsigned int u_zoom;
  int i;
  
  command[0] = 0xFF;
  command[1] = 0x30;
  command[2] = 0x30;
  command[3] = 0x00;
  command[4] = 0xA4;
  command[5] = 0xEF;

  if (SendRequest(command, 6, reply))
    return(-1);
  //  PrintPacket( "getabszoom: ", command, 6);

  reply_len = ReceiveRequestAnswer(reply);

//   if (reply_len < 0)
//     return -1;

  // remove the ascii encoding, and put into 2 bytes
  for (i = 0; i < 4; i++)
  {
    byte = reply[i + 5];
    if (byte < 0x40)
      byte = byte - 0x30;
    else
      byte = byte - 'A' + 10;
    buf[i] = byte;
  }

  // convert the 2 bytes into a number
  u_zoom = 0;
  for (i = 0; i < 4; i++)
    u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i));
  *zoom = u_zoom;  
  return(0);
}

/************************************************************************/
int
canonvcc4::SendAbsZoom(int zoom)
{
  unsigned char command[MAX_PTZ_COMMAND_LENGTH];
  unsigned char buf[5];
  int i;

  if(zoom < 0) 
    zoom = 0;
  //puts("Camera zoom thresholded");
  
  else 
    if(zoom > MAX_ZOOM)
      zoom = MAX_ZOOM;

  command[0] = 0xFF;
  command[1] = 0x30;
  command[2] = 0x30;
  command[3] = 0x00;
  command[4] = 0xB3;

  sprintf((char *)buf, "%4X", zoom);

  for (i=0;i<3;i++)
    if (buf[i] == ' ')
      buf[i] = '0';

  // zoom position
  command[5] = buf[0]; 
  command[6] = buf[1];  
  command[7] = buf[2];  
  command[8] = buf[3];  
  command[9] = 0xEF;

  if (SendCommand(command, 10))
    return -1;
  //  PrintPacket( "setabszoom: ", command, 10);
  return (ReceiveCommandAnswer());
}

/************************************************************************/
 int
 canonvcc4::read_ptz(unsigned char *reply, int size)
{
  struct pollfd poll_fd;
  int len = 0;

  poll_fd.fd = ptz_fd;
  poll_fd.events = POLLIN;
  
  if (poll(&poll_fd, 1, 1000) <= 0)
    return -1;
  len = read(ptz_fd, reply, size);
  if (len < 0)
    return -1;
  
  return len;
}


/************************************************************************/
int
canonvcc4::ReceiveCommandAnswer()
{
  int num;
  unsigned char reply[COMMAND_RESPONSE_BYTES];
  int len = 0;
  unsigned char byte;
  int err;
  // puts("Receivecommandanswer begin\n");
  memset(reply, 0, COMMAND_RESPONSE_BYTES);

  for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++)
    {
      // if we don't get any bytes, or if we've just exceeded the limit
      // then return null
      err = read_ptz(&byte, 1);
      if (byte == (unsigned char)0xFE)
	{
	  reply[0] = byte;
	  len ++;
	  break;
	}
    }
  if (len == 0)
    return -1;

  // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more
  for(num = 1; num <= MAX_PTZ_REQUEST_LENGTH; num++)
    {
      err = read_ptz(&byte, 1);
      if (err == 0) 
	continue;
      if (err < 0)
	{
	  // there are no more bytes, so check the last byte for the footer
	  if (reply[len - 1] !=  (unsigned char)0xEF)
	    {
	      fputs("canonvcc4::receiveRequest: Discarding bad packet.",
		    stderr);
	      return -1;
	    }
	  else
	    break;
	}
      else
	{
	  // add the byte to the array
	  reply[len] = byte;
	  len ++;
	}
    }

  // Check the response
  if (len != 6)
    {
      fputs("canonvcc4::receiveCommand:Incorrect number of bytes in response packet.", stderr);
      return -1;
    }

  // check the header and footer
  if (reply[0] != (unsigned char)0xFE || reply[5] != (unsigned char)0xEF)
    {
      fputs("canonvcc4::receiveCommand: Bad header or footer character in response packet.", stderr);
      return -1;
    }
  // so far so good.  Set myError to the error byte
  error_code = reply[3];
  //PrintPacket("receivecommandasnwer: ", reply, 6);
  if (error_code == CAM_ERROR_NONE)
      return 0;

  return -1;

}

/************************************************************************/
int
canonvcc4::ReceiveRequestAnswer(unsigned char *data)
{
  int num;
  unsigned char reply[MAX_PTZ_REQUEST_LENGTH];
  int len = 0;
  unsigned char byte;
  int err = 0;

  memset(reply, 0, MAX_PTZ_REQUEST_LENGTH);

  for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++)
    {
      // if we don't get any bytes, or if we've just exceeded the limit
      // then return null
      err = read_ptz(&byte, 1); 
      if (byte == (unsigned char)0xFE)
	{
	  reply[0] = byte;
	  len ++;
	  break;
	}
    }
  if (len == 0)
    return -1;
  // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more
  for(num = 1; num <= MAX_PTZ_REQUEST_LENGTH; num++)
    {
      err = read_ptz(&byte, 1);
      if (err == 0)
	continue;
      if (err < 0) 
	{
	  // there are no more bytes, so check the last byte for the footer
	  if (reply[len - 1] !=  (unsigned char)0xEF)
	    {
	      fputs("canonvcc4::receiveRequest: Discarding bad packet.",
		    stderr);
	      return -1;
	    }
	  else
	    break;
	}
      else
	{
	  // add the byte to the array
	  reply[len] = byte;
	  len ++;
	}
    }
  // Check the response length: pt: 14; zoom: 10
  if (len != 6 && len != 8 && len != 14)
    {
      fputs("Arvcc4::packetHandler: Incorrect number of bytes in response packet.", stderr);
      return -1;
    }
  
  if (reply[0] !=  (unsigned char)0xFE || 
      reply[len - 1] != (unsigned char)0xEF)
    {
      fputs("canonvcc4::receiveRequestArvcc4: Bad header or footer character in response packet.", stderr);
      return -1;
    }

  // so far so good.  Set myError to the error byte
  error_code = reply[3];
  //  PrintPacket("receiverequestasnwer: ", reply, len);
  if (error_code == CAM_ERROR_NONE)
    {
      memcpy(data, reply, len);
      return 0;
    }
  return -1;
}

/************************************************************************/
int
canonvcc4::setControlMode()
{
  unsigned char command[MAX_PTZ_COMMAND_LENGTH];

  command[0] = 0xFF;
  command[1] = 0x30;
  command[2] = 0x30;
  command[3] = 0x00;
  command[4] = 0x90;
  command[5] = 0x30;
  command[6] = 0xEF;

  if (SendCommand(command, 7))
    return -1;
  //  usleep(5000000);
  return (ReceiveCommandAnswer());
}
/************************************************************************/
int
canonvcc4::setPower(int on)
{
  unsigned char command[MAX_PTZ_COMMAND_LENGTH];

  command[0] = 0xFF;
  command[1] = 0x30;
  command[2] = 0x30;
  command[3] = 0x00;
  command[4] = 0xA0;
  if (on)
    command[5] = 0x31;
  else
    command[5] = 0x30;
  command[6] = 0xEF;

  if (SendCommand(command, 7))
    return -1;
  //  usleep(5000000);
  return (ReceiveCommandAnswer());
}

/************************************************************************/

// this function will be run in a separate thread
void 
canonvcc4::Main()
{
  player_ptz_data_t data;
  int pan, tilt, zoom;

  while(1) 
    {
      pthread_testcancel();
      ProcessMessages();
      /* get current state */
      if(GetAbsPanTilt(&pan,&tilt) < 0)
	{
	  fputs("canonvcc4:Main():GetAbsPanTilt() errored. bailing.\n", 
		stderr);
	  pthread_exit(NULL);
	}
      /* get current state */

   
      if(GetAbsZoom(&zoom) < 0)
	{
	  fputs("canonvcc4: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->maxfov - this->minfov)) / 1960; 

      data.pan = DTOR((unsigned short)pan);
      data.tilt = DTOR((unsigned short)tilt);
      data.zoom = (unsigned short) (zoom - 3056)/ZOOM_CONV_FACTOR; 
      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);
    }
}

int canonvcc4::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] == 0x01) 
    {
      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;
	}
      
      if(zoomdemand != command.zoom)
	{
	  zoomdemand = (int)command.zoom; //(int)rint(RTOD(command.zoom));
	  newzoom = true;
	}
      ////////////////////////////////////////////////////////////   
      //zoomdemand = (1960 * (this->maxfov - zoomdemand)) / 
      //	(this->maxfov - this->minfov);
      ////////////////////////////////////////////////////////////
      
      if(newzoom)
	{
	  if(SendAbsZoom(zoomdemand))
	    {
	      fputs("canonvcc4:Main():SendAbsZoom() errored. bailing.\n", stderr);
	      pthread_exit(NULL);
	    }
	}
      
      if(newpantilt)
	{
	  pandemand = -pandemand;
	  if(SendAbsPanTilt(pandemand,tiltdemand))
	    {
	      fputs("canonvcc4:Main():SendAbsPanTilt() errored. bailing.\n", stderr);
	      pthread_exit(NULL);
	    }
	  //  }
	} 
      return 0;
    }
  
  return -1;
}




/************************************************************************/


// extern "C" {
//   int player_driver_init(DriverTable* table)
//   {
//     puts("Canonvcc4 driver initializing");
//     canonvcc4_Register(table);
//     puts("Canonvcc4 driver done");
//     return(0);
//   }
// }