standard.c

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

#include <stdio.h>

#include "protocol.h"
#include "standard.h"

static struct TDVSSS_protocol* g_tp = NULL;

#ifdef FOR_HTTPD
struct TDVSSS_command  pantilt_cmd[] = {
	{ACTION_LEFT,		"left",		"left [0-255] [0-255]",		pan_tilt_func,	"pan left"},
	{ACTION_RIGHT,		"right",	"right [0-255] [0-255]",	pan_tilt_func,	"pan right"},
	{ACTION_UP,		"up",		"up [0-255] [0-255]",		pan_tilt_func,	"tilt up"},
	{ACTION_DOWN,		"down",		"down [0-255] [0-255]",		pan_tilt_func,	"tilt down"},
	{ACTION_STOP,		"stop",		"stop",				pan_tilt_func,	"stop pantilt"},
	{ACTION_ZOOM_OUT,	"zoomout",	"zoomout [0-15]",		camera_func,	"camera zoom out"},
	{ACTION_ZOOM_IN,	"zoomin",	"zoomin [0-15]",		camera_func,	"camera zoom in"},
	{ACTION_FOCUS_NEAR,	"focusnear",	"focusnear",			camera_func,	"camera focus near"},
	{ACTION_FOCUS_FAR,	"focusfar",	"focusfar",			camera_func,	"camera focus far"},
	{ACTION_IRIS_OPEN,	"irisopen",	"irisopen",			camera_func,	"camera iris open"},
	{ACTION_IRIS_CLOSE,	"irisclose",	"irisclose",			camera_func,	"camera iris close"},
	{ACTION_STOP_CAMERA,	"stopcamera",	"stopcamera",			camera_func,	"stop camera\n"},
	{0,0,0,0,0}
};
#else
struct TDVSSS_command  pantilt_cmd[] = {
	{ACTION_LEFT,		"l",	"l [0-255] [0-255]",	pan_tilt_func,	"pan left"},
	{ACTION_RIGHT,		"r",	"r [0-255] [0-255]",	pan_tilt_func,	"pan right"},
	{ACTION_UP,		"u",	"u [0-255] [0-255]",	pan_tilt_func,	"tilt up"},
	{ACTION_DOWN,		"d",	"d [0-255] [0-255]",	pan_tilt_func,	"tilt down"},
	{ACTION_STOP,		"s",	"s",			pan_tilt_func,	"stop pantilt"},
	{ACTION_ZOOM_OUT,	"zo",	"zo [0-15]",		camera_func,	"camera zoom out"},
	{ACTION_ZOOM_IN,	"zi",	"zi [0-15]",		camera_func,	"camera zoom in"},
	{ACTION_FOCUS_NEAR,	"fn",	"fn",			camera_func,	"camera focus near"},
	{ACTION_FOCUS_FAR,	"ff",	"ff",			camera_func,	"camera focus far"},
	{ACTION_IRIS_OPEN,	"io",	"io",			camera_func,	"camera iris open"},
	{ACTION_IRIS_CLOSE,	"ic",	"ic",			camera_func,	"camera iris close"},
	{ACTION_STOP_CAMERA,	"sc",	"sc",			camera_func,	"stop camera\n"},
	{0,0,0,0,0}
};
#endif

