基于PELCO-D协议对云台进行定点控制

项DIY一个卫星自动追踪系统，其中天线要使用云台带动控制。经过搜寻网上的资料发现PTZ-301云台(或者PTZ-303Z)应用最为广泛，但是如何控制成为了难点，经过搜寻资料，查询到该云台使用PELCO-D协议和PELCO-P协议。

这个协议开始网上找了一圈，发现下载完整版的协议大多是还都要收费，后面在官方下载了一份原版完整版协议的协议内容，配合网上搜索到的资料完成对手上这块云台的配置。虽然原版的协议支持很多功能，包括摄像头的一些控制，这当然与具体的产品有关，手上的这款只是对云台的控制，包括复位，预置设置，调用和删除等一些基础的功能。

由于最终是要结合手上的串口摄像机，完成定时定点进行拍照的功能，所以这里先写云台的功能函数，下面写摄像头模块时再一起结合。

先来简单了解一下协议本身。

PELCO-D:

数据格式： 1 位起始位、 8 位数据、 1 位停止位，无效验位。波特率： 9600B/S（根据本身改变）；
命令格式：

字节 1

字节1	字节 2	字节 3	字节 4	字节 5	字节 6	字节 7
同步字节	地址码	指令码 1	指令码 2	数据码 1	数据码 2	校验码

说明

1.该协议中所有数值都为十六进制数；

2.同步字节始终为 FFH ；

3.地址码为摄像机的逻辑地址号，地址范围： 00H –FFH ；

4.指令码表示不同的动作；

5.数据码 1、2 分别表示水平、垂直方向速度（ 00-3FH ）,FFH 表示 “turbo ”速度；

6.校验码 = MOD[ （字节 2 + 字节 3 + 字节 4 + 字节 5 + 字节 6）/100H]。

指令码格式：

	Bit 7	Bit 6	Bit 5	Bit 4	Bit 3	Bit 2	Bit 1	Bit 0
命令字 1	Sence码	0	0	自动 /手动扫描	摄像机打开/关闭	光圈关闭	光圈打开	焦距拉近
命令字 2	焦距拉远	视角变宽	视角变窄	上	下	左	右	0

说明

Sence 码：与 Bit4 和 Bit3 有关。在 Bit4 和 Bit3 为 1 的情况下，如果 Sence 码为 1，则命令就是自动扫描和和摄像机打开；如果 Sence 码为 0，则命令就是手动扫描和摄像机关闭。当然如果Bit4 或 Bit3 为 0 的话那命令就无效了；

数据 1：表示镜头左右平移的速度，数值从 $00( 停止 )到$3F( 高速 )，另外还有一个值是 $FF ，表示最高速；

数据 2：表示镜头上下移动的速度，数值从 $00( 停止 )到$3F( 最高速 )；

校验码：指 Byte2 到 Byte6 这 5 个数的和 (若超过 255 则除以 256 然后取余数 )。

以上就是PELCO-D协议的大致内容，如果是PELCO-P协议可以参考文末的文档做出相应的修改即可。

下面列出几组参考用例
以地址码 0x01 为例：
{0xff,0x01,0x00,0x08,0x00,0xff,0x08,}// 上
{0xff,0x01,0x00,0x10,0x00,0xff,0x10,}// 下
{0xff,0x01,0x00,0x04,0xff,0x00,0x04,}// 左
{0xff,0x01,0x00,0x02,0xff,0x00,0x02,}// 右
{0xff,0x01,0x00,0x07,0x00,0x01,0x09,}// 转至预置点 001 
{0xff,0x01,0x00,0x03,0x00,0x01,0x05,}// 设置预置点 001 
{0xff,0x01,0x00,0x05,0x00,0x01,0x07,}// 删除预置点 001 
//以下几组我使用的云台没有功能，仅做参考。
{0xff,0x01,0x00,0x20,0x00,0x00,0x21,}// 变倍短
{0xff,0x01,0x00,0x40,0x00,0x00,0x41,}// 变倍长
{0xff,0x01,0x00,0x80,0x00,0x00,0x81,}// 聚焦近
{0xff,0x01,0x01,0x00,0x00,0x00,0x02,}// 聚焦远
{0xff,0x01,0x02,0x00,0x00,0x00,0x03,}// 光圈小
{0xff,0x01,0x04,0x00,0x00,0x00,0x05,}// 光圈大
{0xff,0x01,0x00,0x0b,0x00,0x01,0x0d,}// 灯光关
{0xff,0x01,0x00,0x09,0x00,0x01,0x0b,}// 灯光开

以上对应的停命令均是 : 
{0xff,0x01,0x00,0x00,0x00,0x00,0x01,}// 停命令

