pelco_d.c

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

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

#include "protocol.h"
#include "pelco_d.h"

struct TDVSSS_command pelco_d_cmd[] = {
	{ACTION_AUTO_FOCUS,	"af",	"",	pd_cmd_extended},   //自动聚焦   
	{ACTION_AUTO_IRIS,	"ai",	"",	pd_cmd_extended},   //自动光圈  
	{ACTION_CAMERA_ON,	"con",	"",	camera_func}, 
	{ACTION_CAMERA_OFF,	"coff",	"",	camera_func},
	{ACTION_AUTO_SCAN_ON,	"aso",	"",	camera_func},
	{ACTION_SENSE,		"sense","",	camera_func},
	{ACTION_SET_PRESET,	"sp",	"",	pd_cmd_extended},
	{ACTION_CLEAR_PRESET,	"cp",	"",     pd_cmd_extended},
	{ACTION_GOTO_PRESET,	"go",	"",     pd_cmd_extended},
	{ACTION_REMOTE_RESET,	"rr",	"",     pd_cmd_extended},
	{ACTION_ZONE_START,	"zs",	"",     pd_cmd_extended},
	{ACTION_ZONE_END,	"ze",	"",     pd_cmd_extended},
	{ACTION_ZONE_SCAN_ON,	"zon",	"",     pd_cmd_extended},
	{ACTION_ZONE_SCAN_OFF,	"zoff",	"",     pd_cmd_extended},
	{ACTION_PATTERN_START,	"pst",	"",     pd_cmd_extended},
	{ACTION_PATTERN_STOP,	"psp",	"",     pd_cmd_extended},
	{ACTION_RUN_PATTERN,	"rp",	"",     pd_cmd_extended},
	{ACTION_BEGIN_AUTO_SCAN,"as",	"",     pd_cmd_extended},
	{ACTION_STOP_SCAN,	"ss",	"",     pd_cmd_extended},
	{0,0,0,0,0}
};

int pelco_d_protocol(struct TDVSSS_protocol* tp)
{
	return 0;
}

//The implement of pelco_d 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_len = 7;
	tp->out[0] = 0xff;
	tp->out[1] = tp->address;

}

int pan_tilt_func(struct TDVSSS_protocol* tp)
{
	if(tp->address > 0x1f)
		return INVALID_ADDRESS;
	public_out(tp);
	tp->out[4] = tp->pan_speed;
	tp->out[5] = tp->tilt_speed;
	switch (tp->user_input.cmd_id)
	{
	case ACTION_LEFT:
		tp->out[2] = 0x00;
		tp->out[3] = 0x04;
		break;
	case ACTION_RIGHT:
		tp->out[2] = 0x00;
		tp->out[3] = 0x02;
		break;
	case ACTION_UP:
		tp->out[2] = 0x00;
		tp->out[3] = 0x08;
		break;
	case ACTION_DOWN:
		tp->out[2] = 0x00;
		tp->out[3] = 0x10;
		break;
	case ACTION_STOP:
		tp->out[2] = 0x00; 
		tp->out[3] = 0x00;
		tp->out[4] = 0x00;
		tp->out[5] = 0x00;
		break;
	}
	tp->out[6] = checksum(tp,1,5);
	return 0;
}