struct TDVSSS_command standard_cmd[] = {
	//1. keep watch
	{ACTION_SET_KEEP_WATCH,		"set_kw",	"set_kw 1-255",		keep_watch_func,
			"set keep watch time (unit: second)"},
	{ACTION_ENABLE_KEEP_WATCH,	"enable_kw",	"enable_kw",		keep_watch_func,
			"enable the function of keep watch"},
	{ACTION_DISABLE_KEEP_WATCH,	"disable_kw",	"disable_kw",		keep_watch_func,
			"disable the function of keep watch\n"},
	//2. preset point
	{ACTION_SET_PRESET_POINT,	"set_pp",	"set_pp 1-80",		preset_point_func,
			"set preset point"},
	{ACTION_CALL_PRESET_POINT,	"call_pp",	"call_pp 1-80",		preset_point_func,
			"call preset point\n"},
	//3. cruise
	{ACTION_ADD_CRUISE_POINT,	"add_cp",	"add_cp 0-7 0-80",	cruise_func,
			"add cruise point"},
	{ACTION_SET_CRUISE_SPEED,	"set_cs",	"set_cs 3-255",		cruise_func,
			"set cruise speed"},
	{ACTION_SET_CRUISE_STAY_TIME,	"set_cst",	"set_cst 3-255",	cruise_func,
			"set cruise stay time(second) in one point"},
	{ACTION_START_CRUISE,		"start_c",	"start_c 0-7",		cruise_func,
			"start cruise"},
	{ACTION_DEL_CRUISE_POINT,	"del_cp",	"del_cp 0-7 0-80",	cruise_func,
			"delete cruise point 0-80 in term 0-7"},
	{ACTION_DEL_ALL_CRUISE_POINT,	"del_all_cp",	"del_all_cp 0-7",	cruise_func,
			"delete all cruise point\n"},
	//4. scan
	{ACTION_SET_LINE_SCAN_BORDER, 	"set_lsb",	"set_lsb 81-90",	scan_func,
			"set line scan left/right border,term 1(81:left,82:right);term 2(83: left,84: right) ... total 5 term"},
	{ACTION_SET_LINE_SCAN_SPEED, 	"set_lss",	"set_lss 3-255 1-5",	scan_func,
			"set line scan speed(3-255) of term 1-5"},
	{ACTION_START_AUTO_SCAN, 	"start_as",	"start_as 1-5",		scan_func,
			"start auto scan"},
	{ACTION_START_RANDOM_SCAN, 	"start_rs",	"start_rs",		scan_func,
			"start random scan"},
	{ACTION_SET_VARIETY_SCAN_SPEED,	"set_vss",	"set_vss 3-255",	scan_func,
			"set variety scan speed"},
	{ACTION_START_VARIETY_SCAN,	"start_vs",	"start_vs 0-100",	scan_func,
			"start variety scan\n" },
	//5. pattern
	{ACTION_SET_PATTERN_START,	"set_ps",	"set_ps 0-4",		pattern_func,
			"start pattern set"},
	{ACTION_SET_PATTERN_END,	"set_pe",	"set_pe 0-4",		pattern_func,
			"end pattern set"},
	{ACTION_START_PATTERN,		"start_p",	"start_p 0-4",		pattern_func,
			"start pattern\n"},
	
	//6. whirl
	{ACTION_SET_WHIRL_SPEED,	"set_ws",	"set_ws 3-255",		whirl_func,
			"set whirl speed"},
	{ACTION_START_WHIRL,		"start_w",	"start_w 0-1",		whirl_func,
			"start whirl\n"},
	//7. stop scan,sruise or pattern
	{ACTION_STOP_FUNCTION,		"stop_func",	"stop_func",		stop_func,
			"stop scan,cruise or pattern\n"},
	//8. assist func
	{ACTION_ASSIST_SWITCH_ON,	"assist_on",	"assist_on 0-1 1-44",	assist_switch_func,
			"assist switch on"},
	{ACTION_ASSIST_SWITCH_OFF,	"assist_off",	"assist_off 0-1 1-44",	assist_switch_func,
			"assist switch off\n"},
	//9. alarm
	{ACTION_ALARM_SET,		"alarm_set",	"alarm_set 1-15 0-4095",alarm_func,
			"alarm set"},	
	{ACTION_ALARM,			"alarm",	"alarm 0-4095",		alarm_func,
			"alarm"},
	{ACTION_ALARM_RESET,		"alarm_reset",	"alarm_reset",		alarm_func,
			"alarm reset\n"},
	//status func
/*
	{ACTION_STATUS_QUERY,		"status_query",	"status_query",		status_func,
			"status query function"},
	{ACTION_STATUS_RESPONSE,	"status_response","status_response",	status_func,
			"status response function\n"},
*/
	//privacy cover func	

