track.c

motion motion

/*	track.c
 *
 *	Experimental motion tracking.
 *
 *	Copyright 2000, Jeroen Vreeken
 *	This program is published under the GNU Public license
 */

#include <math.h>
#include <termios.h>
#include "motion.h"

#ifndef WITHOUT_V4L
#include "pwc-ioctl.h"
#endif


struct trackoptions track_template = {
	dev:            -1,             /* dev open */
	port:           NULL,           /* char *port */
	motorx:         0,              /* int motorx */
	motory:		0,		/* int motory */
	maxx:           0,              /* int maxx; */
	maxy:		0,		/* int maxy; */
	speed:          TRACK_SPEED,    /* speed */
	stepsize:       TRACK_STEPSIZE, /* stepsize */
	active:         0,              /* auto tracking active */
	minmaxfound:    0,  /* flag for minmax values stored for pwc based camera */
	step_angle_x:   10, /* step angle in degrees X-axis that camera moves during auto tracking */
	step_angle_y:   10, /* step angle in degrees Y-axis that camera moves during auto tracking */
	move_wait:      10   /* number of frames to disable motion detection after camera moving */
};


/* Add your own center and move functions here: */
static unsigned short int stepper_center(struct context *cnt, int xoff, int yoff ATTRIBUTE_UNUSED);
static unsigned short int stepper_move(struct context *cnt, struct coord *cent, struct images *imgs);
static unsigned short int iomojo_center(struct context *cnt, int xoff, int yoff);
static unsigned short int iomojo_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs);
#ifndef WITHOUT_V4L
static unsigned short int lqos_center(struct context *cnt, int dev, int xoff, int yoff);
static unsigned short int lqos_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, unsigned short int manual);
#ifdef MOTION_V4L2
static unsigned short int uvc_center(struct context *cnt, int dev, int xoff, int yoff);
static unsigned short int uvc_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, unsigned short int manual);
#endif /* MOTION_V4L2 */
#endif /* WITHOUT_V4L */

/* Add a call to your functions here: */
unsigned short int track_center(struct context *cnt, int dev ATTRIBUTE_UNUSED, unsigned short int manual, int xoff, int yoff)
{
	if (!manual && !cnt->track.active)
		return 0;
	if (cnt->track.type == TRACK_TYPE_STEPPER){
		unsigned short int ret;
		ret = stepper_center(cnt, xoff, yoff);
		if (!ret) {
				motion_log(LOG_ERR, 1, "track_center: internal error (stepper_center)");
				return 0;		
		}
		else return ret;	
	}
#ifndef WITHOUT_V4L	
	else if (cnt->track.type == TRACK_TYPE_PWC)
		return lqos_center(cnt, dev, xoff, yoff);
#ifdef MOTION_V4L2
	else if (cnt->track.type == TRACK_TYPE_UVC)
		return uvc_center(cnt, dev, xoff, yoff);
#endif /* MOTION_V4L2 */
#endif /* WITHOUT_V4L */
	else if (cnt->track.type == TRACK_TYPE_IOMOJO)
		return iomojo_center(cnt, xoff, yoff);
	else if (cnt->track.type == TRACK_TYPE_GENERIC)
		return 10; // FIX ME. I chose to return something reasonable.

	motion_log(LOG_ERR, 1, "track_center: internal error, %hu is not a known track-type", cnt->track.type);

	return 0;
}

/* Add a call to your functions here: */
unsigned short int track_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, unsigned short int manual)
{
	if (!manual && !cnt->track.active)
		return 0;
	if (cnt->track.type == TRACK_TYPE_STEPPER)
		return stepper_move(cnt, cent, imgs);
#ifndef WITHOUT_V4L
	else if (cnt->track.type == TRACK_TYPE_PWC)
		return lqos_move(cnt, dev, cent, imgs, manual);
#ifdef MOTION_V4L2
	else if (cnt->track.type == TRACK_TYPE_UVC)
		return uvc_move(cnt, dev, cent, imgs, manual);
#endif /* MOTION_V4L2 */
#endif /* WITHOUT_V4L */
	else if (cnt->track.type == TRACK_TYPE_IOMOJO)
		return iomojo_move(cnt, dev, cent, imgs);
	else if (cnt->track.type == TRACK_TYPE_GENERIC)
		return cnt->track.move_wait; // FIX ME. I chose to return something reasonable.

	motion_log(LOG_ERR, 1, "track_move: internal error, %hu is not a known track-type", cnt->track.type);

	return 0;
}


/******************************************************************************
	Stepper motor on serial port
	http://www.lavrsen.dk/twiki/bin/view/Motion/MotionTracking
	http://www.lavrsen.dk/twiki/bin/view/Motion/MotionTrackerAPI
******************************************************************************/


