track.c

motion motion

******************************************************************************/
#ifndef WITHOUT_V4L
static unsigned short int lqos_center(struct context *cnt, int dev, int x_angle, int y_angle)
{
	int reset = 3;
	struct pwc_mpt_angles pma;
	struct pwc_mpt_range pmr;

	if (cnt->track.dev==-1) {

		if (ioctl(dev, VIDIOCPWCMPTRESET, &reset) == -1) {
			motion_log(LOG_ERR, 1, "Failed to reset pwc camera to starting position! Reason");
			return 0;
		}

		SLEEP(6,0)

		if (ioctl(dev, VIDIOCPWCMPTGRANGE, &pmr) == -1) {
			motion_log(LOG_ERR, 1, "failed VIDIOCPWCMPTGRANGE");
			return 0;
		}

		cnt->track.dev = dev;
		cnt->track.minmaxfound = 1;
		cnt->track.panmin = pmr.pan_min;
		cnt->track.panmax = pmr.pan_max;
		cnt->track.tiltmin = pmr.tilt_min;
		cnt->track.tiltmax = pmr.tilt_max;
	}

	if (ioctl(dev, VIDIOCPWCMPTGANGLE, &pma) == -1)
		motion_log(LOG_ERR, 1, "ioctl VIDIOCPWCMPTGANGLE");
	
	pma.absolute = 1;

	if (x_angle * 100 < cnt->track.panmax && x_angle * 100 > cnt->track.panmin)
		pma.pan = x_angle * 100;

	if (y_angle * 100 < cnt->track.tiltmax && y_angle * 100 > cnt->track.tiltmin)
		pma.tilt = y_angle * 100;

	if (ioctl(dev, VIDIOCPWCMPTSANGLE, &pma) == -1) {
		motion_log(LOG_ERR, 1, "Failed to pan/tilt pwc camera! Reason");
		return 0;
	}

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

	return cnt->track.move_wait;
}

static unsigned short int lqos_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, unsigned short int manual)
{
	int delta_x = cent->x - (imgs->width / 2);
	int delta_y = cent->y - (imgs->height / 2);
	int move_x_degrees, move_y_degrees;
	struct pwc_mpt_angles pma;
	struct pwc_mpt_range pmr;

	/* If we are on auto track we calculate delta, otherwise we use user input in degrees times 100 */
	if (!manual) {
		if (delta_x > imgs->width * 3/8 && delta_x < imgs->width * 5/8)
			return 0;
		if (delta_y > imgs->height * 3/8 && delta_y < imgs->height * 5/8)
			return 0;

		move_x_degrees = delta_x * cnt->track.step_angle_x * 100 / (imgs->width / 2);
		move_y_degrees = -delta_y * cnt->track.step_angle_y * 100 / (imgs->height / 2);
	} else {
		move_x_degrees = cent->x * 100;
		move_y_degrees = cent->y * 100;
	}
	
	/* If we never checked for the min/max values for pan/tilt we do it now */
	if (cnt->track.minmaxfound == 0) {
		if (ioctl(dev, VIDIOCPWCMPTGRANGE, &pmr) == -1) {
			motion_log(LOG_ERR, 1, "failed VIDIOCPWCMPTGRANGE");
			return 0;
		}
		cnt->track.minmaxfound = 1;
		cnt->track.panmin = pmr.pan_min;
		cnt->track.panmax = pmr.pan_max;
		cnt->track.tiltmin = pmr.tilt_min;
		cnt->track.tiltmax = pmr.tilt_max;
	}

	/* Get current camera position */
	if (ioctl(dev, VIDIOCPWCMPTGANGLE, &pma) == -1)
		motion_log(LOG_ERR, 1, "ioctl VIDIOCPWCMPTGANGLE");


	/* Check current position of camera and see if we need to adjust
	   values down to what is left to move */
	if (move_x_degrees<0 && (cnt->track.panmin - pma.pan) > move_x_degrees)
		move_x_degrees = (cnt->track.panmin - pma.pan);

	if (move_x_degrees>0 && (cnt->track.panmax - pma.pan) < move_x_degrees)
		move_x_degrees = (cnt->track.panmax - pma.pan);

	if (move_y_degrees<0 && (cnt->track.tiltmin - pma.tilt) > move_y_degrees)
		move_y_degrees = (cnt->track.tiltmin - pma.tilt);

	if (move_y_degrees>0 && (cnt->track.tiltmax - pma.tilt) < move_y_degrees)
		move_y_degrees = (cnt->track.tiltmax - pma.tilt);
		
	/* Move camera relative to current position */
	pma.absolute = 0;
	pma.pan = move_x_degrees;
	pma.tilt = move_y_degrees;

	if (ioctl(dev, VIDIOCPWCMPTSANGLE, &pma) == -1) {
		motion_log(LOG_ERR, 1, "Failed to pan/tilt pwc camera! Reason");
		return 0;
	}

	return cnt->track.move_wait;
}
/******************************************************************************

	Logitech QuickCam Sphere camera tracking code by oBi
	
	Modify by Dirk Wesenberg(Munich) 30.03.07 
	- for new API in uvcvideo 
	- add Trace-steps for investigation
******************************************************************************/
#ifdef MOTION_V4L2

