protocol.c

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

#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include "protocol.h"
#include "device.h"

#ifdef AUTO_INPUT
#include "auto_input.h"
#endif

//#define STANDARD_PROTOCOL
#ifdef STANDARD_PROTOCOL
#include "standard.h"
#endif

//#define STANDARD_I2C_PROTOCOL
#ifdef  STANDARD_I2C_PROTOCOL
#include "standard_i2c.h"
#endif

#ifdef YAAN_PROTOCOL
#include "yaan.h"
#endif

#ifdef PELCO_D_PROTOCOL
#include "pelco_d.h"
#endif

#ifdef PELCO_P_PROTOCOL
#include "pelco_p.h"
#endif


struct TDVSSS_entry protocol_entry[] = {
#ifdef STANDARD_PROTOCOL
	{"standard",	standard_protocol,	pantilt_cmd,	standard_cmd},
#endif

#ifdef STANDARD_I2C_PROTOCOL
	{"standard-i2c",standard_i2c_protocol,	pantilt_cmd_i2c,	standard_cmd_i2c },
#endif

#ifdef YAAN_PROTOCOL
	{"yaan",	yaan_protocol,		pantilt_cmd_yaan,	yaan_cmd},
#endif

#ifdef PELCO_D_PROTOCOL
	{"pelco_d",	pelco_d_protocol,	pantilt_cmd_pd,		pelco_d_cmd},
#endif

#ifdef PELCO_P_PROTOCOL
	{"pelco_p",	pelco_p_protocol,	pantilt_cmd_pp,		pelco_p_cmd},
#endif
	{0,0,0,0}
};

static struct TDVSSS_protocol* g_tp = NULL;

static int send_command(int fd,unsigned char* pCmd,int nSize,int response);

static int is_cmd_too_long(struct TDVSSS_command* tc)
{
	int i;
	
	if(tc == NULL)
		return -1;	

	for(i = 0 ; tc[i].command; i++)
	{
		if(strlen(tc[i].command) >= MAX_COMMAND_SIZE)
		{
			fprintf(stderr,"Error: Command  %s is too long!\n",tc[i].command);
			return -1;
		}
	}
	
	return 0;
}

static int init_device(struct TDVSSS_device* td)
{
	int fd;
	if(td->type == DEVICE_TTYS)
	{
		if((fd = initial_serial(td->name,td->baudrate,8,'n',1)) < 0)
		{
			perror("initial serial");
			return -1;
		}
	}
	else if(td->type == DEVICE_I2C)
	{
		fprintf(stderr,"error: device i2c no implement!\n");
		return -1;
	}
	else
	{
		fprintf(stderr,"error: invalid device!\n");
		return -1;
	}
	return fd;
}

static void timer(int sig)
{
	if(g_tp == NULL)
		return;
	if(g_tp->continuous && g_tp->user_input.cmd_id)
	{
		send_command(g_tp->fd,g_tp->out,g_tp->out_len,g_tp->response);
	}
	return;
}	

static int settimer(int millisecond)
{
	int ret;
	struct itimerval value,ovalue;

	value.it_value.tv_sec = millisecond/1000;
	value.it_value.tv_usec = millisecond%1000;
	value.it_interval.tv_sec = millisecond/1000;
	value.it_interval.tv_usec = millisecond%1000;
	ret = setitimer(ITIMER_REAL,&value,&ovalue);	
	
	return ret;
}

static void  handle_command(int sig);

struct TDVSSS_protocol* protocol_init(char* protocol,int addr,struct TDVSSS_device* td)
{
	int j,entry = -1;
	struct TDVSSS_protocol* tp;

	if(!protocol)
		return NULL;
	for(j = 0;protocol_entry[j].protocol;j++)
	{
		if(!strcmp(protocol,protocol_entry[j].protocol))
		{
			entry = j;
			break;
			
		}
	}
	if(entry < 0)
	{
		fprintf(stderr,"protocol %s is not implement!\n",protocol);
		return NULL;
	}
	if(!IS_BETWEEN(addr,0,255))
	{
		fprintf(stderr,"address %d is invalid!\n",addr);
		return NULL;
	}
	if(!td)
		return NULL;
	
