dc1394_format7.c

DCAM1394相机Linux下的驱动源码

    printf("(%s) Unable to set video format %d!\n",__FILE__, FORMAT_SCALABLE_IMAGE_SIZE);
    return DC1394_FAILURE;
  }
  
  if (dc1394_set_video_mode(handle, node,mode) != DC1394_SUCCESS) {
    printf("(%s) Unable to set video mode %d!\n", __FILE__, mode);
    return DC1394_FAILURE;
  }

  // get BPP before setting sizes,...
  if (bytes_per_packet==QUERY_FROM_CAMERA) {
    if (dc1394_query_format7_byte_per_packet(handle, node, mode, &bytes_per_packet) != DC1394_SUCCESS){
      printf("(%s) Unable to get F7 bpp %d!\n", __FILE__, mode);
      return DC1394_FAILURE;
    }
  }

  /*-----------------------------------------------------------------------
   *  set image position. If QUERY_FROM_CAMERA was given instead of a
   *  position, use the actual value from camera
   *-----------------------------------------------------------------------*/

  /* The image position should be checked regardless of the left and top values
     as we also use it for the size setting */

  if (dc1394_query_format7_image_position(handle, node, mode, &camera_left, &camera_top) != DC1394_SUCCESS) {
    printf("(%s) Unable to query image position\n", __FILE__);
    return DC1394_FAILURE;
  }
    
  if( left == QUERY_FROM_CAMERA) left = camera_left;
  if( top == QUERY_FROM_CAMERA)  top = camera_top;
  
  if (dc1394_set_format7_image_position(handle, node, mode, left, top) != DC1394_SUCCESS) {
    printf("(%s) Unable to set format 7 image position to "
           "left=%d and top=%d!\n",  __FILE__, left, top);
    return DC1394_FAILURE;
  }

  /*-----------------------------------------------------------------------
   *  If QUERY_FROM_CAMERA was given instead of an image size
   *  use the actual value from camera.
   *-----------------------------------------------------------------------*/
  if( width == QUERY_FROM_CAMERA || height == QUERY_FROM_CAMERA) {
    if (dc1394_query_format7_image_size(handle, node, mode, &camera_width, &camera_height) != DC1394_SUCCESS) {
      printf("(%s) Unable to query image size\n", __FILE__);
      return DC1394_FAILURE;
    }
    
    /* Idea from Ralf Ebeling: we should check if the image sizes are > 0.
       If == 0, we use the maximum size available */
    if (width == QUERY_FROM_CAMERA) {
      if (camera_width>0)
	width = camera_width;
      else
	width = USE_MAX_AVAIL;
    }
    if (height == QUERY_FROM_CAMERA) {
      if (camera_height>0)
	height = camera_height;
      else
	height = USE_MAX_AVAIL;
    }
  }

  /*-----------------------------------------------------------------------
   *  If USE_MAX_AVAIL was given instead of an image size
   *  use the max image size for the given image position
   *-----------------------------------------------------------------------*/
  if( width == USE_MAX_AVAIL || height == USE_MAX_AVAIL) {
    if (dc1394_query_format7_max_image_size(handle, node, mode, &max_width, &max_height) != DC1394_SUCCESS) {
      printf("(%s) Unable to query max image size\n", __FILE__);
      return DC1394_FAILURE;
    }
    if( width == USE_MAX_AVAIL)  width  = max_width - left;
    if( height == USE_MAX_AVAIL) height = max_height - top;
    
  }

  if (dc1394_set_format7_image_size(handle, node, mode, width, height) != DC1394_SUCCESS) {
    printf("(%s) Unable to set format 7 image size to width=%d and height=%d!\n", __FILE__, width, height);
    return DC1394_FAILURE;
  }

  /*-----------------------------------------------------------------------
   *  Bytes-per-packet definition
   *-----------------------------------------------------------------------*/
  if (dc1394_query_format7_recommended_byte_per_packet(handle, node, mode, &recom_bpp) != DC1394_SUCCESS) {
    printf("Recommended byte-per-packet inq error\n");
    return DC1394_FAILURE;
  }
  
  if (dc1394_query_format7_packet_para(handle, node, mode, &unit_bytes, &max_bytes) != DC1394_SUCCESS) { /* PACKET_PARA_INQ */
    printf("Packet para inq error\n");
    return DC1394_FAILURE;
  }

  //fprintf(stderr,"recommended bpp: %d\n",recom_bpp);
  
  switch (bytes_per_packet) {
  case USE_RECOMMENDED:
    if (recom_bpp>0) {
      bytes_per_packet=recom_bpp;
    }
    else { // recom. bpp asked, but register is 0. IGNORED
      printf("(%s) Recommended bytes per packet asked, but register is zero. Falling back to MAX BPP for mode %d \n", __FILE__, mode);
      bytes_per_packet=max_bytes;
    }
    break;
  case USE_MAX_AVAIL:
    bytes_per_packet = max_bytes;
    break;
  //this case was handled by a previous call. Show error if we get in there. 
  case QUERY_FROM_CAMERA:
    /*if (dc1394_query_format7_byte_per_packet(handle, node, mode, &bytes_per_packet) != DC1394_SUCCESS) {
      printf("(%s) Bytes_per_packet query failure\n", __FILE__);
      return DC1394_FAILURE;
    }*/
    // if we wanted QUERY_FROM_CAMERA, the QUERY_FROM_CAMERA value has been overwritten by
    // the current value at the beginning of the program. It is thus not possible to reach this code fragment.
    printf("(%s:%d) Bytes_per_packet error: we should not reach this code region\n", __FILE__,__LINE__);
    break;
  default:
    // we have to take several tricks into account here:
    // 1) BPP could be zero, in which case it becomes MAX_BPP
    // 2) UNIT_BYTES could also be zero, in which case we force it to MAX_BPP.
    //    This actually further forces BPP to be set to MAX_BPP too.

    if (unit_bytes==0) {
      unit_bytes=max_bytes;
    }
    if (bytes_per_packet > max_bytes) {
      bytes_per_packet = max_bytes;
    }
    else {
      if (bytes_per_packet < unit_bytes) {
	bytes_per_packet = unit_bytes;
      }
    }
    bytes_per_packet-=bytes_per_packet % unit_bytes;
    break;
  }
      
  if (dc1394_set_format7_byte_per_packet(handle, node, mode, bytes_per_packet) != DC1394_SUCCESS) {
    printf("(%s) Unable to set format 7 bytes per packet %d \n", __FILE__, mode);
    return DC1394_FAILURE;
  }
  
  if (dc1394_query_format7_byte_per_packet(handle, node, mode, &packet_bytes) == DC1394_SUCCESS) {
    camera->quadlets_per_packet= packet_bytes /4;
    if (camera->quadlets_per_packet<=0) {
      printf("(%s) No format 7 bytes per packet %d \n", __FILE__, mode);
      return DC1394_FAILURE;
    }
    //printf("Camera has now %d bytes per packet\n", packet_bytes);
  }
  else {
    printf("(%s) Unable to get format 7 bytes per packet %d \n", __FILE__, mode);
    return DC1394_FAILURE;
  }
  
  camera->node = node;
  /*
   * TODO: frame_rate not used for format 7, may be calculated
   */
  camera->frame_rate = 0; 
  camera->channel=channel;
  
  /*-----------------------------------------------------------------------
   *  ensure that quadlet aligned buffers are big enough, still expect
   *  problems when width*height  != quadlets_per_frame*4
   *-----------------------------------------------------------------------*/
  if (camerahandle->sw_version == 0x102UL) { // if version is 1.30
    if (dc1394_query_format7_packet_per_frame(handle, node, mode, &packets_per_frame)!=DC1394_SUCCESS) {
      printf("(%s) Unable to get format 7 packets per frame %d \n", __FILE__, mode);
      return DC1394_FAILURE;
    }
    camera->quadlets_per_frame=(packets_per_frame*packet_bytes)/4;
  }
  else {
    // For other specs revisions, we use a trick to determine the total bytes.
    // We don't use the total_bytes register in 1.20 as it has been interpreted in
    // different ways by manufacturers. Thanks to Martin Gramatke for pointing this trick out.
    if (dc1394_query_format7_color_coding_id(handle, node, mode, &color_coding)!=DC1394_SUCCESS) {
      printf("(%s) Unable to get format 7 color coding for mode %d \n", __FILE__, mode);
      return DC1394_FAILURE;
    }
    else {
      //fprintf(stderr,"color coding: %d\n",color_coding);
      packets_per_frame = ((int)(width * height * _Format7BytePerPixel(color_coding)) +
			   bytes_per_packet -1) / bytes_per_packet;
      camera->quadlets_per_frame=(packets_per_frame*bytes_per_packet)/4;
    }
    /*
      if (dc1394_query_format7_total_bytes(handle, node, mode, &camera->quadlets_per_frame)!= DC1394_SUCCESS) {
      printf("(%s) Unable to get format 7 total bytes per frame %d \n", __FILE__, mode);
      return DC1394_FAILURE;
      }
      camera->quadlets_per_frame/=4;
    */
    //fprintf(stderr,"quadlets per frame: %d\n",camera->quadlets_per_frame);
  }

  if (camera->quadlets_per_frame<=0) {
    return DC1394_FAILURE;
  }
  camera->frame_width = width; /* irrespective of pixel depth */
  camera->frame_height= height;
  
  if (is_iso_on){
    if (dc1394_start_iso_transmission(handle,node) != DC1394_SUCCESS) {
      printf("(%s) Unable to start iso transmission!\n", __FILE__);
      return DC1394_FAILURE;
    }
  }

  return DC1394_SUCCESS;
}


