云台控制

最近做了一个云台设备的开发。

云台设备，说白了就是一个单片机驱动电机或者继电器从而实现摄像头在垂直方向的运动，水平方向的摆动，以及电子镜头的聚焦光圈变化和变倍。

 

上位机通过串口连到单片机。真正的控制功能是通过上位机向串口发送数据而实现的。LINUX编程实现就是写串口。

控制协议一般为PELCO-D与PELCO-P。本项目用的是前者。

PELCO-D:

　　数据格式：1位起始位、8位数据、1位停止位，无效验位。波特率：2400B/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]

　　以地址码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,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,0x07,0x00,0x01,0x09,}//转至预置点001

　　{0xff,0x01,0x00,0x03,0x00,0x01,0x05,}//设置预置点001

　　{0xff,0x01,0x00,0x05,0x00,0x01,0x07,}//删除预置点001

　　以上对应的停命令均是:

　　{0xff,0x01,0x00,0x00,0x00,0x00,0x01,}//停命令

按照以上协议格式，编写代码seri.c如下：

#include
#include
#include
#include
#include
#include
#include
#include
#include

#define TTYS      "/dev/ttyS0" //串口设备
#define ADDR   0x01      //云台设备逻辑地址
#define BAUD_RATE 2400  //波特率
#define DATA_BITS 8    //数据位
#define NEVENT    'N'   //校验
#define NSTOP     1    //停止位

//#define DBG_YT    //调试信息打印

/*串口设置函数*/
int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
{
 struct termios newtio,oldtio;
 if  ( tcgetattr( fd,&oldtio)  !=  0) {
  perror("SetupSerial 1");
  return -1;
 }
 bzero( &newtio, sizeof( newtio ) );
 newtio.c_cflag  |=  CLOCAL | CREAD;
 newtio.c_cflag &= ~CSIZE;

 switch( nBits )
 {
 case 7:
  newtio.c_cflag |= CS7;
  break;
 case 8:
  newtio.c_cflag |= CS8;
  break;
 }

 switch( nEvent )
 {
 case 'O':
  newtio.c_cflag |= PARENB;
  newtio.c_cflag |= PARODD;
  newtio.c_iflag |= (INPCK | ISTRIP);
  break;
 case 'E':
  newtio.c_iflag |= (INPCK | ISTRIP);
  newtio.c_cflag |= PARENB;
  newtio.c_cflag &= ~PARODD;
  break;
 case 'N': 
  newtio.c_cflag &= ~PARENB;
  break;
 }

 switch( nSpeed )
 {
 case 2400:
  cfsetispeed(&newtio, B2400);
  cfsetospeed(&newtio, B2400);
  break;
 case 4800:
  cfsetispeed(&newtio, B4800);
  cfsetospeed(&newtio, B4800);
  break;
 case 9600:
  cfsetispeed(&newtio, B9600);
  cfsetospeed(&newtio, B9600);
  break;
 case 115200:
  cfsetispeed(&newtio, B115200);
  cfsetospeed(&newtio, B115200);
  break;
 default:
  cfsetispeed(&newtio, B9600);
  cfsetospeed(&newtio, B9600);
  break;
 }
 if( nStop == 1 )
  newtio.c_cflag &=  ~CSTOPB;
 else if ( nStop == 2 )
 newtio.c_cflag |=  CSTOPB;
 newtio.c_cc[VTIME]  = 0;
 newtio.c_cc[VMIN] = 0;
 tcflush(fd,TCIFLUSH);
 if((tcsetattr(fd,TCSANOW,&newtio))!=0)
 {
  perror("com set error");
  return -1;
 }
#ifdef DBG_YT
 printf("set done!\n");
#endif
 return 0;
}

/*打开串口函数*/

int open_port()
{
 int fd;

 fd = open( TTYS , O_RDWR|O_NOCTTY|O_NDELAY);
 if (-1 == fd){
  perror("Can't Open Serial Port");
  return(-1);
 }
#ifdef DBG_YT
 else
  printf("open ttyS0 .....\n");
#endif 

 if(fcntl(fd, F_SETFL, 0)<0)
  printf("fcntl failed!\n");
#ifdef DBG_YT
 else
  printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));
#endif

 if(isatty(STDIN_FILENO)==0)
  printf("standard input is not a terminal device\n");
#ifdef DBG_YT
 else
  printf("isatty success!\n");
 printf("fd-open=%d\n",fd);
#endif

 return fd;
}

int main(void)
{
 int fd;
 int id,speed,i;
 char flag;
 char a[7]={0xff,ADDR,0,0,0,0,0};

//打开串口TTYS

 if((fd = open_port())<0){    

  perror("open_port error");
  return 0;
 }          

//设置串口TTYS
 if((i = set_opt(fd, BAUD_RATE, DATA_BITS, NEVENT, NSTOP))<0){
  perror("set_opt error");
  return 0;
 }
 //功能实现

 flag=0;
 while(!flag){
  printf("Enter the command id and speed:\n");
  scanf("%d%d",&id,&speed);   //此处通过命令行获取操作命令
  switch(id){
   case 0:       //停止
    break;
   case 1:       //向上
    a[3]=0x08; a[5]=speed; break;
   case 2:       //向下
    a[3]=0x10; a[5]=speed; break;
   case 3:       //向左
    a[3]=0x04; a[4]=speed; break;
   case 4:       //向右
    a[3]=0x02; a[4]=speed; break;
   case 5:       //变倍短
    a[3]=0x20; break;
   case 6:       //变倍长
    a[3]=0x40; break;
   case 7:       //聚焦近
    a[3]=0x80; break;
   case 8:       //聚焦远
    a[2]=0x01; break;
   case 9:       //光圈小
    a[2]=0x02; break;
   case 10:             //光圈大
    a[2]=0x04; break;
   default:
    flag=1;      //程序退出
  }
  a[6]=(a[1]+a[2]+a[3]+a[4]+a[5])%(0x100); //校验
  while( (i = write(fd,a,7) )!=7);               //向串口写命令
  bzero(&a[2],4);
 }
 /*关闭串口*/
 close(fd);
#ifdef DBG_YT
 printf("YT control is exiting\n");
#endif
 return 0;
}