	//malloc memory
	if((tp = malloc(sizeof(struct TDVSSS_protocol))) == NULL)
	{
		perror("malloc memory");
		return NULL;
	}
	g_tp = tp;	
	//initial memory
	memset(tp,0,sizeof(struct TDVSSS_protocol));
	strcpy(tp->protocol, protocol_entry[entry].protocol);
	tp->protocol_start = protocol_entry[entry].protocol_start;
	tp->pantilt_cmd     = protocol_entry[entry].pantilt_cmd;
	tp->function_cmd    = protocol_entry[entry].function_cmd;
	if(is_cmd_too_long(tp->pantilt_cmd) < 0)
	{
		free(tp);
		return NULL;
	}
	if(is_cmd_too_long(tp->function_cmd) < 0)
	{
		free(tp);
		return NULL;
	}
	tp->address = addr;
	tp->source_addr = 0;
	tp->pan_speed = 0x90;
	tp->tilt_speed = 0x90;
	tp->continuous = 0;
	tp->transparent = 0;
	tp->response = 0;
	tp->millisecond = 1000;
	strcpy(tp->device.name,td->name);
	tp->device.type = td->type;
	tp->device.baudrate = td->baudrate;
	tp->device.parity = td->parity;
	if((tp->fd = init_device(&(tp->device))) < 0)
	{
		perror("init_device");
		free(tp);
		return NULL;
	}
	tp->protocol_start(tp);

	//set timer
	signal(SIGALRM,timer);
	settimer(tp->millisecond);	
	#ifdef FOR_HTTPD
	signal(SIGBGN,handle_command);
	#endif

/*
	#ifdef STANDARD_PROTOCOL
	signal(SIGRESP,response);	
	#endif
*/	

	return tp;	
}

static void print_command(struct TDVSSS_protocol* tp,int cmd_id)
{
	int i;
	struct TDVSSS_command* tc;
	
	if(!tp->pantilt_cmd)
		return;
	tc = tp->pantilt_cmd;

	if(cmd_id == 0)
	{
		fprintf(stderr,"Command List:\n"); 
		fprintf(stderr,"\t%-32s%-32s\t%s\n\n","command","call format","comment");
		#ifndef FOR_HTTPD
		fprintf(stderr,"\t%-32s%-32s\t%s\n","q","q","quit process\n");
		#endif
	}
	else if(cmd_id > 0)
		fprintf(stderr,"Command Usage:\n");

//	fprintf(stderr,"pan tilt command:\n");
	for(i = 0;tc[i].command; i++)
		if((cmd_id > 0)?(cmd_id == tc[i].cmd_id):1)
			fprintf(stderr,"%d\t%-32s%-32s\t%s\n",
				i,
				tc[i].command,
				tc[i].format?tc[i].format:"",
				tc[i].help?tc[i].help:""); 

	if(!tp->function_cmd)
		return;
	tc = tp->function_cmd;
//	fprintf(stderr,"functional command:\n");
	for(i = 0;tc[i].command; i++)
		if((cmd_id > 0)?(cmd_id == tc[i].cmd_id):1)
			fprintf(stderr,"%d\t%-32s%-32s\t%s\n",
				i,
				tc[i].command,
				tc[i].format?tc[i].format:"",
				tc[i].help?tc[i].help:""); 
	
	return;
}

static char* align_string(char* pTemp)
{
	int i = 0;
	while(pTemp[i] == ' ')
		i++;
	return pTemp+i;
}

static int is0_65535(char* num)
{
	int i,len;
	
	if(num == NULL)
		return -1;

	len = strlen(num);
	if(len == 0 || len > 5)
		return  -1;
	for(i = 0; i< len; i++)
	{
		if(num[i] < '0' || num[i] > '9')
			return -1;
	}
	len = atoi(num);
	if(len < 0 || len > 65535)
		return -1;
	return 0;
}