static unsigned short int stepper_command(struct context *cnt, unsigned short int motor, unsigned short int command,  
					unsigned short int data)
{
	char buffer[3];
	time_t timeout=time(NULL);

	buffer[0]=motor;
	buffer[1]=command;
	buffer[2]=data;
	if (write(cnt->track.dev, buffer, 3)!=3){
		motion_log(LOG_ERR, 1, "stepper_command port %s dev fd %i, motor %hu command %hu data %hu",
		                       cnt->track.port, cnt->track.dev, motor, command, data);
		return 0;
	}

	while (read(cnt->track.dev, buffer, 1)!=1 && time(NULL) < timeout+1);
	if (time(NULL) >= timeout+2) {
		motion_log(LOG_ERR, 1, "Status byte timeout!");
		return 0;
	}

	return buffer[0];
}


static unsigned short int stepper_status(struct context *cnt,  unsigned short int motor)
{
	return stepper_command(cnt, motor, STEPPER_COMMAND_STATUS, 0);
}


static unsigned short int stepper_center(struct context *cnt, int x_offset, int y_offset)
{
	struct termios adtio;

	if (cnt->track.dev<0) {
		motion_log(LOG_INFO, 0, "Try to open serial device %s", cnt->track.port);
		
		if ((cnt->track.dev=open(cnt->track.port, O_RDWR | O_NOCTTY)) < 0) {
			motion_log(LOG_ERR, 1, "Unable to open serial device %s", cnt->track.port);
			return 0;
		}

		bzero (&adtio, sizeof(adtio));
		adtio.c_cflag= STEPPER_BAUDRATE | CS8 | CLOCAL | CREAD;
		adtio.c_iflag= IGNPAR;
		adtio.c_oflag= 0;
		adtio.c_lflag= 0;	/* non-canon, no echo */
		adtio.c_cc[VTIME]=0;	/* timer unused */
		adtio.c_cc[VMIN]=0;	/* blocking read until 1 char */
		tcflush (cnt->track.dev, TCIFLUSH);

		if (tcsetattr(cnt->track.dev, TCSANOW, &adtio) < 0) {
			motion_log(LOG_ERR, 1, "Unable to initialize serial device %s", cnt->track.port);
			return 0;
		}
		motion_log(LOG_INFO, 0, "Opened serial device %s and initialize, fd %i", cnt->track.port, cnt->track.dev);
	}

	/* x-axis */
	
	stepper_command(cnt, cnt->track.motorx, STEPPER_COMMAND_SPEED, cnt->track.speed);
	stepper_command(cnt, cnt->track.motorx, STEPPER_COMMAND_LEFT_N, cnt->track.maxx);

	while (stepper_status(cnt, cnt->track.motorx) & STEPPER_STATUS_LEFT);

	stepper_command(cnt, cnt->track.motorx, STEPPER_COMMAND_RIGHT_N,
	                cnt->track.maxx / 2 + x_offset * cnt->track.stepsize);

	while (stepper_status(cnt, cnt->track.motorx) & STEPPER_STATUS_RIGHT);

	/* y-axis */

	stepper_command(cnt, cnt->track.motory, STEPPER_COMMAND_SPEED, cnt->track.speed);
	stepper_command(cnt, cnt->track.motory, STEPPER_COMMAND_UP_N, cnt->track.maxy);

	while (stepper_status(cnt, cnt->track.motory) & STEPPER_STATUS_UP)
	
	stepper_command(cnt, cnt->track.motory, STEPPER_COMMAND_DOWN_N,
			cnt->track.maxy / 2 + y_offset * cnt->track.stepsize);
		
	while (stepper_status(cnt, cnt->track.motory) & STEPPER_STATUS_DOWN);
	
	return cnt->track.move_wait;
}

static unsigned short int stepper_move(struct context *cnt, struct coord *cent, struct images *imgs)
{
	unsigned short int command = 0, data = 0;

	if (cnt->track.dev < 0){
		motion_log(LOG_INFO, 0, "No device %s started yet , trying stepper_center()", cnt->track.port);	
		if (!stepper_center(cnt, 0, 0)){
			motion_log(LOG_ERR, 1, "Stepper_center() failed to initialize stepper device on %s , fd [%i].", 
			                        cnt->track.port, cnt->track.dev);	
			return 0;
		}
		motion_log(LOG_INFO, 0, "stepper_center() succeed , device started %s , fd [%i]", cnt->track.port, cnt->track.dev);	
	}

	/* x-axis */
	
	if (cent->x < imgs->width / 2) {
		command = STEPPER_COMMAND_LEFT_N;
		data = imgs->width / 2 - cent->x;
	}

	if (cent->x > imgs->width / 2) {
		command = STEPPER_COMMAND_RIGHT_N;
		data = cent->x - imgs->width / 2;
	}

	data = data * cnt->track.stepsize / imgs->width;

	if (data) stepper_command(cnt, cnt->track.motorx, command, data);

	/* y-axis */

	if (cent->y < imgs->height / 2) {
		command = STEPPER_COMMAND_UP_N;
		data = imgs->height / 2 - cent->y;
	}

	if (cent->y > imgs->height / 2) {
		command = STEPPER_COMMAND_DOWN_N;
		data = cent->y - imgs->height / 2;
	}
	
	data = data * cnt->track.stepsize / imgs->height;

	if (data) stepper_command(cnt, cnt->track.motory, command, data);	
	
	
	return cnt->track.move_wait;
}

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

	Iomojo Smilecam on serial port

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