int camera_func(struct TDVSSS_protocol* tp)
{
	if(tp->address > 0x1f)
		return INVALID_ADDRESS;
	public_out(tp);

	switch (tp->user_input.cmd_id)
	{
	case ACTION_ZOOM_OUT:
		tp->out[2] = 0x00;
		tp->out[3] = 0x40;
		break;		
	case ACTION_ZOOM_IN:
		tp->out[2] = 0x00;
		tp->out[3] = 0x20;
		break;
	case ACTION_FOCUS_NEAR:
		tp->out[2] = 0x01;
		tp->out[3] = 0x00;
		break;
	case ACTION_FOCUS_FAR:
		tp->out[2] = 0x00;
		tp->out[3] = 0x80;
		break;	
	case ACTION_IRIS_OPEN:
		tp->out[2] = 0x02;
		tp->out[3] = 0x00;
		break;	
	case ACTION_IRIS_CLOSE:
		tp->out[2] = 0x04;
		tp->out[3] = 0x00;
		break;	
	case ACTION_AUTO_SCAN_ON:         //  
		tp->out[2] = 0x10;
		tp->out[3] = 0x00;
		break;	
	case ACTION_CAMERA_ON:         
		tp->out[2] = 0x88;
		tp->out[3] = 0x00;
		break;	
	case ACTION_CAMERA_OFF:        
		tp->out[2] = 0x08;
		tp->out[3] = 0x00;
		break;	
	case ACTION_SENSE:
		tp->out[2] = 0x98;
		tp->out[3] = 0x00;
		break;
	default:
		return NO_SUCH_COMMAND;
	}
	tp->out[4] = tp->pan_speed;
	tp->out[5] = tp->tilt_speed;
	tp->out[6] = checksum(tp,1,5);
	return 0;

}

int  pd_cmd_extended(struct TDVSSS_protocol* tp)
{
	if(tp->address > 0x1f)
		return INVALID_ADDRESS;
	public_out(tp);
	tp->out[2] = 0x00; 
	tp->out[4] = 0x00;
	
	switch (tp->user_input.cmd_id)
	{
	case ACTION_SET_PRESET:
		if(tp->user_input.param1[1] < 1 || tp->user_input.param1[1] > 0x20)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x03;
		tp->out[5] = tp->user_input.param1[1];
		break;
	case ACTION_CLEAR_PRESET:           
		if(tp->user_input.param1[1] < 1 || tp->user_input.param1[1] > 0x20)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x05;
		tp->out[5] = tp->user_input.param1[1];
		break;
	case ACTION_GOTO_PRESET:
		if(tp->user_input.param1[1] < 1 || tp->user_input.param1[1] > 0x20)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x07;
		tp->out[5] = tp->user_input.param1[1];
		break;
	case ACTION_REMOTE_RESET:
		tp->out[3] = 0x0f;
		tp->out[5] = 0x00;
		break;
	case ACTION_ZONE_START:
		if(tp->user_input.param1[1] < 1 || tp->user_input.param1[1] > 8)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x11;
		tp->out[5] = tp->user_input.param1[1];
		break;
	case ACTION_ZONE_END:
		if(tp->user_input.param1[1] < 1 || tp->user_input.param1[1] > 8)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x13;
		tp->out[5] = tp->user_input.param1[1];
		break;
	case ACTION_ZONE_SCAN_ON:
		tp->out[3] = 0x1b;
		tp->out[5] = 0x00;
		break;
	case ACTION_ZONE_SCAN_OFF:
		tp->out[3] = 0x1d;
		tp->out[5] = 0x00;
		break;
	case ACTION_PATTERN_START:
		tp->out[3] = 0x1f;
		tp->out[5] = 0x00;
		break;
	case ACTION_PATTERN_STOP:
		tp->out[3] = 0x21;
		tp->out[5] = 0x00;
		break;
	case ACTION_RUN_PATTERN:
		tp->out[3] = 0x23;
		tp->out[5] = 0x00;
		break;
	case ACTION_BEGIN_AUTO_SCAN:
		tp->out[3] = 0x07;
		tp->out[5] = 0x63;
		break;
	case ACTION_STOP_SCAN:
		tp->out[3] = 0x07;
		tp->out[5] = 0x60;
		break;		
	case ACTION_AUTO_FOCUS:
		if(tp->user_input.param1[1] > 2)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x2b;
		tp->out[5] = tp->user_input.param1[1]; //0: auto 1:on 2:off
		break;
	case ACTION_AUTO_IRIS:	
		if(tp->user_input.param1[1] > 2)
			return INVALID_ARGUMENT;
		tp->out[3] = 0x2d;
		tp->out[5] = tp->user_input.param1[1]; //0:auto 1:on 2:off
		break;
	}	
	tp->out[6] = checksum(tp,1,5);
	return SUCCESSFULLY;
}