static int parse_input(char* input,struct TDVSSS_protocol* tp)
{
	char szAction[MAX_USER_INPUT_SIZE],szParam1[32],szParam2[32],*pPos;
	int i = 0;
	int match = NO_SUCH_COMMAND,p1=0,p2=0;
	struct TDVSSS_command* tc = NULL;
	
	memset(szAction,0,MAX_USER_INPUT_SIZE);
	memset(szParam1,0,32);
	memset(szParam2,0,32);
	pPos = align_string(input);
	while(pPos[i] != ' ' && pPos[i] != '\n')
	{
		szAction[i] = pPos[i];
		i++;
	}
	if( i == 0)
		return NO_SUCH_COMMAND;

	if(pPos[i] != '\n')
	{
		pPos = align_string(pPos+i);
		i = 0;
		while(pPos[i] != ' ' && pPos[i] != '\n')
		{
			szParam1[i] = pPos[i];
			i++;
		}
		if( i > 0)
			p1 = 1;
	}
	if(pPos[i] != '\n')
	{
		pPos = align_string(pPos+i);
		i = 0;
		while(pPos[i] != ' ' && pPos[i] != '\n')
		{
			szParam2[i] = pPos[i];
			i++;
		}
		if(i > 0)
			p2 = 1;
	}
	
	tc = tp->pantilt_cmd;
	for(i = 0;tc[i].command; i++)
	{
		if(!strcmp(szAction,tc[i].command))
		{
			strcpy(tp->user_input.command,tc[i].command);
			tp->user_input.cmd_id = tc[i].cmd_id;
			tp->user_input.cmd_func = tc[i].cmd_func;
			match = 1;
			break;
		}
	}	
	if(match < 0)
	{
		tc = tp->function_cmd;
		for(i = 0;tc[i].command; i++)
		{
			if(!strcmp(szAction,tc[i].command))
			{
				#ifdef STANDARD_I2C_PROTOCOL
				if(!strcmp(szAction,"transparent"))
					return NO_SUCH_COMMAND;
				#endif
				strcpy(tp->user_input.command,tc[i].command);
				tp->user_input.cmd_id = tc[i].cmd_id;
				tp->user_input.cmd_func = tc[i].cmd_func;
				match = 1;
				break;
			}
		}
	}
	if(match < 0)
		return NO_SUCH_COMMAND;
	
	if(p1 == 1)
	{
		if(!is0_65535(szParam1))
		{
			tp->user_input.param1[0] = 1;
			tp->user_input.param1[1] = atoi(szParam1);
		}
		#if (defined STANDARD_PROTOCOL) || (defined STANDARD_I2C_PROTOCOL)
		else if(tp->user_input.cmd_id == ACTION_SET_CHAR)
		{
			tp->user_input.param1[0] = 1;
			tp->user_input.param1[1] = ((unsigned char)szParam1[0] << 8) + (unsigned char)szParam1[1];
		}
		#endif
		else
			return INVALID_ARGUMENT;
	}
	else
		tp->user_input.param1[0] = 0;
		
	if(p2 == 1)
	{
		if(!is0_65535(szParam2))
		{
			tp->user_input.param2[0] = 1;
			tp->user_input.param2[1] = atoi(szParam2);
		}
		else
			return INVALID_ARGUMENT;
	}
	else
		tp->user_input.param2[0] = 0;

	return 0;
}

