mirror of
https://github.com/Motion-Project/motion.git
synced 2026-02-02 02:51:28 -05:00
512 lines
14 KiB
C
512 lines
14 KiB
C
/* 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"
|
|
//#include "conf.h"
|
|
//#include "track.h"
|
|
//#include "alg.h"
|
|
|
|
#include "pwc-ioctl.h"
|
|
|
|
|
|
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 int stepper_center(struct context *cnt, int xoff, int yoff ATTRIBUTE_UNUSED);
|
|
static int stepper_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs);
|
|
static int iomojo_center(struct context *cnt, int xoff, int yoff);
|
|
static int iomojo_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs);
|
|
static int lqos_center(struct context *cnt, int dev, int xoff, int yoff);
|
|
static int lqos_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, int manual);
|
|
|
|
/* Add a call to your functions here: */
|
|
int track_center(struct context *cnt, int dev, int manual, int xoff, int yoff)
|
|
{
|
|
if (!manual && !cnt->track.active)
|
|
return 0;
|
|
if (cnt->track.type == TRACK_TYPE_STEPPER){
|
|
int ret;
|
|
ret = stepper_center(cnt, xoff, yoff);
|
|
if (ret < 1) {
|
|
motion_log(LOG_ERR, 1, "track_center: internal error (stepper_center)");
|
|
return 0;
|
|
}
|
|
else return ret;
|
|
}
|
|
else if (cnt->track.type == TRACK_TYPE_PWC)
|
|
return lqos_center(cnt, dev, xoff, yoff);
|
|
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, %d is not a known track-type", cnt->track.type);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Add a call to your functions here: */
|
|
int track_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, int manual)
|
|
{
|
|
if (!manual && !cnt->track.active)
|
|
return 0;
|
|
if (cnt->track.type == TRACK_TYPE_STEPPER)
|
|
return stepper_move(cnt, dev, cent, imgs);
|
|
else if (cnt->track.type == TRACK_TYPE_PWC)
|
|
return lqos_move(cnt, dev, cent, imgs, manual);
|
|
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, %d is not a known track-type", cnt->track.type);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
|
|
Stepper motor on serial port
|
|
|
|
******************************************************************************/
|
|
|
|
|
|
static int stepper_command(struct context *cnt, int motor, int command, int n)
|
|
{
|
|
char buffer[3];
|
|
time_t timeout=time(NULL);
|
|
|
|
buffer[0]=motor;
|
|
buffer[1]=command;
|
|
buffer[2]=n;
|
|
if (write(cnt->track.dev, buffer, 3)!=3)
|
|
return -1;
|
|
|
|
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 int stepper_status(struct context *cnt, int motor)
|
|
{
|
|
return stepper_command(cnt, motor, STEPPER_COMMAND_STATUS, 0);
|
|
}
|
|
|
|
|
|
static int stepper_center(struct context *cnt, int x_offset, int y_offset)
|
|
{
|
|
struct termios adtio;
|
|
|
|
if (cnt->track.dev<0) {
|
|
motion_log(LOG_INFO, 1, "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 -1;
|
|
}
|
|
|
|
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 -1;
|
|
}
|
|
motion_log(LOG_INFO, 1, "Opened serial device %s and initialize", cnt->track.port);
|
|
}
|
|
|
|
/* 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 int stepper_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs)
|
|
{
|
|
int command = 0;
|
|
int n = 0;
|
|
|
|
if (dev < 0){
|
|
motion_log(LOG_INFO, 1, "No device started yet , try stepper_center()");
|
|
if (stepper_center(cnt, 0, 0) < 0){
|
|
motion_log(LOG_ERR, 1, "Stepper_center() failed to initialize stepper device.");
|
|
return(0);
|
|
}
|
|
motion_log(LOG_INFO, 1, "stepper_center() succeed , device started");
|
|
}
|
|
|
|
/* x-axis */
|
|
|
|
if (cent->x < imgs->width / 2) {
|
|
command = STEPPER_COMMAND_LEFT_N;
|
|
n = imgs->width / 2 - cent->x;
|
|
}
|
|
|
|
if (cent->x > imgs->width / 2) {
|
|
command = STEPPER_COMMAND_RIGHT_N;
|
|
n = cent->x - imgs->width / 2;
|
|
}
|
|
|
|
n = n * cnt->track.stepsize / imgs->width;
|
|
|
|
if (n) stepper_command(cnt, cnt->track.motorx, command, n);
|
|
|
|
/* y-axis */
|
|
|
|
if (cent->y < imgs->height / 2) {
|
|
command = STEPPER_COMMAND_UP_N;
|
|
n = imgs->height / 2 - cent->y;
|
|
}
|
|
|
|
if (cent->y > imgs->height / 2) {
|
|
command = STEPPER_COMMAND_DOWN_N;
|
|
n = cent->y - imgs->height / 2;
|
|
}
|
|
|
|
n = n * cnt->track.stepsize / imgs->height;
|
|
|
|
if (n) stepper_command(cnt, cnt->track.motory, command, n);
|
|
|
|
|
|
return cnt->track.move_wait;
|
|
}
|
|
|
|
/******************************************************************************
|
|
|
|
Iomojo Smilecam on serial port
|
|
|
|
******************************************************************************/
|
|
|
|
static char iomojo_command(struct context *cnt, char *command, int len, 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;
|
|
}
|
|
}
|
|
return buffer[0];
|
|
}
|
|
|
|
static void iomojo_setspeed(struct context *cnt, 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 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);
|
|
}
|
|
|
|
return cnt->track.move_wait;
|
|
}
|
|
|
|
static 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) < 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 folkert@vanheusden.com
|
|
|
|
******************************************************************************/
|
|
|
|
static 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 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 camera! Reason");
|
|
return 0;
|
|
}
|
|
|
|
return cnt->track.move_wait;
|
|
}
|
|
|
|
static int lqos_move(struct context *cnt, int dev, struct coord *cent, struct images *imgs, 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 camera! Reason");
|
|
return 0;
|
|
}
|
|
|
|
return cnt->track.move_wait;
|
|
}
|