模块学习2：基于PELCO-D协议对云台进行定点控制

开发手上的一个云台，使用的就是PELCO-D协议。这个协议开始网上找了一圈，发现下载完整版的协议大多是还都要收费，后面在官方下载了一份原版完整版协议的协议内容，配合网上搜索到的资料完成对手上这块云台的配置。虽然原版的协议支持很多功能，包括摄像头的一些控制，这当然与具体的产品有关，手上的这款只是对云台的控制，包括复位，预置设置，调用和删除等一些基础的功能。
由于最终是要结合手上的串口摄像机，完成定时定点进行拍照的功能，所以这里先写云台的功能函数，下面写摄像头模块时再一起结合。

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

PELCO-D:

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

字节 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;              //command1PTZ_TX_BUF[num++]=0x33;                    //command2PTZ_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;              //command1PTZ_TX_BUF[num++]=0x00;            //command2PTZ_TX_BUF[num++]=0x00;                //level_speedPTZ_TX_BUF[num++]=0x00;         //pitch_speedcheck_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;              //command1PTZ_TX_BUF[num++]=command2;            //command2PTZ_TX_BUF[num++]=0x00;                //level_speedPTZ_TX_BUF[num++]=PTZ_speed;        //pitch_speedcheck_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;              //command1PTZ_TX_BUF[num++]=command2;            //command2PTZ_TX_BUF[num++]=PTZ_speed;               //level_speedPTZ_TX_BUF[num++]=0x00;         //pitch_speedcheck_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;              //command1PTZ_TX_BUF[num++]=command2;            //command2PTZ_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;              //command1PTZ_TX_BUF[num++]=command2;            //command2PTZ_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;              //command1PTZ_TX_BUF[num++]=command2;            //command2PTZ_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();}}
}

这就完成了云台定点移动的基本功能，之前找资料下载了一个相对比较完整的小程序，我一起打包上传到资源中，有需要自取。