/**********************/
/* External functions */
/**********************/


/*=========================================================================
 *  DESCRIPTION OF FUNCTION:  dc1394_setup_format7_capture
 *  ==> see headerfile
 *=======================================================================*/
int
dc1394_setup_format7_capture(raw1394handle_t handle, nodeid_t node,
                             int channel, int mode, int speed,
                             int bytes_per_packet,
                             unsigned int left, unsigned int top,
                             unsigned int width, unsigned int height, 
                             dc1394_cameracapture * camera)

{
  /* printf( "trying to setup format7 with \n"
          "bpp    = %d\n"
          "pos_x  = %d\n"
          "pos_y  = %d\n"
          "size_x = %d\n"
          "size_y = %d\n",
          bytes_per_packet, left, top, width, height);*/
  
  if (_dc1394_basic_format7_setup(handle,node, channel, mode,
                                  speed, bytes_per_packet,
                                  left, top, width, height, camera) ==
      DC1394_FAILURE)
  {
    return DC1394_FAILURE;
  }

  camera->capture_buffer= (int*)malloc(camera->quadlets_per_frame*4);
  
  if (camera->capture_buffer == NULL)
  {
    printf("(%s) unable to allocate memory for capture buffer\n",
           __FILE__);
    return DC1394_FAILURE;
  }
  
  return DC1394_SUCCESS;
}



