yaan.c

本人写的linux下云台控制程序

/****************************************************************************
 * Resource of the protocol switch.
 * Name: protocol.c
 * Author: EdgarLiu
 * Date: 2004/9/6
****************************************************************************/
#include <stdio.h>

#include "protocol.h"
#include "yaan.h"


struct TDVSSS_command  yaan_cmd[] = {
	{ACTION_AUTO_FOCUS,		"af",	"",	camera_func},
	{ACTION_AUTO_IRIS,		"ai",	"",	camera_func},
	{ACTION_CALL_PREP ,		"cp",	"",	yaan_cmd_2},
	{ACTION_START_PREP,		"p",	"",	yaan_cmd_3},
	{ACTION_SAVE_PREP,		"sp",	"",	yaan_cmd_5},
	{ACTION_INSERT_PREP,		"ip",	"",	yaan_cmd_6},
	{ACTION_DELETE_PREP,		"dp",	"",	yaan_cmd_7},
	{ACTION_CLEAR_PREP,		"c",	"",	yaan_cmd_8},
	{ACTION_DWELL_TIME,		"dt",	"",	yaan_cmd_11},
	{ACTION_PT_SPEED,		"pt",	"",	yaan_cmd_14},
	{ACTION_AUTO_PANNING,		"ap",	"",	yaan_cmd_15},
	{ACTION_AUTO_PANNING_SPEED,	"aps",	"",	yaan_cmd_15},
	{ACTION_AUTO_PANNING_LIMITS,	"apl",	"",	yaan_cmd_15},
	{0,0,0,0,0}
};

// The implement of yaan protocol
static unsigned char checksum(struct TDVSSS_protocol* tp,int start,int end)
{
	int sum = 0,i;
	for(i = start;i <= end;i++)
		sum += tp->out[i];
	return (unsigned char)sum % 256;	
}

static void public_out(struct TDVSSS_protocol* tp)
{
	tp->out[0] = 0x02;
	tp->out[1] = tp->address;

}

int pan_tilt_func(struct TDVSSS_protocol* tp)
{
	tp->out_len = 12;
	tp->out[2] = 0x01;
	public_out(tp);
	
	switch (tp->user_input.cmd_id)
	{
	case ACTION_RIGHT:
		tp->out[3] = 0x01;
		tp->out[4] = 0x00;
		tp->out[9] = tp->pan_speed;
		tp->out[10] = tp->tilt_speed;
		break;
	case ACTION_LEFT:
		tp->out[3] = 0x02;
		tp->out[4] = 0x00;
		tp->out[9] = tp->pan_speed;
		tp->out[10] = tp->tilt_speed;
		break;
	case ACTION_UP:
		tp->out[3] = 0x04;
		tp->out[4] = 0x00;
		tp->out[9] = tp->pan_speed;
		tp->out[10] = tp->tilt_speed;
		break;
	case ACTION_DOWN:
		tp->out[3] = 0x08;
		tp->out[4] = 0x00;
		tp->out[9] = tp->pan_speed;
		tp->out[10] = tp->tilt_speed;
		break;
	case ACTION_STOP:
		tp->out[3] = 0x00;
		tp->out[4] = 0x00;
		tp->out[9] = 0x00;
		tp->out[10] = 0x00;
	}	
	tp->out[5] = checksum(tp,0,4);
	tp->out[6] = 0x02;
	tp->out[7] = tp->address;
	tp->out[8] = 0x0e;
	tp->out[11] = checksum(tp,6,10);

	return 0;
}

int camera_func(struct TDVSSS_protocol* tp)
{
	tp->out_len = 6;
	tp->out[2] = 0x01;
	public_out(tp);

	switch (tp->user_input.cmd_id)
	{
	case ACTION_ZOOM_OUT:
		tp->out[3] = 0x10;
		tp->out[4] = 0x00;
		break;
	case ACTION_ZOOM_IN:
		tp->out[3] = 0x20;
		tp->out[4] = 0x00;
		break;
	case ACTION_FOCUS_NEAR:
		tp->out[3] = 0x40;
		tp->out[4] = 0x00;
		break;	
	case ACTION_FOCUS_FAR:
		tp->out[3] = 0x80;
		tp->out[4] = 0x00;
		break;	
	case ACTION_IRIS_OPEN:
		tp->out[3] = 0x00;
		tp->out[4] = 0x01;
		break;	
	case ACTION_IRIS_CLOSE:
		tp->out[3] = 0x00;
		tp->out[4] = 0x02;
		break;	
	case ACTION_AUTO_FOCUS:
		tp->out[3] = 0xc0;
		tp->out[4] = 0x00;
		break;	
	case ACTION_AUTO_IRIS:
		tp->out[3] = 0x00;
		tp->out[4] = 0x03;
		break;	
	default:
		return NO_SUCH_COMMAND;
	}
	tp->out[5] = checksum(tp,0,4);

	return 0;	

}