在应用到云台之前做了测试。

开发板运行本程序seri.c，向串口发送（write）数据，主机通过串口接收(read)数据。如果接收到的数据格式为PELCO-D格式，就说明成功完成写串口操作。

过程：

1 开发板通过串口与主机连接。

2 交叉编译seri.c生成seri可执行文件，下载到开发板运行。

3 主机编译测试代码rec.c生成rec可执行文件，运行。

结果：

附：rec.c

#include
#include
#include
#include
#include
#include
#include
#include
#include

#define TTYS      "/dev/ttyS0" //串口设备
#define ADDR   0x01      //云台设备逻辑地址
#define BAUD_RATE 115200  //波特率
#define DATA_BITS 8    //数据位
#define NEVENT    'N'   //校验
#define NSTOP     1    //停止位

//#define DBG_YT    //调试信息打印

/*串口设置*/
int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
{
 struct termios newtio,oldtio;
 if  ( tcgetattr( fd,&oldtio)  !=  0) {
  perror("SetupSerial 1");
  return -1;
 }
 bzero( &newtio, sizeof( newtio ) );
 newtio.c_cflag  |=  CLOCAL | CREAD;
 newtio.c_cflag &= ~CSIZE;

 switch( nBits )
 {
 case 7:
  newtio.c_cflag |= CS7;
  break;
 case 8:
  newtio.c_cflag |= CS8;
  break;
 }

 switch( nEvent )
 {
 case 'O':
  newtio.c_cflag |= PARENB;
  newtio.c_cflag |= PARODD;
  newtio.c_iflag |= (INPCK | ISTRIP);
  break;
 case 'E':
  newtio.c_iflag |= (INPCK | ISTRIP);
  newtio.c_cflag |= PARENB;
  newtio.c_cflag &= ~PARODD;
  break;
 case 'N': 
  newtio.c_cflag &= ~PARENB;
  break;
 }

 switch( nSpeed )
 {
 case 2400:
  cfsetispeed(&newtio, B2400);
  cfsetospeed(&newtio, B2400);
  break;
 case 4800:
  cfsetispeed(&newtio, B4800);
  cfsetospeed(&newtio, B4800);
  break;
 case 9600:
  cfsetispeed(&newtio, B9600);
  cfsetospeed(&newtio, B9600);
  break;
 case 115200:
  cfsetispeed(&newtio, B115200);
  cfsetospeed(&newtio, B115200);
  break;
 default:
  cfsetispeed(&newtio, B9600);
  cfsetospeed(&newtio, B9600);
  break;
 }
 if( nStop == 1 )
  newtio.c_cflag &=  ~CSTOPB;
 else if ( nStop == 2 )
 newtio.c_cflag |=  CSTOPB;
 newtio.c_cc[VTIME]  = 0;
 newtio.c_cc[VMIN] = 0;
 tcflush(fd,TCIFLUSH);
 if((tcsetattr(fd,TCSANOW,&newtio))!=0)
 {
  perror("com set error");
  return -1;
 }
#ifdef DBG_YT
 printf("set done!\n");
#endif
 return 0;
}

int open_port()
{
 int fd;

 fd = open( TTYS , O_RDWR|O_NOCTTY);
 if (-1 == fd){
  perror("Can't Open Serial Port");
  return(-1);
 }
#ifdef DBG_YT
 else
  printf("open ttyS0 .....\n");
#endif 

 if(fcntl(fd, F_SETFL, 0)<0)
  printf("fcntl failed!\n");
#ifdef DBG_YT
 else
  printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));
#endif

 if(isatty(STDIN_FILENO)==0)
  printf("standard input is not a terminal device\n");
#ifdef DBG_YT
 else
  printf("isatty success!\n");
 printf("fd-open=%d\n",fd);
#endif

 return fd;
}

int main(void)
{
 int fd;
 int id,speed,i;
 char flag;
 unsigned char receive[9];

 if((fd = open_port())<0){    //打开串口TTYS
  perror("open_port error");
  return 0;
 }          //设置串口TTYS
 if((i = set_opt(fd, BAUD_RATE, DATA_BITS, NEVENT, NSTOP))<0){
  perror("set_opt error");
  return 0;
 }
 
 while(1){         //接受数据
 bzero(receive,7);
 i=read(fd,receive,7);
 if(i>0){
 for(i=0;i<=6;i++)
 printf("0x%2x  ",receive[i]);
 printf("\n");
 }
 sleep(2);
 }

 close(fd);
#ifdef DBG_YT
 printf("YT control is exiting\n");
#endif
 return 0;
}