static unsigned short int uvc_center(struct context *cnt, int dev, int x_angle, int y_angle)
{
	/* CALC ABSOLUTE MOVING : Act.Position +/- delta to request X and Y */
	int move_x_degrees = 0, move_y_degrees = 0;

	union pantilt {
		struct {
			short pan;
			short tilt;
		} s16;
		int value;
	};
	union pantilt pan;

	if (cnt->track.dev==-1) {

		int reset = 3; //0-non reset, 1-reset pan, 2-reset tilt, 3-reset pan&tilt
		struct v4l2_control control_s;

		control_s.id = V4L2_CID_PANTILT_RESET;
		control_s.value = (unsigned char) reset;

		if (ioctl(dev, VIDIOC_S_CTRL, &control_s) < 0) {
			motion_log(LOG_ERR, 1, "Failed to reset UVC camera to starting position! Reason");
			return 0;
		}
		motion_log(LOG_DEBUG, 0, "Reseting UVC camera to starting position");

		SLEEP(8,0)

		/* Get camera range */
		struct v4l2_queryctrl queryctrl;

		queryctrl.id = V4L2_CID_PAN_RELATIVE;
		if (ioctl(dev, VIDIOC_QUERYCTRL, &queryctrl) < 0 ) {
			motion_log(LOG_ERR, 1, "ioctl querycontrol error %d",errno);
			return 0;
		}
		motion_log(LOG_DEBUG, 0, "Getting camera range");
		

		/* DWe 30.03.07 The orig request failed : 
		* must be VIDIOC_G_CTRL separate for pan and tilt or via VIDIOC_G_EXT_CTRLS - now for 1st manual 
		* Range X = -70 to +70 degrees              
		* Y = -30 to +30 degrees  
		*/	
		
//		//get mininum
//		pan.value = queryctrl.minimum;

		cnt->track.panmin = -4480 / INCPANTILT;
		cnt->track.tiltmin = -1920 / INCPANTILT;
//		//get maximum
		cnt->track.panmax = 4480 / INCPANTILT; 
		cnt->track.tiltmax = 1920 / INCPANTILT;
//		pan.value = queryctrl.maximum;

		cnt->track.dev = dev;
		cnt->track.pan_angle = 0;
		cnt->track.tilt_angle = 0;
		cnt->track.minmaxfound = 1;

	}

	struct v4l2_control control_s;

	motion_log(LOG_DEBUG, 0, "INPUT_PARAM_ABS pan_min %d,pan_max %d,tilt_min %d,tilt_max %d ", cnt->track.panmin, cnt->track.panmax, cnt->track.tiltmin, cnt->track.tiltmax );
	motion_log(LOG_DEBUG, 0, "INPUT_PARAM_ABS X_Angel %d, Y_Angel %d ", x_angle, y_angle);

	if (x_angle <= cnt->track.panmax && x_angle >= cnt->track.panmin)
		move_x_degrees = x_angle - (cnt->track.pan_angle);

	if (y_angle <= cnt->track.tiltmax && y_angle >= cnt->track.tiltmin)
		move_y_degrees = y_angle - (cnt->track.tilt_angle);
			

	/*
	tilt up: - value
	tilt down: + value
	pan left: - value
	pan right: + value
	*/
	pan.s16.pan = -move_x_degrees * INCPANTILT;
	pan.s16.tilt = -move_y_degrees * INCPANTILT;
	
	motion_log(LOG_DEBUG, 0, "For_SET_ABS move_X %d,move_Y %d", move_x_degrees, move_y_degrees);
		
	/* DWe 30.03.07 Must be broken in diff calls, because 
    	- one call for both is not accept via VIDIOC_S_CTRL -> maybe via VIDIOC_S_EXT_CTRLS
    	- The Webcam or uvcvideo does not like a call with a zero-move 
	*/
    
	if (move_x_degrees != 0 ) {
		control_s.id = V4L2_CID_PAN_RELATIVE;
	//	control_s.value = pan.value;
		control_s.value = pan.s16.pan;
		if (ioctl(dev, VIDIOC_S_CTRL, &control_s) < 0) {
			motion_log(LOG_ERR, 1, "Failed to move UVC camera!");
			return 0;
		}
	}

	/* DWe 30.03.07 We must wait a little,before we set the next CMD, otherwise PAN is mad ... */ 	
	if ((move_x_degrees != 0) && (move_y_degrees != 0)) {
		SLEEP (1,0);
	}   
	
	if (move_y_degrees != 0 ) {
		control_s.id = V4L2_CID_TILT_RELATIVE;
	//	control_s.value = pan.value;
		control_s.value = pan.s16.tilt;
		if (ioctl(dev, VIDIOC_S_CTRL, &control_s) < 0) {
			motion_log(LOG_ERR, 1, "Failed to move UVC camera!");
			return 0;
		}	
	
	}

	motion_log(LOG_DEBUG, 0,"Found MINMAX = %d", cnt->track.minmaxfound); 

	if (cnt->track.dev!=-1) {
		motion_log(LOG_DEBUG, 0," Before_ABS_Y_Angel : x= %d , Y= %d , ", cnt->track.pan_angle, cnt->track.tilt_angle );
		if (move_x_degrees != -1) { 
			cnt->track.pan_angle += move_x_degrees;
		}
		if (move_x_degrees != -1) { 
			cnt->track.tilt_angle += move_y_degrees;
		}
		motion_log(LOG_DEBUG, 0," After_ABS_Y_Angel : x= %d , Y= %d , ", cnt->track.pan_angle, cnt->track.tilt_angle );	
	}

	return cnt->track.move_wait;
}

