firewire.c

Machine Vision Toolbox for MATLAB (Release 2)澳大利亚机器视觉工具箱 第二版

/* firewire.c
 *
 * Firewire camera interface for Linux
 *
 *	handle = firewire(port, [res], color, rate)
 *	im = firewire(handle)
 *
 *	$Header: /home/autom/pic/cvsroot/image-toolbox/firewire.c,v 1.2 2005/10/23 11:10:56 pic Exp $
 *
 *	$Log: firewire.c,v $
 *	Revision 1.2  2005/10/23 11:10:56  pic
 *	Bug with number of planes in mono mode.
 *
 *	Revision 1.1  2005/10/21 06:07:06  pic
 *	Mex-file wrapper for reading from firewire interface using dc1394 libs
 *	under Linux.
 *
 *
 * Copyright (c) Peter Corke, 2005  Machine Vision Toolbox for Matlab
 *		pic 6/2005
 *
 */
#include "mex.h"
#include <math.h>

#include </usr/include/libraw1394/raw1394.h>
#include </usr/include/libdc1394/dc1394_control.h>


typedef struct
{
	// Firewire interface 

	raw1394handle_t handle;	// handle to raw firewire
	int             verbose;
} FWdev;

typedef struct
{
	// Firewire camera

	raw1394handle_t handle;	// handle to raw firewire
	dc1394_cameracapture cam;	// handle to camera for dc control
	int             Bus;	// bus number camera is on                
	int             isoChannel;	// ISO channel selection
	int             cameraNode;	// Camera node on the bus (chosen by index)
	int             frameRate;	// Camera frame rate
	int             colorFormat;	// Camera mode

	// Useful debugging/operation info
	dc1394_camerainfo camInfo;
	dc1394_miscinfo camMiscInfo;
	dc1394_feature_set camFeatureSet;
	int             verbose;
	int             status;
	int             errCount;
	int             frameCount;
	int             width;
	int             height;
	int             depth;

} FWcamera;


// Input Arguments 

#define	PORT_IN		prhs[0]
#define	HANDLE_IN	prhs[0]
#define	COLOR_IN	prhs[1]
#define	RATE_IN		prhs[2]

// Output Arguments 

#define	IM_OUT		plhs[0]
#define	HANDLE_OUT	plhs[0]

// forward defines
int             FW_close(FWdev * fw);

FWcamera       *FW_camera_init(FWdev * fw, int guid, int format,
			       int framerate);
int             FW_camera_close(FWcamera * c);
int             FW_init_capture(FWcamera * c);
unsigned char  *FW_grab(FWcamera * c);

void            FW_info_print(FWcamera * c);
static void           * FW_error_null(char *s);
static int FW_error(char *s);
static int fw_grab(double *dimage, int slot, int width, int height, int nplanes);
static int fw_grab2(double *Y, double *U, double *V, int slot, int height, int width);
static FWdev          * FW_open(int verbose);
static int fw_open(int port, int mode, int rate);

#define	NCAM	8

static FWcamera *camHandle[NCAM];

#define	BUFLEN	4096

void
mexFunction(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[])
{
	mxArray        *r;
	char            color[BUFLEN];
	double          rate;
	int             port;
	int             cmode;
	int             rmode;
	int             ret;

	/*
	 * Check for proper number of arguments 
	 */

	switch (nrhs) {
	case 3: {
		double         *p;

		// h = firewire(port, color, rate);
		//

		port = round(mxGetScalar(PORT_IN));

		if (mxGetString(COLOR_IN, color, BUFLEN) != 0)
			mexErrMsgTxt("bad string for colormode");
		if (strcmp(color, "mono") == 0)
			cmode = MODE_640x480_MONO;
		else if (strcmp(color, "rgb") == 0)
			cmode = MODE_640x480_RGB;
		else if (strcmp(color, "yuv") == 0)
			cmode = MODE_640x480_YUV422;
		else
			mexErrMsgTxt("bad color spec string");

		// set rate to camera rate less than or equal 
		// to request
		rate = mxGetScalar(RATE_IN);
		if (rate >= 60)
			rmode = FRAMERATE_60;
		else if (rate >= 30)
			rmode = FRAMERATE_30;
		else if (rate >= 15)
			rmode = FRAMERATE_15;
		else if (rate >= 7.5)
			rmode = FRAMERATE_7_5;
		else if (rate >= 3.75)
			rmode = FRAMERATE_3_75;
		else
			rmode = FRAMERATE_1_875;

		ret = fw_open(port, cmode, rmode);
		if (ret < 0)
			mexErrMsgTxt("error opening firewire camera");

		//fprintf(stderr, "rmode=%d, cmode=%d, slot=%d\n", rmode, cmode, ret);
		// save the handle and return it
		HANDLE_OUT = mxCreateDoubleMatrix(1, 1, mxREAL);
		p = mxGetPr(HANDLE_OUT);
		*p = ret;
		break;
	}
	case 1: {
		//  im = firewire(h);
		//
		//  grab a frame from the firewire device and return
		//  it to matlab workspace
		//
		int             slot = round(mxGetScalar(HANDLE_IN));
		double         *p;
		FWcamera       *cam = camHandle[slot];

		if (nlhs == 0)
			return;

		// allocate space according to color format
		switch (cam->colorFormat) {
		case MODE_640x480_MONO:
			IM_OUT = mxCreateDoubleMatrix(cam->height,
						      cam->width,
						      mxREAL);
			if (IM_OUT == NULL) {
				mexErrMsgTxt("couldnt allocate matrix");
				return;
			}
			p = mxGetPr(IM_OUT);
			fw_grab(p, slot, cam->height, cam->width, 1);
			break;

		case MODE_640x480_YUV422: {
			mxArray	*Y, *U, *V;
			const char	*names[] = {"y", "u", "v"};
			int	dims[] = {1, 1};


			Y = mxCreateDoubleMatrix(cam->height, cam->width, mxREAL);

			U = mxCreateDoubleMatrix(cam->height, cam->width/2, mxREAL);
			V = mxCreateDoubleMatrix(cam->height, cam->width/2, mxREAL);

			if ((Y == NULL) || (U == NULL) || (V == NULL)) 
				mexErrMsgTxt("couldnt allocate plane array");
			// create a structure
			IM_OUT = mxCreateStructArray(2, dims, 3, names);
			if (IM_OUT == NULL)
				mexErrMsgTxt("couldnt allocate cell array");
			mxSetField(IM_OUT, 0, "y", Y);
			mxSetField(IM_OUT, 0, "u", U);
			mxSetField(IM_OUT, 0, "v", V);

			fw_grab2(mxGetPr(Y), mxGetPr(U), mxGetPr(V), slot, cam->height, cam->width);
			break;
		    }
		case MODE_640x480_RGB: {
			int             dims[3];

			dims[0] = cam->height;
			dims[1] = cam->width;
			dims[2] = 3;
			IM_OUT = mxCreateNumericArray(3, dims,
						      mxDOUBLE_CLASS,
						      mxREAL);
			if (IM_OUT == NULL) {
				mexErrMsgTxt("couldnt allocate matrix");
				return;
			}
			p = mxGetPr(IM_OUT);
			fw_grab(p, slot, cam->height, cam->width, 3);
			break;
		    }
		}

		return;
	}
	default:
		mexErrMsgTxt("Incorrect number of arguments");
	}
}