int yaan_cmd_2(struct TDVSSS_protocol* tp )
{
	unsigned char buf[2];
	public_out(tp);
	strncpy(buf,tp->user_input.param1,2);
	if(buf[0]== 0 || buf[1] < 1 ||buf[1] > 128)
		return INVALID_ARGUMENT;
	tp->out_len = 5;
	tp->out[2] = 0x02;
	tp->out[3] = tp->user_input.param1[1];
	tp->out[4] = checksum(tp,0,3);
	return SUCCESSFULLY;
}

int yaan_cmd_3(struct TDVSSS_protocol* tp )
{
	public_out(tp);
	tp->out_len = 5;
	tp->out[2] = 0x03;
	tp->out[3] = 0x00;
	tp->out[4] = checksum(tp,0,3);
	return SUCCESSFULLY;
}

/*
void yaan_cmd_4(struct TDVSSS_protocol* tp )
{
	tp->out_len = 5;
}
*/
int yaan_cmd_5(struct TDVSSS_protocol* tp )
{
	if(tp->user_input.param1[0] == 0 || tp->user_input.param1[1] < 1 ||
				tp->user_input.param1[1] > 128)
		return INVALID_ARGUMENT;
	public_out(tp);
	tp->out_len = 5;
	tp->out[2] = 0x05;
	tp->out[3] = tp->user_input.param1[1];
	tp->out[4] = checksum(tp,0,3);
	return SUCCESSFULLY;
}
int yaan_cmd_6(struct TDVSSS_protocol* tp )
{
	if(tp->user_input.param1[0] == 0 || tp->user_input.param1[1] < 1 ||
				tp->user_input.param1[1] > 128)
		return INVALID_ARGUMENT;
	public_out(tp);
	tp->out_len = 5;
	tp->out[2] = 0x06;
	tp->out[3] = tp->user_input.param1[1];
	tp->out[4] = checksum(tp,0,3);
	return SUCCESSFULLY;	
}

int yaan_cmd_7(struct TDVSSS_protocol* tp )
{
	if(tp->user_input.param1[0] == 0 || tp->user_input.param1[1] < 1 ||
				tp->user_input.param1[1] > 128)
		return INVALID_ARGUMENT;
	public_out(tp);
	tp->out_len = 5;
	tp->out[2] = 0x07;
	tp->out[3] = tp->user_input.param1[1];
	tp->out[4] = checksum(tp,0,3);		
	return SUCCESSFULLY;
}

int yaan_cmd_8(struct TDVSSS_protocol* tp )
{
	public_out(tp);
	tp->out_len = 5;
	tp->out[2] = 0x08;
	tp->out[3] = 0x00;
	tp->out[4] = checksum(tp,0,3);	
	return SUCCESSFULLY;
}
/*
void yaan_cmd_9(struct TDVSSS_protocol* tp )
{
	tp->out_len = 5;

}

void yaan_cmd_10(struct TDVSSS_protocol* tp )
{
	tp->out_len = 5;

}
*/

int yaan_cmd_11(struct TDVSSS_protocol* tp )
{
	public_out(tp);
	tp->out_len = 5;
	tp->out[2] = 0x0b;
	tp->out[3] = tp->user_input.param1[1];
	tp->out[4] = checksum(tp,0,3);		
	return SUCCESSFULLY;
}
/*
void yaan_cmd_12(struct TDVSSS_protocol* tp )
{
	tp->out_len = 6;

}
void yaan_cmd_13(struct TDVSSS_protocol* tp )
{
	tp->out_len = 6;

}
*/
int yaan_cmd_14(struct TDVSSS_protocol* tp )
{
	public_out(tp);

	tp->out_len = 6;
	tp->out[2] = 0x0e;
	tp->out[3] = tp->user_input.param1[1];
	tp->out[4] = tp->user_input.param2[1];
	tp->out[5] = checksum(tp,0,4);

	tp->pan_speed  = tp->user_input.param1[1];
	tp->tilt_speed = tp->user_input.param2[1];	
	return SUCCESSFULLY;
}

int yaan_cmd_15(struct TDVSSS_protocol* tp )
{
	public_out(tp);
	tp->out_len = 6;
	tp->out[2] = 0x0f;
	switch (tp->user_input.cmd_id)
	{
	case ACTION_AUTO_PANNING:
		tp->out[3] = 0x03;
		tp->out[4] = 0x00;
		break;
	case ACTION_AUTO_PANNING_SPEED:
		tp->out[3] = 0x01;
		tp->out[4] = tp->user_input.param1[1];
		break;
	case ACTION_AUTO_PANNING_LIMITS:
		if(tp->user_input.param1[1] != 1 && tp->user_input.param1[1] != 2)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x02;
		tp->out[4] = tp->user_input.param1[1];
		break;
	}
	tp->out[5] = checksum(tp,0,4);
	return SUCCESSFULLY;
}
/*
void yaan_cmd_16(struct TDVSSS_protocol* tp )
{
	tp->out_len = 6;
}
void yaan_cmd_17(struct TDVSSS_protocol* tp )
{
	tp->out_len = 6;
}
*/
//**************************************************************************
int yaan_protocol(struct TDVSSS_protocol* tp )
{
	return 0;
}