/*=========================================================================
 *  DESCRIPTION OF FUNCTION:  dc1394_dma_setup_format7_capture
 *  ==> see headerfile
 *=======================================================================*/
int
dc1394_dma_setup_format7_capture(raw1394handle_t handle, nodeid_t node,
                                 int channel, int mode, int speed,
                                 int bytes_per_packet,
                                 unsigned int left, unsigned int top,
                                 unsigned int width, unsigned int height,
                                 int num_dma_buffers,
				 int drop_frames,
				 const char *dma_device_file,
                                 dc1394_cameracapture *camera)
{
    dc1394_camerahandle *camera_handle;
    camera_handle = (dc1394_camerahandle*) raw1394_get_userdata( handle );
 
    if (_dc1394_basic_format7_setup(handle,node, channel, mode,
				    speed, bytes_per_packet,
				    left, top, width, height, camera) != DC1394_SUCCESS)
    {
      return DC1394_FAILURE;
    }

    camera->port = camera_handle->port;
    camera->dma_device_file = dma_device_file;
    camera->drop_frames = drop_frames;

    return _dc1394_dma_basic_setup(channel,num_dma_buffers, camera);

}



int
dc1394_query_format7_max_image_size(raw1394handle_t handle, nodeid_t node,
 				    unsigned int mode,
                                    unsigned int *horizontal_size,
 				    unsigned int *vertical_size)
{
    int retval;
    quadlet_t value;
    
    if ( (mode > MODE_FORMAT7_MAX) || (mode < MODE_FORMAT7_MIN) )
    {
        return DC1394_FAILURE;
    }

    retval= GetCameraFormat7Register(handle, node, mode,
				     REG_CAMERA_FORMAT7_MAX_IMAGE_SIZE_INQ,
				     &value);
    *horizontal_size  = (unsigned int) ( value & 0xFFFF0000UL ) >> 16;
    *vertical_size= (unsigned int) ( value & 0x0000FFFFUL );
    
    return retval;
}
 
int
dc1394_query_format7_unit_size(raw1394handle_t handle, nodeid_t node,
 			       unsigned int mode,
                               unsigned int *horizontal_unit,
 			       unsigned int *vertical_unit)
{
    int retval;
    quadlet_t value;
   
    if ( (mode > MODE_FORMAT7_MAX) || (mode < MODE_FORMAT7_MIN) )
    {
        return DC1394_FAILURE;
    }

    retval=GetCameraFormat7Register(handle, node, mode,
				    REG_CAMERA_FORMAT7_UNIT_SIZE_INQ,
				    &value);
    *horizontal_unit  = (unsigned int) ( value & 0xFFFF0000UL ) >> 16;
    *vertical_unit= (unsigned int) ( value & 0x0000FFFFUL );

    return retval;
}
 
int
dc1394_query_format7_image_position(raw1394handle_t handle, nodeid_t node,
 				    unsigned int mode,
                                    unsigned int *left_position,
 				    unsigned int *top_position)
{
    int retval;
    quadlet_t value;
   
    if ( (mode > MODE_FORMAT7_MAX) || (mode < MODE_FORMAT7_MIN) )
    {
        return DC1394_FAILURE;
    }

    retval=GetCameraFormat7Register(handle, node, mode,
				    REG_CAMERA_FORMAT7_IMAGE_POSITION,
				    &value);
    *left_position = (unsigned int) ( value & 0xFFFF0000UL ) >> 16;
    *top_position= (unsigned int) ( value & 0x0000FFFFUL );       

    return retval;
}
 
 
int
dc1394_query_format7_image_size(raw1394handle_t handle, nodeid_t node,