下面是各个封装的功能函数。

返回校验和

/********************************************************************
名称：uint16_t  ADD8_Check(uint8_t *num_Data,uint16_t num_Size)
功能：返回校验和
输入：校验数组，校验长度
输出：校验值
作者：liuguizhou
**********************************************************************/
uint16_t  ADD8_Check(uint8_t *num_Data,uint16_t num_Size){
	uint16_t i;
	uint16_t data_sum=0;
	for(i=1; i<num_Size; i++){
		data_sum += num_Data[i];
	}
	data_sum = (data_sum%256)&0xFF;	
	return data_sum;
}

复位功能

这个复位功能需要参考完整版协议，上面没有提到。
可以参考这篇博客，有个别功能的讲解。

协议要点整理

/********************************************************************
名称：void PTZ_rest()
功能：云台复位
输入：无
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_rest(void)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=0x33;					//command2
	PTZ_TX_BUF[num++]=REST_level_speed; 
	PTZ_TX_BUF[num++]=REST_pitch_speed; 
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
	PTZ_Send_String(PTZ_TX_BUF, num);
}

云台停止移动

/********************************************************************
名称：void PTZ_stop()
功能：云台停止移动
输入：无
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_stop(void)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=0x00;			//command2
	PTZ_TX_BUF[num++]=0x00; 				//level_speed
	PTZ_TX_BUF[num++]=0x00; 		//pitch_speed
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
	PTZ_Send_String(PTZ_TX_BUF, num);
}

云台上下移动

/********************************************************************
名称：void PTZ_up_down(uint8_t command2)
功能：云台上下移动
输入：功能码2
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_up_down(uint8_t command2)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=command2;			//command2
	PTZ_TX_BUF[num++]=0x00; 				//level_speed
	PTZ_TX_BUF[num++]=PTZ_speed; 		//pitch_speed
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
}

云台左右移动


/********************************************************************
名称：void PTZ_left_right(uint8_t command2)
功能：云台左右移动
输入：功能码2
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_left_right(uint8_t command2)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=command2;			//command2
	PTZ_TX_BUF[num++]=PTZ_speed; 				//level_speed
	PTZ_TX_BUF[num++]=0x00; 		//pitch_speed
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
	PTZ_Send_String(PTZ_TX_BUF, num);
}

云台预置点的设置、调用和删除


/********************************************************************
名称：void PTZ_Set_preset_point(uint8_t command2, uint8_t serial_num)
功能：云台设置预置点
输入：功能码2, 预置点参数
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_Set_preset_point(uint8_t command2, uint8_t serial_num)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=command2;			//command2
	PTZ_TX_BUF[num++]=0x00; 				
	PTZ_TX_BUF[num++]=serial_num; 		
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
	PTZ_Send_String(PTZ_TX_BUF, num);
}

/********************************************************************
名称：void PTZ_Delete_preset_point(uint8_t command2, uint8_t serial_num)
功能：云台删除预置点
输入：功能码2, 预置点参数
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_Delete_preset_point(uint8_t command2, uint8_t serial_num)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=command2;			//command2
	PTZ_TX_BUF[num++]=0x00; 				
	PTZ_TX_BUF[num++]=serial_num; 		
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
	PTZ_Send_String(PTZ_TX_BUF, num);
}


/********************************************************************
名称：void PTZ_Delete_preset_point(uint8_t command2, uint8_t serial_num)
功能：云台调用预置点
输入：功能码2, 预置点参数
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_Call_preset_point(uint8_t command2, uint8_t serial_num)
{
	uint16_t num=0;
	uint8_t check_num = 0;
	PTZ_TX_BUF[num++]=Sync_byte;
	PTZ_TX_BUF[num++]=PTZ_addres; 	//PTZ地址码
	PTZ_TX_BUF[num++]=0x00; 				//command1
	PTZ_TX_BUF[num++]=command2;			//command2
	PTZ_TX_BUF[num++]=0x00; 				
	PTZ_TX_BUF[num++]=serial_num; 
	check_num = ADD8_Check(PTZ_TX_BUF,num);
	PTZ_TX_BUF[num++]=check_num; 
	PTZ_Send_String(PTZ_TX_BUF, num);
}

以上就是最基础的云台控制功能，下面就要实现跑点的操作。这个云台本身无法获取当前位置的信息，所以只有采取发送移动指令，运行一段时间，停止指令。但是每次这样通过时间控制，难免会出现空转，卡转等一些情况，所以还是采取先设置预置点，再依次跑预置点的方案。这样子会更准确一些，其实这里可以看到，预置点就是记录位置信息，所以肯定是可以获取位置信息的，而且通过查看协议，也可以看到有获取位置信息的功能码，不过经过测试，我手上这款云台并不支持这个功能。

/********************************************************************
/********************************************************************
名称：void PTZ_Init(void)
功能：云台第一次初始化
输入：无
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_Init(void)
{
	if(PTZ_Init_flag == 1){
		if(preset_point_count == 0){
			PTZ_move_left_right(left,14);
			if(set_Preset_point_success_flag == 1){
				set_Preset_point_success_flag = 0;
				Preset_point = ((Preset_point)%256)&0xff;
				PTZ_Set_preset_point(Set_preset_point, Preset_point);
				stop_timer_flag = 1;
				Timer_Start(ETIM1);
				feed_dogs();
				}
			}
		if((preset_point_count >= 1 && preset_point_count < 3) || (preset_point_count > 6 && preset_point_count < 9)){
			PTZ_move_left_right(left,76);
			if(set_Preset_point_success_flag == 1){
				set_Preset_point_success_flag = 0;
				Preset_point = ((Preset_point+1)%256)&0xff;
				PTZ_Set_preset_point(Set_preset_point, Preset_point);
				stop_timer_flag = 1;
				Timer_Start(ETIM1);
				feed_dogs();
			}
		}
		if(preset_point_count == 3 || preset_point_count == 6){
			PTZ_move_up_down(down, 35);
			if(set_Preset_point_success_flag == 1){
				set_Preset_point_success_flag = 0;
				Preset_point = ((Preset_point+1)%256)&0xff;
				PTZ_Set_preset_point(Set_preset_point, Preset_point);
				stop_timer_flag = 1;
				Timer_Start(ETIM1);
				feed_dogs();
			}
		}	
		if(preset_point_count > 3 && preset_point_count < 6){
			PTZ_move_left_right(right, 76);
			if(set_Preset_point_success_flag == 1){
				set_Preset_point_success_flag = 0;
				Preset_point = ((Preset_point+1)%256)&0xff;
				PTZ_Set_preset_point(Set_preset_point, Preset_point);
				stop_timer_flag = 1;
				Timer_Start(ETIM1);
				feed_dogs();
			}
		}
	}
}


/**
名称：void PTZ_move_left_right(uint8_t command2,uint16_t move_time)
功能：云台左右移动
输入：功能码2， 移动时间
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_move_left_right(uint8_t command2,uint16_t move_time)
{
	if(preset_point_count < 9){
		if(move_success_flag == 1){
			PTZ_left_right(command2);
			move_timer_flag = 1;
			Timer_Start(ETIM2);
			move_success_flag = 0;
			feed_dogs();
		}
		if(move_timer_count >= move_time){
			Timer_Close(ETIM2);
			move_timer_count = 0;
			stop_success_flag = 1;
			feed_dogs();
			}
		
		if(stop_success_flag == 1){
			set_Preset_point_success_flag = 1;
			printf("set_Preset_point_success_flag\r\n");
			PTZ_stop();
			stop_success_flag = 0;
			feed_dogs();
		}
		if(stop_timer_count >= 20){
			Timer_Close(ETIM1);
			stop_timer_count = 0;
			stop_timer_flag = 0;
			move_success_flag = 1;
			preset_point_count++;
			printf("preset_point_count+++ = %d \r\n", preset_point_count);
			feed_dogs();
		}
	}
}

/********************************************************************
名称：void PTZ_move_up_down(uint8_t command2, uint16_t move_time)
功能：云台上下移动
输入：功能码2，移动时间
输出：无
作者：liuguizhou
**********************************************************************/
void PTZ_move_up_down(uint8_t command2, uint16_t move_time)
{
	if(preset_point_count < 9){
		if(move_success_flag == 1){
			move_timer_flag = 1;
			Timer_Start(ETIM2);
			PTZ_up_down(command2);
			move_success_flag = 0;
			feed_dogs();
		}
		if(move_timer_count == move_time){
			move_timer_count = 0;
			Timer_Close(ETIM2);
			stop_success_flag = 1;
			feed_dogs();
			}
		
		if(stop_success_flag == 1){
			set_Preset_point_success_flag = 1;
			PTZ_stop();
			stop_success_flag = 0;
			feed_dogs();
		}
		if(stop_timer_count == 20){
			Timer_Close(ETIM1);
			stop_timer_count = 0;
			stop_timer_flag = 0;
			move_success_flag = 1;
			preset_point_count++;
			printf("preset_point_count--- = %d \r\n", preset_point_count);
			feed_dogs();
		}
	}
}

Bing's Blogs