	{ACTION_PRIVACY_SET_START,	"privacy_set_start",	"privacy_set_start 0-31",	privacy_cover_func,
			"start set privacy cover function"},
	{ACTION_PRIVACY_OK_ENABLE,	"privacy_ok_enable",	"privacy_ok_enable 0-31",	privacy_cover_func,
			"privacy cover set ok and enable the function"},
	{ACTION_PRIVACY_DISABLE,	"privacy_disable",	"privacy_disable 0-31",		privacy_cover_func,
			"disable the privacy cover function"},
	{ACTION_PRIVACY_SET_COBOUNDARY,	"privacy_coboundary",	"privacy_coboundary 0-31 0-255",	privacy_cover_func,
			"设置上边界位置"},
	{ACTION_PRIVACY_SET_LOWERBOUND,	"privacy_lowerbound",	"privacy_lowerbound 0-31 0-255",	privacy_cover_func,
			"设置下边界位置"},
	{ACTION_PRIVACY_SET_LEFTMARGIN,	"privacy_leftmargin",	"privacy_leftmargin 0-31 0-255",	privacy_cover_func,
			"设置左边界位置"},
	{ACTION_PRIVACY_SET_RIGHTMARGIN,"privacy_rightmargin",	"privacy_rightmargin 0-31 0-255",	privacy_cover_func,
			"设置右边界位置"},
	{ACTION_PRIVACY_COARSEN,	"privacy_coarsen",	"privacy_coarsen 0-31",		privacy_cover_func,
			"高度上下各增加一个单位"},
	{ACTION_PRIVACY_ATTENUAT,	"privacy_attenuat",	"privacy_attenuat 0-31",	privacy_cover_func,
			"高度上下各减少一个单位"},
	{ACTION_PRIVACY_WIDEN,		"privacy_widen",	"privacy_widen 0-31",		privacy_cover_func,
			"宽度左右各增加一个单位"},
	{ACTION_PRIVACY_NARROW,		"privacy_narrow",	"privacy_narrow 0-31",		privacy_cover_func,
			"宽度左右各减少一个单位"},
	{ACTION_PRIVACY_MOVE_UP,	"privacy_move_up",	"privacy_move_up 0-31",		privacy_cover_func,
			"调整位置,上移一个单位"},
	{ACTION_PRIVACY_MOVE_DOWN,	"privacy_move_down",	"privacy_move_down 0-31",	privacy_cover_func,
			"调整位置,下移一个单位"},
	{ACTION_PRIVACY_MOVE_LEFT,	"privacy_move_left",	"privacy_move_left 0-31",	privacy_cover_func,
			"调整位置,左移一个单位"},
	{ACTION_PRIVACY_MOVE_RIGHT,	"privacy_move_right",	"privacy_move_right 0-31",	privacy_cover_func,
			"调整位置,右移一个单位\n"},

	//camera menu
	{ACTION_OPEN_MENU,		"open_m",	"open_m",		camera_menu_func,
			"open menu"},
	{ACTION_CLOSE_MENU,		"close_m",	"close_m",		camera_menu_func,
			"close menu"},
	{ACTION_FATHER_MENU,		"father_m",	"father_m",		camera_menu_func,
			"entry father menu"},
	{ACTION_CHILD_MENU,		"child_m",	"child_m",		camera_menu_func,
			"entry child menu"},
	{ACTION_UPWORD_MENU,		"upword_m",	"upword_m",		camera_menu_func,
			"upword"},
	{ACTION_DOWNWORD_MENU,		"downword_m",	"downword_m",		camera_menu_func,
			"downword"},
	{ACTION_LEFTWORD_MENU,		"leftword_m",	"leftword_m",		camera_menu_func,
			"leftword"},
	{ACTION_RIGHTWORD_MENU,		"rightword_m",	"rightword_m",		camera_menu_func,
			"rightword"},
	{ACTION_OK_MENU,		"ok_m",		"ok_m",			camera_menu_func,
			"ok\n"},
	//char set
	{ACTION_SET_CHAR_POSITION,	"set_charp",	"set_charp 0-255 0-255",char_func,
			"set char position"},
	{ACTION_SET_CHAR,		"set_char",	"set_char char",	char_func,
			"set char\n"},
	//other
	{ACTION_RESET,			"reset",	"reset",		other_func,
			"reset"},
	{ACTION_ORIGINAL_SET,		"original",	"original",		other_func,
			"original set"},
	{ACTION_MIN_FOCUS_DIST,		"min_focus",	"min_focus 0-65535",	other_func,
			"set minimum focus distance\n"},
	{0,0,0,0,0}
};