static char iomojo_command(struct context *cnt, char *command, unsigned short int len, unsigned short int ret)
{
	char buffer[1];
	time_t timeout = time(NULL);

	if (write(cnt->track.dev, command, len) != len)
		return 0;

	if (ret) {
		while (read(cnt->track.dev, buffer, 1) != 1 && time(NULL) < timeout + 2);
		
		if (time(NULL) >= timeout + 2) {
			motion_log(LOG_ERR, 1, "Return byte timeout!");
			return 0;
		}
	}
	/* range values ? */
	return buffer[0];
}

static void iomojo_setspeed(struct context *cnt, unsigned short int speed)
{
	char command[3];
	
	command[0] = IOMOJO_SETSPEED_CMD;
	command[1] = cnt->track.iomojo_id;
	command[2] = speed;
	
	if (iomojo_command(cnt, command, 3, 1)!=IOMOJO_SETSPEED_RET)
		motion_log(LOG_ERR, 1, "Unable to set camera speed");
}

static void iomojo_movehome(struct context *cnt)
{
	char command[2];
	
	command[0] = IOMOJO_MOVEHOME;
	command[1] = cnt->track.iomojo_id;

	iomojo_command(cnt, command, 2, 0);
}

static unsigned short int iomojo_center(struct context *cnt, int x_offset, int y_offset)
{
	struct termios adtio;
	char command[5], direction=0;

	if (cnt->track.dev<0) {
		if ((cnt->track.dev=open(cnt->track.port, O_RDWR | O_NOCTTY)) < 0) {
			motion_log(LOG_ERR, 1, "Unable to open serial device %s", cnt->track.port);
			return 0;
		}
		bzero (&adtio, sizeof(adtio));
		adtio.c_cflag = IOMOJO_BAUDRATE | CS8 | CLOCAL | CREAD;
		adtio.c_iflag = IGNPAR;
		adtio.c_oflag = 0;
		adtio.c_lflag = 0;      /* non-canon, no echo */
		adtio.c_cc[VTIME] = 0;  /* timer unused */
		adtio.c_cc[VMIN] = 0;   /* blocking read until 1 char */
		tcflush(cnt->track.dev, TCIFLUSH);
		if (tcsetattr(cnt->track.dev, TCSANOW, &adtio) < 0) {
			motion_log(LOG_ERR, 1, "Unable to initialize serial device %s", cnt->track.port);
			return 0;
		}
	}

	iomojo_setspeed(cnt, 40);
	iomojo_movehome(cnt);

	if (x_offset || y_offset) {
		if (x_offset > 0)
			direction |= IOMOJO_DIRECTION_RIGHT;
		else {
			direction |= IOMOJO_DIRECTION_LEFT;
			x_offset *= -1;
		}

		if (y_offset > 0)
			direction |= IOMOJO_DIRECTION_UP;
		else {
			direction |= IOMOJO_DIRECTION_DOWN;
			y_offset *= -1;
		}

		if (x_offset > 180)
			x_offset = 180;

		if (y_offset > 60)
			y_offset = 60;

		command[0] = IOMOJO_MOVEOFFSET_CMD;
		command[1] = cnt->track.iomojo_id;
		command[2] = direction;
		command[3] = x_offset;
		command[4] = y_offset;
		iomojo_command(cnt, command, 5, 0);
	}

	motion_log(LOG_INFO, 0, "iomojo_center() succeed");

	return cnt->track.move_wait;
}

static unsigned short int iomojo_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs)
{
	char command[5];
	int direction = 0;
	int nx = 0, ny = 0;
	int i;
	
	if (dev < 0)
		if (!iomojo_center(cnt, 0, 0))
			return 0;

	if (cent->x < imgs->width / 2) {
		direction |= IOMOJO_DIRECTION_LEFT;
		nx = imgs->width / 2 - cent->x;
	}

	if (cent->x > imgs->width / 2) {
		direction |= IOMOJO_DIRECTION_RIGHT;
		nx = cent->x - imgs->width / 2;
	}

	if (cent->y < imgs->height / 2) {
		direction |= IOMOJO_DIRECTION_DOWN;
		ny = imgs->height / 2 - cent->y;
	}

	if (cent->y > imgs->height / 2) {
		direction |= IOMOJO_DIRECTION_UP;
		ny = cent->y - imgs->height / 2;
	}

	nx = nx * 72 / imgs->width;
	ny = ny * 72 / imgs->height;

	if (nx || ny) {
		if (nx > 180)
			nx = 180;

		if (ny > 60)
			ny = 60;

		command[0] = IOMOJO_MOVEOFFSET_CMD;
		command[1] = cnt->track.iomojo_id;
		command[2] = direction;
		command[3] = nx;
		command[4] = ny;
		iomojo_command(cnt, command, 5, 0);

		/* Number of frames to skip while moving */
		if (ny >= nx)
			i = 25 * ny / 90;
		else
			i = 25 * nx / 90;
		return i;
	}

	return 0;
}

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

	Logitech QuickCam Orbit camera tracking code by [e-mail]