/* 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 definesint             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	8static FWcamera *camHandle[NCAM];#define	BUFLEN	4096voidmexFunction(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 intfw_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 intfw_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 intfw_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_0static const int DMA_BUFFERS = 40;static const char *DMA_DEVICE_NAME = "/dev/video1394";static intFW_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 cameraFWcamera       *