int standard_protocol(struct TDVSSS_protocol* tp)
{
	g_tp = tp;
	return 0;
}

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_len = 7;
	tp->out[0] = 0xa5;
	tp->out[1] = tp->address;
	tp->out[5] = tp->source_addr;
	tp->out[6] = checksum(tp,0,5);

	return;
}

int stop_func(struct TDVSSS_protocol* tp)
{
	tp->continuous = 0;
	tp->out[2] = 0x83;
	tp->out[3] = 0;
	tp->out[4] = 90;
	public_out(tp);
	return 0;
}

int pan_tilt_func(struct TDVSSS_protocol* tp)
{
	int ret;
	unsigned char code = tp->out[2];	

	tp->continuous = 1;
	if((code & CAMERA_COMMAND) || (code & FUNCTION_COMMAND))
		tp->out[2] = 0;

	switch (tp->user_input.cmd_id)
	{
	case ACTION_LEFT:
		tp->out[2] |= MASK_LEFT;
		tp->out[2] &= ~MASK_RIGHT;
		break;		
	case ACTION_RIGHT:
		tp->out[2] |= MASK_RIGHT;
		tp->out[2] &= ~MASK_LEFT;
		break;
	case ACTION_UP:
		tp->out[2] |= MASK_UP;
		tp->out[2] &= ~MASK_DOWN;
		break;
	case ACTION_DOWN:
		tp->out[2] |= MASK_DOWN;
		tp->out[2] &= ~MASK_UP;
		break;
	case ACTION_STOP:
		tp->out[2] = COMMAND_STOP ;
		tp->continuous = 0;
		break;
	default:
		tp->continuous = 0;
		return PARSE_CMD_ERROR;
	}

	if(tp->user_input.param1[0])
	{
		if((tp->user_input.param1[1]) <= 255)
			tp->pan_speed = tp->user_input.param1[1];
		else
		{
			tp->continuous = 0;
			return INVALID_ARGUMENT;
		}
	}
	if(tp->user_input.param2[0])
	{
		if((tp->user_input.param2[1]) <= 255)
			tp->tilt_speed = tp->user_input.param2[1];
		else
		{
			tp->continuous = 0;
			return INVALID_ARGUMENT;
		}
	}
	tp->out[3] = tp->pan_speed;
	tp->out[4] = tp->tilt_speed;
	if(tp->out[3] == 0 && tp->out[4] == 0)
		tp->continuous = 0;

	public_out(tp);

	return ret;
}

int camera_func(struct TDVSSS_protocol* tp)
{
	int ret;
	unsigned char code = tp->out[2];

	if(!(code & CAMERA_COMMAND))
		tp->out[2] = 0;

	tp->continuous = 0;
	tp->out[3] = 0x00;
	tp->out[4] = 0x00;

	switch (tp->user_input.cmd_id)
	{
	case ACTION_ZOOM_OUT:
		tp->out[2] &= ~MASK_ZOOM_IN;
		tp->out[2] |= MASK_ZOOM_OUT;
		if(tp->user_input.param1[0])
		{		
			if(IS_BETWEEN(tp->user_input.param1[1],1,15))
			{
				tp->out[4] = tp->user_input.param1[1];
				tp->continuous = 1;
			}
			else
				return INVALID_ARGUMENT;
		}
		break;		
	case ACTION_ZOOM_IN:
		tp->out[2] &= ~MASK_ZOOM_OUT;
		tp->out[2] |= MASK_ZOOM_IN;
		if(tp->user_input.param1[0])
		{		
			if(IS_BETWEEN(tp->user_input.param1[1],1,15))
			{
				tp->out[4] = tp->user_input.param1[1];
				tp->continuous = 1;
			}
			else
				return INVALID_ARGUMENT;
		}
		break;
	case ACTION_FOCUS_NEAR:
		tp->out[2] &= ~MASK_FOCUS_FAR;
		tp->out[2] |= MASK_FOCUS_NEAR;
		break;
	case ACTION_FOCUS_FAR:
		tp->out[2] &= ~MASK_FOCUS_NEAR;
		tp->out[2] |= MASK_FOCUS_FAR;
		break;
	case ACTION_IRIS_OPEN:
		tp->out[2] &= ~MASK_IRIS_CLOSE;
		tp->out[2] |= MASK_IRIS_OPEN;
		break;
	case ACTION_IRIS_CLOSE:
		tp->out[2] &= ~MASK_IRIS_OPEN;
		tp->out[2] |= MASK_IRIS_CLOSE;
		break;
	case ACTION_STOP_CAMERA:
		tp->out[2] = COMMAND_STOP_CAMERA;
		break;
	default:
		return PARSE_CMD_ERROR;
	}
	public_out(tp);

	return ret;
}