static int handle_input(struct TDVSSS_protocol* tp)
{
	char* input = NULL;
	int ret;

	input = tp->user_input.input;
	if((ret = parse_input(input,tp)) < 0)
	{
		if(ret == NO_SUCH_COMMAND)
		{
			fprintf(stderr,"error:No Such Command!\n");
			print_command(tp,0);
		}
		else if(ret == INVALID_ARGUMENT)
		{
			fprintf(stderr,"error:Invalid Argument!\n");
			print_command(tp,tp->user_input.cmd_id);
		}
		return ret;
	}
	if((ret = tp->user_input.cmd_func(tp)) < 0)
	{
		if(ret == NO_SUCH_COMMAND)
		{
			fprintf(stderr,"error:No Such Command!\n");
			print_command(tp,0);
		}
		else if(ret == INVALID_ARGUMENT)
		{
			fprintf(stderr,"error:Invalid Argument!\n");
			print_command(tp,tp->user_input.cmd_id);
		}
		return ret;
	}

	return 0;
}
/*
static int get_index_from_string(char* command,struct TDVSSS_command* list,int* cmd_id)
{
	int index;
	
	for(index = 0;list[index].command;index++)
	{
		if(!strcmp(command,list[index].command))
		{
			*cmd_id = list[index].cmd_id;
			return index;
		}
	}
	return -1;
}
*/
static int handle_transparent(struct TDVSSS_protocol* tp)
{
	#ifdef STANDARD_I2C_PROTOCOL
	int trans_cmd[] = {23,24,25,26,27,28,29,30},count = sizeof(trans_cmd)/sizeof(int);
	char szCmd[MAX_COMMAND_SIZE], *pPos;
	int i = 0,match = -1,ret;
	struct TDVSSS_command* tc;
	
	memset(szCmd,0,MAX_COMMAND_SIZE);
	pPos = align_string(tp->user_input.input);
	while(pPos[i] != ' ' && pPos[i] != '\n')
	{
		szCmd[i] = pPos[i];
		i++;
	}
	if(i == 0)
		return NO_SUCH_COMMAND;
	if(pPos[i] != '\n')
	{
		pPos = align_string(pPos+i);
	}
	for(i=0;i < count;i++)
	{
		tc = &(tp->function_cmd[trans_cmd[i]]);
		if(!strcmp(szCmd,tc->command))
		{
			strcpy(tp->user_input.command,tc->command);
			tp->user_input.cmd_id = tc->cmd_id;
			tp->user_input.cmd_func = tc->cmd_func;
			match = 1;
		}
	}
	if(match < 0)
	{
		fprintf(stderr,"error:No Such Command!\n");
		for(i = 0;i< count;i++)
		{
			print_command(tp,tp->function_cmd[trans_cmd[i]].cmd_id);
		}
		return -1;
	}
	tp->user_input.param = pPos;
	fprintf(stderr,"transparent...... \n");
	if((ret = tp->user_input.cmd_func(tp)) < 0)
	{
		if(ret == NO_SUCH_COMMAND)
		{
			fprintf(stderr,"error:No Such Command!\n");
//			print_command(tp,0);
		}
		else if(ret == INVALID_ARGUMENT)
		{
			fprintf(stderr,"error:Invalid Argument!\n");
			print_command(tp,tp->user_input.cmd_id);
		}
		return ret;
	}
	return 0;
	#else
	return -1;
	#endif
}
static int send_command(int fd,unsigned char* pCmd,int nSize,int response)
{
	int n;

	n = write(fd,pCmd,nSize);
	if(n < nSize)
	{
		fprintf(stderr,"write data failed\n");
		return -1;
	}
	if(response)
	{
		raise(SIGRESP);
	}
	return 0;
}

int command_loop(struct TDVSSS_protocol* tp)
{
	char* input;
	int j,n;

	input = tp->user_input.input;
	memset(input,0,MAX_USER_INPUT_SIZE);
	#ifndef	AUTO_INPUT 
	printf("(cmd)");  
	fgets(input,MAX_USER_INPUT_SIZE,stdin); 
	#else
	sleepn(20);  
	auto_input_cmd(input); 
	#endif

	while(strcmp(input,"q\n"))
	{
		if(tp->transparent == 0)
		{
			if(handle_input(tp) < 0)
				goto next_loop;
		}
		else
		{
			if(handle_transparent(g_tp) < 0)
				goto next_loop;
		}
		#ifndef AUTO_INPUT
		printf("Protocol: %s\tCommand: id:%d code:%s\t length:%d hexdump:",tp->protocol,
				tp->user_input.cmd_id,tp->user_input.command,tp->out_len);
		n = tp->out_len;
		for(j=0;j < n;j++)
			printf("%02x ",tp->out[j]);
		printf("\n");
		#endif
		send_command(tp->fd,tp->out,tp->out_len,tp->response);
		//entry next loop to receive input;
		next_loop:
		memset(input,0,MAX_USER_INPUT_SIZE);
		#ifndef AUTO_INPUT
		printf("(cmd)"); 
		fgets(input,MAX_USER_INPUT_SIZE,stdin);
		#else
		sleepn(10);  
		auto_input_cmd(input); 
		#endif
	}
	return 0;
}

static void  handle_command(int sig)
{
	if(sig != SIGBGN)
		return;
	if(g_tp ==NULL)
		return;
	if(g_tp->transparent == 0)
	{
		if(handle_input(g_tp) < 0)
			return;
	}
	else
	{
		if(handle_transparent(g_tp) < 0)
			return;
	}
	send_command(g_tp->fd,g_tp->out,g_tp->out_len,g_tp->response);
	return;
}
//called in httpd
int handle_control(char* control)
{
	if(!control)
		return -1;
	if(g_tp == NULL)
	{
		fprintf(stderr,"error: youhave not initial protocol!\n");
		return -1;
	}

	strcpy(g_tp->user_input.input,control);
	raise(SIGBGN);
	return 0;
}