static unsigned short int uvc_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, unsigned short int manual)
{
	/* RELATIVE MOVING : Act.Position +/- X and Y */
	
	int delta_x = cent->x - (imgs->width / 2);
	int delta_y = cent->y - (imgs->height / 2);
	int move_x_degrees, move_y_degrees;
	
	/* DWe 30.03.07 Does the request of act.position from WebCam work ? luvcview shows at every position 180 :( */
	/*		Now we init the Web by call Reset, so we can sure, that we are at x/y = 0,0                 */
	/* 		Don't worry, if the WebCam make a sound - over End at PAN  - hmmm, should it be normal ...? */
	/* 		PAN Value 7777 in relative will init also a want reset for CAM - it will be "0" after that  */  
	if (( cnt->track.minmaxfound != 1) || (cent->x == 7777 )) {
		unsigned short int reset = 3; //0-non reset, 1-reset pan, 2-reset tilt, 3-reset pan&tilt
		struct v4l2_control control_s;

		control_s.id = V4L2_CID_PANTILT_RESET;
		control_s.value = (unsigned char) reset;

		if (ioctl(dev, VIDIOC_S_CTRL, &control_s) < 0) {
			motion_log(LOG_ERR, 1, "Failed to reset UVC camera to starting position! Reason");
			return 0;
		}
		motion_log(LOG_DEBUG, 0, "Reseting UVC camera to starting position");
		
		/* set the "helpvalue" back to null because after reset CAM should be in x=0 and not 70 */
		cent->x = 0;
		SLEEP(8,0);
		
		/* DWe 30.03.07 The orig request failed : 
		* must be VIDIOC_G_CTRL separate for pan and tilt or via VIDIOC_G_EXT_CTRLS - now for 1st manual 
		* Range X = -70 to +70 degrees              
		*	Y = -30 to +30 degrees  
		*/	

		cnt->track.panmin = -4480 / INCPANTILT;
		cnt->track.tiltmin = -1920 / INCPANTILT;
		cnt->track.panmax = 4480 / INCPANTILT; 
		cnt->track.tiltmax = 1920 / INCPANTILT;
		cnt->track.dev = dev;
		cnt->track.pan_angle = 0;
		cnt->track.tilt_angle = 0;
		cnt->track.minmaxfound = 1;
	}

	
	/* If we are on auto track we calculate delta, otherwise we use user input in degrees */
	if (!manual) {
		if (delta_x > imgs->width * 3/8 && delta_x < imgs->width * 5/8)
			return 0;
		if (delta_y > imgs->height * 3/8 && delta_y < imgs->height * 5/8)
			return 0;

		move_x_degrees = delta_x * cnt->track.step_angle_x / (imgs->width / 2);
		move_y_degrees = -delta_y * cnt->track.step_angle_y / (imgs->height / 2);
	} else {
		move_x_degrees = cent->x;
		move_y_degrees = cent->y;
	}

	union pantilt {
		struct {
			short pan;
			short tilt;
		} s16;
		int value;
	};

	struct v4l2_control control_s;
	union pantilt pan;

	if (cnt->track.minmaxfound == 1) {
	/* Check current position of camera and see if we need to adjust
	values down to what is left to move */
		if (move_x_degrees<0 && (cnt->track.panmin - cnt->track.pan_angle) > move_x_degrees)
			move_x_degrees = (cnt->track.panmin - cnt->track.pan_angle);

		if (move_x_degrees>0 && (cnt->track.panmax - cnt->track.pan_angle) < move_x_degrees)
			move_x_degrees = (cnt->track.panmax - cnt->track.pan_angle);

		if (move_y_degrees<0 && (cnt->track.tiltmin - cnt->track.tilt_angle) > move_y_degrees)
			move_y_degrees = (cnt->track.tiltmin - cnt->track.tilt_angle);

		if (move_y_degrees>0 && (cnt->track.tiltmax - cnt->track.tilt_angle) < move_y_degrees)
			move_y_degrees = (cnt->track.tiltmax - cnt->track.tilt_angle);
	}


	motion_log(LOG_DEBUG, 0, "For_SET_REL pan_min %d,pan_max %d,tilt_min %d,tilt_max %d ", cnt->track.panmin, cnt->track.panmax, cnt->track.tiltmin, cnt->track.tiltmax );
	motion_log(LOG_DEBUG, 0, "For_SET_REL track_pan_Angel %d, track_tilt_Angel %d ", cnt->track.pan_angle, cnt->track.tilt_angle);	
	motion_log(LOG_DEBUG, 0, "For_SET_REL move_X %d,move_Y %d", move_x_degrees, move_y_degrees);
	/*
	tilt up: - value
	tilt down: + value
	pan left: - value
	pan right: + value
	*/

	pan.s16.pan = -move_x_degrees * INCPANTILT;
	pan.s16.tilt = -move_y_degrees * INCPANTILT;
	
	/* DWe 30.03.07 Must be broken in diff calls, because 
    	   - one call for both is not accept via VIDIOC_S_CTRL -> maybe via VIDIOC_S_EXT_CTRLS
    	   - The Webcam or uvcvideo does not like a call with a zero-move 
    	*/

	if (move_x_degrees != 0) {

		control_s.id = V4L2_CID_PAN_RELATIVE;

//	control_s.value = pan.value;
		control_s.value = pan.s16.pan;
		motion_log(LOG_DEBUG, 0," dev %d,addr= %d, control_S= %d,Wert= %d,", dev,VIDIOC_S_CTRL, &control_s, pan.s16.pan ); 
		if (ioctl(dev, VIDIOC_S_CTRL, &control_s) < 0) {
	    	    motion_log(LOG_ERR, 1, "Failed to move UVC camera!");
		    return 0;
		}
	}
	
	/* DWe 30.03.07 We must wait a little,before we set the next CMD, otherwise PAN is mad ... */ 	
        if ((move_x_degrees != 0) && (move_y_degrees != 0)) {
	    SLEEP (1,0);
	}   


	if (move_y_degrees != 0) {

	control_s.id = V4L2_CID_TILT_RELATIVE;

//	control_s.value = pan.value;
	control_s.value = pan.s16.tilt;
		motion_log(LOG_DEBUG, 0," dev %d,addr= %d, control_S= %d, Wert= %d, ", dev,VIDIOC_S_CTRL, &control_s, pan.s16.tilt ); 
		if (ioctl(dev, VIDIOC_S_CTRL, &control_s) < 0) {
		motion_log(LOG_ERR, 1, "Failed to move UVC camera!");
		return 0;
		}
	}
	
  
    		motion_log(LOG_DEBUG, 0,"Found MINMAX = %d", cnt->track.minmaxfound); 
		if (cnt->track.minmaxfound == 1) {
			motion_log(LOG_DEBUG, 0," Before_REL_Y_Angel : x= %d , Y= %d", cnt->track.pan_angle, cnt->track.tilt_angle  );
		
		    	if (move_x_degrees != 0){ 
			    cnt->track.pan_angle += -pan.s16.pan / INCPANTILT;
			}	
			if (move_y_degrees != 0){
			    cnt->track.tilt_angle += -pan.s16.tilt / INCPANTILT;
			}    
			motion_log(LOG_DEBUG, 0," After_REL_Y_Angel : x= %d , Y= %d", cnt->track.pan_angle, cnt->track.tilt_angle  );
		}

	return cnt->track.move_wait;
}
#endif /* MOTION_V4L2 */
#endif /* WITHOUT_V4L */