//1. keep watch
int keep_watch_func(struct TDVSSS_protocol* tp)
{
	unsigned short buf = tp->user_input.param1[1];

	tp->continuous = 0;
	switch (tp->user_input.cmd_id)
	{
	case ACTION_SET_KEEP_WATCH:
		tp->out[2] = 0x81;
		tp->out[4] = 0;
		if((tp->user_input.param1[0]) && IS_BETWEEN(buf,1,255))
			tp->out[3] = buf;
		else
			return INVALID_ARGUMENT;
		break;
	case ACTION_ENABLE_KEEP_WATCH:
		tp->out[2] = 0x83;
		tp->out[3] = tp->out[4] = 0;
		break;
	case ACTION_DISABLE_KEEP_WATCH:
		tp->out[2] = 0x81;
		tp->out[3] = tp->out[4] = 0;
		break;
	}
	public_out(tp);
	return 0;
}

//2. preset point
int preset_point_func(struct TDVSSS_protocol* tp)
{
	unsigned short buf;

	if(!(tp->user_input.param1[0]))
		return INVALID_ARGUMENT;

	tp->continuous = 0;
	switch (tp->user_input.cmd_id)
	{
	case ACTION_SET_PRESET_POINT:
		tp->out[2] = 0x81;
		break;
	case ACTION_CALL_PRESET_POINT:
		tp->out[2] = 0x83;
		break;
	}
	tp->out[3] = 0;
	buf = tp->user_input.param1[1];
	if(IS_BETWEEN(buf,1,80))
		tp->out[4] = buf;
	else
		return INVALID_ARGUMENT;

	public_out(tp);
	return 0;
}

//3. cruise
int cruise_func(struct TDVSSS_protocol* tp)
{
	unsigned short buf,buf2;
	
	if(!(tp->user_input.param1[0]))
		return INVALID_ARGUMENT;
	
	buf = tp->user_input.param1[1];
	buf2 = tp->user_input.param2[1];

	tp->continuous = 0;
	switch (tp->user_input.cmd_id)
	{
	case ACTION_ADD_CRUISE_POINT:
		tp->out[2] = 0x84;
		if(buf <= 7)
			tp->out[3] = buf;
		else
			return INVALID_ARGUMENT;
		if((tp->user_input.param2[0]) && (buf2 <= 80))
			tp->out[4] = buf2;
		else
			return INVALID_ARGUMENT;
		break;
	case ACTION_SET_CRUISE_SPEED:
		tp->out[2] = 0x81;
		tp->out[4] = 99;
		if(IS_BETWEEN(buf,3,255))
			tp->out[3] = buf;
		else
			return INVALID_ARGUMENT;
		break;
	case ACTION_SET_CRUISE_STAY_TIME:
		tp->out[2] = 0x81;
		tp->out[4] = 96;
		if(IS_BETWEEN(buf,3,255))
			tp->out[3] = buf;
		else
			return INVALID_ARGUMENT;
		break;
	case ACTION_START_CRUISE:
		tp->out[2] = 0x83;
		if(buf <= 7)