static int
fw_open(int port, int mode, int rate)
{
	FWcamera       *cam;
	FWdev          *fw;
	int             slot;

	// initialize a connection to the camera and stash the
	// handle in the slot table
	fw = FW_open(port);
	if (fw == NULL)
		return -1;
	//printf("cmode = %d, rate = %d\n", mode, rate);
	cam = FW_camera_init(fw, 0, mode, rate);
	if (cam == NULL)
		return -1;

	if (FW_init_capture(cam) < 0)
		return FW_error("camera init capture failed");

	//fprintf(stderr, "image is %d x %d\n", cam->width, cam->height);

	// put the camera handle into a vacant slot
	for (slot = 0; slot < NCAM; slot++)
		if (camHandle[slot] == NULL) {
			camHandle[slot] = cam;
			return slot;
		}
	return -1;
}

static int
fw_grab(double *dimage, int slot, int height, int width, int nplanes)
{
	FWcamera       *cam = camHandle[slot];
	unsigned char  *image;
	int             r, c, p;

	image = (unsigned char *) FW_grab(cam);

	for (p = 0; p < nplanes; p++)
		for (c = 0; c < width; c++)
			for (r = 0; r < height; r++)
				*dimage++ = (double) image[c*nplanes+r*width*nplanes+p];
}

static int
fw_grab2(double *Y, double *U, double *V, int slot, int height, int width)
{
	FWcamera       *cam = camHandle[slot];
	unsigned char  *image;
	int             r, c, p;

	image = (unsigned char *) FW_grab(cam);

	for (r = 0; r < height; r++)
		for (c = 0; c < width; c+=2) {
			U[(c>>1)*height+r] = (double) (*image++);
			Y[c*height+r] = (double) (*image++);
			V[(c>>1)*height+r] = (double) (*image++);
			Y[(c+1)*height+r] = (double) (*image++);
		}
}

/*******************************************************************
  below be dragons...
  ******************************************************************/

#include <assert.h>
#include <unistd.h>
#include <memory.h>
#include <time.h>


// Support for the new version of libdc1394
#define DC1394_0_9_0

static const int DMA_BUFFERS = 40;
static const char *DMA_DEVICE_NAME = "/dev/video1394";

static int
FW_error(char *s)
{
	fprintf(stderr, s);
	return -1;
}

static void           *
FW_error_null(char *s)
{
	fprintf(stderr, s);
	return NULL;
}

static FWdev          *
FW_open(int verbose)
{

  /*-----------------------------------------------------------------------
   * Get the raw firewire handle on the desired firewire bus (i/o card)
   *-----------------------------------------------------------------------*/
	FWdev          *fw;

	fw = (FWdev *) calloc(1, sizeof(FWdev));
	if (fw == NULL)
		FW_error_null("FW_i1394_open: no mem");
	fw->handle = dc1394_create_handle(0);	// bus number
	if (fw->handle == NULL)
		FW_error_null("FW_i1394_open: create_handle failed");

	fw->verbose = verbose;
	if (fw->verbose)
		fprintf(stderr, "ifImageServerFW: acquired handle\n");

	return fw;
}

// Initialize the Firewire camera
FWcamera       *