CN104062933B - 基于dsp和fpga的无人直升机飞行控制方法 - Google Patents

Abstract

一种基于DSP和FPGA的无人直升机飞行控制系统及方法，由无人直升机机体、机载舵机组和无线电接收机构成的模型直升机分别连接机载控制平台和地面站监控平台，机载控制平台有机载下位机和分别与机载下位机相连的姿态测量模块、经纬度测量模块和机载上位机，以及与机载下位机相连的气压传感器，地面站监控平台设置有与无线电接收机无线连接的遥控器和与机载上位机相连的地面站PC。方法有用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据上传，用于DSP的姿态、经纬度和高度，转发飞行数据和命令字，实现飞行数据存储，飞行数据的接收和显示，以及向机载平台发布飞行命令。本发明使无人直升机具有了自主飞行能力：自主调节姿态和位置。

Description

基于DSP和FPGA的无人直升机飞行控制方法

技术领域

本发明涉及一种无人直升机飞行控制系统。特别是涉及一种基于DSP和FPGA的无人直升机飞行控制方法。

背景技术

与固定翼飞机相比，直升机具有起飞着陆场地小、可垂直起降、空中悬停、使用灵活等一系列优点。作为直升机的一种特例，无人直升机还具有无人员伤亡、体积小、造价低、战场生存能力高等有人驾驶直升机无法比拟的优越性。它的这些独特性，使其在民用和军事方面均有着广泛的应用。

在民用方面，主要的应用领域有：空中交通管制、大地测量、气象观测、城市环境控制、地质勘测、森林防火、人工降雨、超高压输电线路巡检和通信中继等。

在军事方面，主要应用领域有：战场侦察、通信中继、通信情报侦察、军事测绘、探测战场上大规模杀伤性武器(有害物质)、与其他空中平台协同作战、与卫星或预警机组成多/双基地雷达预警/侦察系统、信息战、电子干扰及对敌防空兵压制等。

无人直升机的应用前提是自主飞行能力。所以，设计一个使无人直升机具有自主飞行能力的无人直升机飞行控制系统是有意义的。

发明内容

本发明所要解决的技术问题是，提供一种能够增强系统的可靠性和灵活性的基于DSP和FPGA的无人直升机飞行控制方法。

本发明所采用的技术方案是：一种用于基于DSP和FPGA的无人直升机飞行控制系统的控制方法，所述系统包括有由无人直升机机体、机载舵机组和无线电接收机构成的模型直升机，所述的模型直升机分别连接机载控制平台和地面站监控平台，其中，所述的机载控制平台包括有机载下位机和分别通过RS232接口与所述的机载下位机相连的姿态测量模块、经纬度测量模块和机载上位机，以及通过SPI接口与所述的机载下位机相连的气压传感器，所述的地面站监控平台设置有与所述的无线电接收机无线连接的遥控器和与所述的机载上位机相连的地面站PC，所述的方法包括有机载下位机控制方法、机载上位机控制方法和地面站PC控制方法三部分，其中，所述的机载下位机控制方法包括有用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据上传的控制方法，以及用于DSP的姿态、经纬度和高度的控制方法；所述机载上位机控制方法包括有转发飞行数据和命令字，实现飞行数据的存储；所述的地面站PC控制方法包括飞行数据的接收和显示，以及向机载控制平台发布飞行命令；

所述的机载下位机控制方法中的信息采集是指：FPGA通过串口采集姿态测量模块和经纬度测量模块测量的姿态测量值和经纬度测量值，通过SPI接口采集气压传感器测量的大气压和温度，然后发起中断通知DSP读取采集信息；所述的机载下位机控制方法中的命令字接收是指：FPGA通过串口接收并处理机载上位机转发的命令字；所述的机载下位机控制方法中的舵机驱动是指：FPGA根据飞行模式产生驱动舵机组的PWM信号；所述的机载下位机控制方法中的飞行数据上传是指：FPGA通过串口将飞行数据上传给机载上位机，所述的通过SPI接口采集气压传感器测量的大气压和温度，包括如下步骤：

1)复位：FPGA向设置在无人直升机上的气压传感器发送复位命令，延时3ms后进入下一步骤；

2)校正参数读取：FPGA向气压传感器发送校正参数读取命令，然后FPGA读取气压传感器中的校正参数，并存入内部寄存器供DSP读取；

3)主循环：FPGA先向气压传感器发送大气压转换命令，延时9ms后读取气压传感器中的大气压，并存入内部寄存器，再向气压传感器发送温度转换命令，延时9ms后读取气压传感器中的温度，并存入内部寄存器，然后，将FPGA大气压中断标志位置1，FPGA发起大气压中断通知DSP读取大气压测量值和温度测量值；

4)FPGA延时1s的采样周期后返回步骤3)。

所述的命令字接收包括接收飞行模式切换命令字、姿态给定命令字和位置给定命令字，具体包括如下步骤：

(1)接收命令字第一个字节；

(2)判断所接收字节的高4位是否都为0，若是，说明正在接收的命令字为飞行模式切换命令字，将所接收字节的低4位赋给飞行模式寄存器，上传飞行模式返回步骤(1)；若不是，说明正在接收的命令字为姿态给定命令字或位置给定命令字，进入下一步骤；

(3)接收命令字后四个字节；

(4)根据命令字类型将姿态给定值或者位置给定值存入FPGA内特定的寄存器供DSP读取，然后返回步骤(1)。

所述的舵机驱动是选用FPGA中5路PWM信号输出通道的前两路PWM信号输出通道驱动机载舵机组中的总距舵机和油门舵机，后三路PWM信号输出通道驱动横滚舵机、俯仰舵机和偏航舵机，具体包括如下步骤：

(1)将FPGA中的5个计数寄存器和5个初值寄存器的初始值设定为1500，重装载寄存器和周期寄存器的初始值设定为0；

(2)判断是否经过一微秒的计数周期，是则将计数寄存器中非零的寄存器减1，周期寄存器加1后进入下一步骤，否则直接进入下一步骤；

(3)判断DSP是否将重装载寄存器设为0xFF或者周期寄存器是否大于28000，是则说明DSP更新了初值寄存器或者本周期超过28了毫秒，将计数寄存器中的计数初值装载至计数寄存器，将重装载寄存器和周期寄存器设为0，产生下个周期的PWM信号后进入下一步；否则直接进入下一步；

(4)由飞行模式寄存器判断飞行模式，飞行模式寄存器等于0为手动模式，进入步骤(5)；飞行模式寄存器等于1为姿态模式，进入步骤(6)；飞行模式寄存器等于2为位置模式，进入步骤(7)；

(5)将与5个计数寄存器相对应的5路PWM信号输入通道接收的信号输出在同样与5个计数寄存器相对应的5路PWM信号输出通道上后返回步骤(2)；

(6)将前两路PWM信号输入通道接收的信号输出在前两路PWM信号输出通道上；5个计数寄存器中的后三个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤(2)；

(7)5个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤(2)。

所述的DSP的姿态控制方法包括如下步骤：

(1)从FPGA读取姿态给定值、姿态测量值和飞行模式；

(2)判断当前飞行模式是否为姿态模式或者位置模式，是则进入下一步，否者直接结束；

(3)将横滚角给定值、俯仰角给定值和偏航角给定值依次与横滚角测量值、俯仰角测量值和偏航角测量值相减，得横滚角偏差roll_error、俯仰角偏差pitch_error和偏航角偏差yaw_error；

(4)计算横滚舵机控制量增量Δu_roll、俯仰舵机控制量增量Δu_pitch和偏航舵机控制量增量Δu_yaw：

Δu_roll＝roll_kp×(roll_error(k)-roll_error(k-1))+roll_ki×roll_error(k)

Δu_pitch＝pitch_kp×(pitch_error(k)-pitch_error(k-1))+pitch_ki×pitch_error(k)

Δu_yaw＝yaw_kp×(yaw_error(k)-yaw_error(k-1))+yaw_ki×yaw_error(k)；

其中roll_error(k)、pitch_error(k)和yaw_error(k)是本次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，roll_error(k-1)、pitch_error(k-1)和yaw_error(k-1)是上次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，roll_kp和roll_ki是横滚角控制的比例增益和积分增益，pitch_kp和pitch_ki是俯仰角控制的比例增益和积分增益，yaw_kp和yaw_ki是偏航角控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准；

(5)累加得横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw：

u_roll＝Δu_roll+u_roll

u_pitch＝Δu_pitch+u_pitch

u_yaw＝Δu_pitch+u_yaw

然后对横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw进行限幅；

(6)将横滚舵机控制量、俯仰舵机控制量和偏航舵机控制量写入FPGA内部寄存器；

(7)将FPGA内部重装载寄存器置为0xFF，使写入FPGA的舵机控制量生效。

所述的DSP的经纬度控制方法包括如下步骤：

(1)从FPGA读取经纬度给定值、经纬度测量值和飞行模式；

(2)将表示经纬度测量值的字符串解算成对应的浮点数；

(3)将解算出的浮点数写入FPGA，通知FPGA上传经纬度给定值和经纬度测量值；

(4)判断当前飞行模式是否为位置模式，是则进入下一步，否者直接结束；

(5)采用高斯克吕格投影方法，将经纬度给定值和经纬度测量值换算成两点间南北向距离偏差NS_error和东西向距离偏差EW_error；

(6)采用坐标变换，将南北向距离偏差NS_error和东西向距离偏差EW_error换算成机身纵向距离偏差x_error和横向距离偏差y_error；坐标变换公式为：

x_error＝NS_error*cos(Ψ)+EW_error*sin(Ψ)

y_error＝-NS_error*sin(Ψ)+EW_error*cos(Ψ)

其中Ψ为无人直升机的偏航角；

(7)计算横滚角给定增量ΔPhi_G和俯仰角给定增量ΔTheta_G：

ΔPhi_G＝y_kp×(y_error(k)-y_error(k-1))+y_ki×y_error(k)

ΔTheta_G＝x_kp×(x_error(k)-x_error(k-1))+x_ki×x_error(k)；

其中x_error(k)和y_error(k)是本次经纬度控制周期计算出的机身纵向距离偏差和横向距离偏差，x_error(k-1)和y_error(k-1)是上次经纬度控制周期计算出的机身纵向距离偏差和横向距离偏差，x_kp和x_ki是机身纵向距离控制的比例增益和积分增益，y_kp和y_ki是机身横向距离控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准；

(8)累加得横滚角给定值Phi_G和俯仰角给定值Theta_G：

Phi_G＝ΔPhi_G+Phi_G

Theta_G＝ΔTheta_G+Theta_G

然后将Phi_G和Theta_G限幅在-10°和10°之间。

所述的DSP的高度的控制方法包括如下步骤：

(1)从FPGA读取大气压测量值、温度测量值、高度给定值和飞行模式；

(2)根据校正参数和温度对大气压测量值进行温度补偿，得到校正大气压，然后利用大气压和海拔高度的关系，将校正大气压解算成高度测量值；

(3)将解算出的高度测量值写入FPGA，通知FPGA上传高度给定值和高度测量值；

(4)判断当前飞行模式是否为位置模式，是则进入下一步，否则直接结束；

(5)用高度给定值减去高度测量值得到高度偏差z_error；

(6)计算油门舵机控制量增量Δu_pow：

Δu_pow＝z_kp×(z_error(k)-z_error(k-1))+z_ki×z_error(k)；

其中z_error(k)是本次高度控制周期计算出的高度偏差，z_error(k-1)是上次高度控制周期计算出的高度偏差，z_kp和z_ki是高度控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准，

(7)累加得新油门舵机控制量u_pow＝Δu_pow+u_pow并对u_pow限幅；

(8)根据油门舵机控制量u_pow和总距舵机控制量u_col的对应关系，计算出总距舵机控制量u_col；

(9)将油门舵机控制量和总距舵机控制量写入FPGA内部寄存器。

本发明的基于DSP和FPGA的无人直升机飞行控制方法，使无人直升机具有了自主飞行能力：自主调节姿态和位置。本发明的基于DSP和FPGA的无人直升机飞行控制系统具有三种飞行模式：手动模式、姿态模式和位置模式。通过地面站PC，使用者可实时获知系统的飞行数据，根据应用需要改变系统的飞行模式、姿态给定和位置给定。这些特点极大地增强系统的可靠性和灵活性，降低无人直升机使用难度因而能扩大其应用范围。

附图说明

图1是本发明的整体系统构成结构示意图；

图2是本发明的机载下位机的构成结构示意图；

图中

1：模型直升机 2：机载控制平台

3：地面站监控平台 11：无人直升机机体

12：机载舵机组 13：无线电接收机

21：机载下位机 22：姿态测量模块

23：经纬度测量模块 24：气压传感器

25：机载上位机 31：遥控器

32：地面站PC 211：现场可编程门阵列

212：数字信号处理模块 213：RS232协议电平转换模块

214：40M有源晶振 215：现场可编程门阵列程序存储模块

216：接30M无源晶振 217：JTAG接口

218：多路三态缓冲模块 219：JTAG接口

图3是FPGA采集姿态的流程图；

图4是FPGA采集经纬度的流程图；

图5是FPGA采集大气压的流程图；

图6是FPGA接收命令字的流程图；

图7是FPGA驱动舵机的流程图；

图8是FPGA上传飞行数据的流程图；

图9是姿态控制方法流程图；

图10是经纬度控制方法流程图；

图11是高度控制方法流程图；

图12是机载上位机工作流程图；

图13是地面站PC工作流程图。

具体实施方式

下面结合实施例和附图对本发明的基于DSP和FPGA的无人直升机飞行控制方法做出详细说明。

本发明的基于DSP和FPGA的无人直升机飞行控制方法，提供了一种无人直升机自主飞行的控制系统和控制方法。该系统有三种飞行模式。手动模式下，地面操控员可通过遥控器操纵直升机飞行；姿态模式下，系统自主调节无人直升机姿态；位置模式下，系统自主调节无人直升机位置。这极大地增强系统的可靠性和灵活性，降低无人直升机使用难度因而能扩大其应用范围。

如图1所示，本发明的基于DSP和FPGA的无人直升机飞行控制系统，包括有由无人直升机机体11、机载舵机组12和无线电接收机13构成的模型直升机1，所述的模型直升机1分别连接机载控制平台2和地面站监控平台3，其中，所述的机载控制平台2包括有机载下位机21和分别通过RS232接口与所述的机载下位机21相连的姿态测量模块(IMU)22、经纬度测量模块(GSP)23和机载上位机25，以及通过SPI接口与所述的机载下位机21相连的气压传感器24，所述的地面站监控平台3设置有与所述的无线电接收机13无线连接的遥控器31和与所述的机载上位机25相连的地面站PC 32。

所述的机载舵机组12含有5个舵机，可接收机载控制平台2发出的PWM信号，实现对无人直升机的合理操纵。5个舵机中的油门舵机和总距舵机，可调节发动机油门和主旋翼总距，从而控制主旋翼的转速和升力；俯仰舵机和横滚舵机可改变主旋翼的纵横向周期变距，使主旋翼前后左右倾斜，从而实现无人直升机的俯仰和滚转运动；偏航舵机可调节尾桨桨距，从而改变无人直升机的航向。所述的无线电接收机13通过射频无线电与地面站监控平台通讯，将遥控器31发出的无线电信号转化为五路PWM信号，上传给机载控制平台2的机载下位机21。

所述的机载控制平台2中的IMU为荷兰Xsens Technologies B.V.公司的MTi小型(IMU)。其角度分辨率：0.05°RMS；角度动态精度：2°RMS。工作温度：-55℃至+125℃；通讯协议：RS232C标准。

所述的机载控制平台2中的GPS与机载下位机21，通过RS232接口相连，按照NMEA-0183格式进行异步串行通信。因此任何遵照NMEA-0183格式、提供RS232接口的GPS模块都适用于本发明。

所述的机载控制平台2中的气压传感器模块24，主要由MEAS公司的MS5611芯片组成。大气压力受温度影响，需要考虑温度补偿，因此MS5611既可以测量大气压又可测量温度。MS5611采用SPI接口将校正参数和温度、大气压的测量值上传给FPGA，FPGA再通知DSP读取。DSP利用校正参数对大气压测量值进行补偿，得到校正大气压，然后根据大气压和海拔高度的关系计算出无人直升机的海拔高度。

所述的机载控制平台2中的机载上位机25，主要完成飞行数据存储并充当地面站监控平台与机载下位机进行的通讯中介。机载上位机采用北京盛博系统科技有限公司的PC104PLUS嵌入式计算机AT-5010和图像采集卡MPEG-4。AT5010具有RS232接口和USB接口。本发明使用其RS232接口与机载下位机通讯，使用USB接口安装WIFI模块与地面站PC通讯。AT-5010还可以安装Windows操作系统，其操作方式与PC机完全相同，并且Windows的许多应用软件都可安装在AT-5010上。这使得在机载上位机上进行二次开发非常灵活方便。

机载下位机21是本发明的核心。如图2所示，所述的机载下位机21包括有：现场可编程门阵列(FPGA)模块211，分别与所述的现场可编程门阵列模块211相连的数字信号处理(DSP)模块212、RS232协议电平转换模块213、现场可编程门阵列程序存储模块215、多路三态缓冲模块218，其中，所述的现场可编程门阵列模块211通过SPI接口连接所述的气压传感器24，所述的RS232协议电平转换模块213通过RS232接口分别连接姿态测量模块22、经纬度测量模块23和机载上位机25，所述的多路三态缓冲模块218通过PWM输出通道连接机载舵机组12和通过PWM输入通道连接无线电接收机13。所述的现场可编程门阵列模块211还连接40M有源晶振214，JTAG接口219。所述的数字信号处理(DSP)模块212还连接30M无源晶振216和JTAG接口217。其中：

(1)DSP模块：主要由TI公司的数字信号处理器TMS320F2812组成。TMS320F2812外部配置30MHz无源晶振和JTAG接口电路。配置TMS320F2812的PLL(锁相环)为5倍频，其工作主频设置为150MHz。本发明使用DSP的外部存储器接口与FPGA连接。连接二者的信号线中，地址线7位，数据线8位。因此FPGA可将其内部的寄存器(不超过128字节)映射至DSP的地址空间。DSP通过读FPGA内部的寄存器来实现：获取IMU、GPS和气压传感器测量的姿态角、经纬度和大气压信息；获取机载上位机转发的命令字。DSP通过写FPGA内部存储单元来实现：更新驱动机载舵机组的PWM波，操纵机载舵机组，实现无人直升机的自主飞行。此外DSP的外部中断输入引脚XINT1与FPGA相连，FPGA有数据需DSP读取时会在这个引脚上发起中断请求。

(2)FPGA模块211：主要由Altera公司的芯片EP2C8T144C8N组成。EP2C8T144C8N外部配置有源晶振和JTAG接口电路。设置PLL为1倍频，EP2C8T144C8工作频率设置为40MHZ。通过编程，为EP2C8T144C8N配置4路RS232接口：COM1-COM4，一路SPI接口，8路PWM信号输出通道和8路PWM信号输入通道。

四路RS232接口中COM1、COM3分别与IMU和机载上位机进行数据通讯，其配置为：波特率115200、8个数据位、1个起始位、1个停止位，无奇偶校验。COM2与GPS模块通讯，其配置由被选的GPS模块决定。COM4保留不用。FPGA通过COM1、COM2接收姿态、经纬度，将其存入特定寄存器后发出中断请求DSP读取。为了实现数据存储和地面站的实时监控，FPGA通过COM2上传飞行数据给机载上位机，接收地面PC发布的命令字后发出中断请求通知DSP读取。

FPGA的SPI接口用于和气压传感器进行通讯。FPGA通过SPI接口可读取气压传感器校正参数和气压、温度侧量值，然后发出中断请求通知DSP读取。

8路PWM信号输出通道PWMout[0:7]可输出8路周期22ms，脉宽1.2-1.8ms(不同脉宽决定舵机不同动作)的PWM信号。本发明采用PWMout[0:4]驱动5个机载舵机。8路PWM信号输入通道PWMin[0:7]可接收8路相同周期、相同脉宽范围的PWM信号。本发明采用PWMin[0:4]接收无线电接收机上传的PWM信号。

(3)RS232协议电平转换模块213：主要由MAXIM公司芯片MAX3232E组成。FPGA的I/O逻辑1为高电平3.3V，逻辑0为低电平0V；RS232电平为负逻辑，即逻辑1为负电平-3V～-12V，逻辑0为正电平+3V～+12V。因此本发明采用MAX3232E实现FPGA电平标准和RS232电平标准的转换。一片MAX3232E能实现两路RS232接口的电平转换。本发明的下位机拥有四路RS232接口，故采用两片MAX3232E。

(4)多路三态缓冲模块218：主要由TI公司的74ALVC164245组成。本发明中无人直升机的舵机需要5V高电平的PWM波驱动，机载无线电接收机也输出高电平5V的PWM波。而FPGA直接输入输出的高点平只有3.3V，因此需要74ALV164245缓冲，实现电平转换。74ALVC16425的A端口高电平3.3V，用来连接FPGA；B端口高电平5V，用来连接直升机控制舵机和机载无线电接收机。74ALVC164245能实现16路电平转换，每一路有两个配对引脚如1A1(位于A端口)和1B1(位于B端口)。16路电平转换分为两组。第一组1A1-1B1至1A8-1B8被配置为从A端口输入信号，B端口输出信号，可以转换FPGA输出的PWM信号驱动舵机；第二组2A1-2B1至2A8-2B8被配置为从B端口输入信号，A端口输出信号，可以转换无线电接收机输出的PWM信号输入至FPGA。

(5)FPGA程序存储模块215：主要由Altera公司的芯片EPCS4SI8N组成。Altera公司的FPGA芯片EP2C8T144C8N掉电后不能保存程序。因此需采用EPCS4SI8N保存程序。上电后，EP2C8T144C8N从EPCS4SI8N读入程序。

本发明的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，包括有机载下位机控制方法、机载上位机控制方法和地面站PC控制方法三部分，，其中，所述的机载下位机控制方法包括有用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据上传的控制方法，以及用于DSP的姿态、经纬度和高度的控制方法；所述机载上位机控制方法包括有转发飞行数据和命令字，实现飞行数据的存储；所述的地面站PC控制方法包括飞行数据的接收和显示，以及向机载控制平台发布飞行命令。

FPGA通过串口采集姿态测量模块22和经纬度测量模块23测量的姿态测量值和经纬度测量值，通过SPI接口采集气压传感器测量的大气压和温度，然后发起中断通知DSP读取采集信息，FPGA发起中断前，会将FPGA内姿态中断标志位、经纬度中断标志位或大气压中断标志位置为1，以说明本次中断的原因；所述的机载下位机控制方法中的命令字接收是指：FPGA通过串口接收并处理机载上位机转发的命令字；所述的机载下位机控制方法中的舵机驱动是指：FPGA根据飞行模式产生驱动舵机组的PWM信号；所述的机载下位机控制方法中的飞行数据上传是指：FPGA通过串口将飞行数据上传给机载上位机。

(一)机载下位机控制方法中的用于FPGA的信息采集，包括姿态信息采集、经纬度信息采集和大气压信息采集三个部分。其中，

1)姿态信息采集：

IMU通过RS232接口，每22ms将姿态测量值按帧格式0xFAFF320E<1><2><3><4>发送给FPGA。前四个字节0xFAFF320E是帧头；<1>、<2>和<3>是数据段，各占四个字节，是三个浮点数，分别表示横滚角、俯仰角和偏航角；<4>是数据段的校验和，占两个字节。因此采集姿态信息，需要在字节接收的基础上实现帧接收。

如图3所示，按照RS232通讯协议，FPGA实时检测外部特定引脚(该引脚与IMU相连)的电平变化，实现字节的接收。在字节接收的基础上，FPGA按照IMU约定的帧格式实现姿态帧接收。姿态帧接收分如下三个环节

(1)帧头匹配：接收四个字节与帧头0xFAFF320E进行匹配，如匹配成功，进入数据接收环节，否则一直进行帧头匹配。

(2)数据接收：连续接收十二字节的姿态角信息，将其写入FPGA内部的横滚角寄存器Phi、俯仰角寄存器Theta和偏航角寄存器Psi，然后计算这十二个字节的校验和，进入校验环节。

(3)校验环节：接收两个字节的校验和，然后与上一环节计算出的校验和进行比较，若两者相等，说明数据有效，将姿态中断标志位置为1，发起姿态中断通知DSP读取姿态测量值，将Update_Reg[0]置为1，以启动姿态信息上传过程，然后返回帧头匹配环节；若两者不相等，说明数据无效，直接返回帧头匹配环节。

2)经纬度信息采集：

本发明FPGA与GPS模块按照NMEA-0183格式进行异步串行通信。NMEA-0183定义了很多语句，本发明选择接收GPRMC语句。GPRMC语句格式为ASCII码序列：$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*<13><CR><LF>。其中前六个字节$GPRMC为帧头；逗号为分隔符；<1>为UTC时间，共10个字节，格式为hhmmss.sss；<2>为状态标志位，占1个字节；<3>为表示纬度的字符串，共9个字节，格式为ddmm.mmmm；<4>为纬度半球N或S；<5>为表示经度的字符串，共10个字节，格式为dddmm.mmmm；<6>为经度半球E或W；<7>至<13>为其他信息；<CR><LF>是回车和换行，作为语句结束标志。从GPRMC语句的帧格式可知，接收帧头后，再连续接收38个字节，即可获取经纬度信息。

经纬度信息采集与姿态信息采集类似，也需在字节接收的基础上实现帧接收，区别只是帧格式不同。如图4所示，经纬度帧接收可分为如下两个环节：

(1)帧头匹配：接收六个字节与帧头$GPRMC进行匹配，若匹配成功，进入数据接收环节，否则一直进行帧头匹配；

(2)数据接收：连续接收38个字节，滤除<1>、<2>和逗号这些无关信息，将纬度信息<3>、<4>和经度信息<5>、<6>写入FPGA内部的纬度寄存器Latitude和经度寄存器Longitude，将经纬度中断标志位置为1，发起经纬度中断通知DSP读取经纬度测量值，然后返回帧头匹配环节。

3)大气压信息采集：

大气压信息的采集需要FPGA通过SPI接口主动读取。如图5所示，所述的通过SPI接口采集气压传感器测量的大气压和温度，包括如下步骤：

(1)复位：FPGA向设置在无人直升机上的气压传感器发送复位命令，延时3ms后进入下一步骤；

(2)校正参数读取：FPGA向气压传感器发送校正参数读取命令，然后FPGA读取气压传感器中的校正参数，并存入内部寄存器供DSP读取；

(3)主循环：FPGA先向气压传感器发送大气压转换命令，延时9ms后读取气压传感器中的大气压，并存入内部寄存器Pressure，再向气压传感器发送温度转换命令，延时9ms后读取气压传感器中的温度，并存入内部温度寄存器Temp，然后，将FPGA内大气压中断标志位置1，FPGA发起大气压中断通知DSP读取大气压测量值和温度测量值；

(4)FPGA延时1s的采样周期后返回步骤3)。

(二)机载下位机控制方法中的命令字接收：

地面站PC将用户发布的命令转换为命令字，通过WIFI模块发送给机载上位机，机载上位机再转发给机载下位机。命令字共三种：飞行模式切换命令字，姿态给定命令字，位置给定命令字。命令字的格式详见表1。从表1可知：飞行模式切换命令字仅为1个字节，另两个命令字为5个字节；命令字的第一个字节用于区分不同命令字。

如图6所示，所述的命令字接收包括接收飞行模式切换命令字、姿态给定命令字和位置给定命令字，具体包括如下步骤：

(1)接收命令字第一个字节；

(2)判断所接收字节的高4位是否都为0，若是，说明正在接收的命令字为飞行模式切换命令字，将所接收字节的低4位赋给飞行模式寄存器F_Mode，将Update_Reg[0]置为1，以启动飞行模式上传过程，由于之前已设定飞行模式切换命令字仅含一个字节，所以命令字已接收完毕，返回步骤(1)；若不是，说明正在接收的命令字为姿态给定命令字或位置给定命令字，进入下一步骤；

(3)接收命令字后四个字节；

(4)根据命令字类型将姿态给定值或者位置给定值存入FPGA内特定的寄存器供DSP读取，然后返回步骤(1)。

表1命令字表

(三)机载下位机控制方法中的舵机驱动

所述的舵机驱动是选用FPGA中5路PWM信号输出通道PWMout[0:4]的前两路PWM信号输出通道PWMout[0:1]驱动机载舵机组中的总距舵机和油门舵机，后三路PWM信号输出通道PWMout[2:4]驱动横滚舵机、俯仰舵机和偏航舵机。

不同飞行模式，PWMout[0:4]的信号来源不同。手动模式下，PWMout[0:4]输出的信号与PWMin[0:4]输入的信号一致。此时遥控器控制所有机载舵机，无人直升机的姿态和位置也完全由它控制。姿态模式下，PWMout[0:1]输出的信号与PWMin[0:1]输入的信号一致，PWMout[2:4]输出的信号由DSP的控制算法决定。此时，遥控器控制油门舵机和总距舵机，进而控制无人直升机主旋翼的升力；DSP控制横滚、俯仰和偏航舵机，自主调节无人直升机姿态。位置模式下，PWMout[0:4]输出的信号都由DSP的控制算法决定。此时，DSP控制所有机载舵机，自主调节无人直升机的位置和姿态。

如图7所示，舵机驱动具体包括如下步骤：

(1)将FPGA中的5个计数寄存器(CNT0-CNT4)和5个初值寄存器(CR0-CR5)的初始值设定为1500，重装载寄存器(Re_Load)和周期寄存器(Period)的初始值设定为0；

(2)判断是否经过一微秒的计数周期，是则将计数寄存器中非零的寄存器减1，周期寄存器加1后进入下一步骤，否则直接进入下一步骤；

(3)判断DSP是否将重装载寄存器设为0xFF或者周期寄存器是否大于28000，是则说明DSP更新了初值寄存器或者本周期超过28了毫秒，将计数寄存器中的计数初值装载至计数寄存器，将重装载寄存器和周期寄存器设为0，产生下个周期的PWM信号后进入下一步；否则直接进入下一步；

(4)由飞行模式寄存器(F_Mode)判断飞行模式，飞行模式寄存器等于0为手动模式，进入步骤5)；飞行模式寄存器等于1为姿态模式，进入步骤(6)；飞行模式寄存器等于2为位置模式，进入步骤(7)；

(5)将与5个计数寄存器相对应的5路PWM信号输入通道接收的信号输出在同样与5个计数寄存器相对应的5路PWM信号输出通道上后返回步骤(2)；

(6)将前两路PWM信号输入通道接收的信号输出在前两路PWM信号输出通道上；5个计数寄存器中的后三个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤(2)；

(7)5个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤(2)。

需说明，初值寄存器决定了PWM信号的脉宽，从而决定舵机的动作，因此初值寄存器也可称为舵机控制量寄存器。其中CR0为油门舵机控制量寄存器，CR1为总距舵机控制量寄存器，CR2为横滚舵机控制量寄存器，CR3为俯仰舵机控制量寄存器，CR4为偏航舵机控制量寄存器。DSP将舵机控制量写入舵机控制量寄存器(初值寄存器)，即可控制舵机的动作，实现无人直升机的姿态和位置调节。

PWM信号周期与重装载周期一致。重装载由DSP内姿态控制算法引起。因此PWM信号周期、重装载周期、姿态控制周期和姿态采集周器保持一致，都为22ms。

(四)机载下位机控制方法中的飞行数据上传：

FPGA通过RS232飞行数据上传给机载上位机，机载上位机将其存储后，转发至地面站PC。根据更新周期不同，飞行数据分为五类：无人直升机姿态、无人直升机所处经纬度、无人直升机所处高度、舵机控制量和飞行模式。FPGA内部设置寄存器Update_Reg作为飞行数据的更新标志。Update_Reg[0]为姿态更新标志位，FPGA接收到新姿态帧后将其置位，表明姿态已更新需上传。UPDATE_REG[1]为经纬度更新标志位，Update_Reg[2]为高度更新标志位，Update_Reg[3]为舵机控制量更新标志位。经纬度、高度和舵机控制量的更新与DSP有关，因此这三个标志位只能由DSP置位。Update_Reg[4]为飞行模式更新标志位，FPGA接收到飞行模式切换命令字后，会改变飞行模式寄存器F_Mode，置位该标志位，表示飞行模式已更新需上传。

如图8所示，飞行数据上传具体包括如下步骤：

(1)判断Update_Reg[0]是否为1，是则说明姿态已更新，上传姿态，将Update_Reg[0]置为0，然后转下一步；否则直接进入下一步；

(2)判断Update_Reg[1]是否为1，是则说明经纬度已更新，上传经纬度，将Update_Reg[1]置为0，然后进入下一步；否则直接进入下一步；

(3)判断Update_Reg[2]是否为1，是则说明高度已更新，上传高度，将Update_Reg[2]置为0，然后进入下一步；否则直接进入下一步；

(4)判断Update_Reg[3]是否为1，是则说明舵机控制量已更新，上传舵机控制量，将Update_Reg[3]置为0，然后进入下一步；否则直接进入下一步；

(5)判断Update_Reg[4]是否为1，是则说明飞行模式已更新，上传飞行模式，将Update_Reg[4]置为0，然后进入下一步；否则直接进入下一步。

(五)机载下位机控制方法中的DSP的姿态控制方法：

DSP响应FPGA发起的姿态中断后，会执行姿态控制方法。姿态控制方法执行后，FPGA内姿态中断标志位会被设为0。如图9所示，所述的DSP的姿态控制方法包括如下步骤：

(1)从FPGA读取姿态给定值、姿态测量值和飞行模式；

(2)判断当前飞行模式是否为姿态模式或者位置模式，是则进入下一步，否者直接结束；

(3)将横滚角给定值、俯仰角给定值和偏航角给定值依次与横滚角测量值、俯仰角测量值和偏航角测量值相减，得横滚角偏差roll_error、俯仰角偏差pitch_error和偏航角偏差yaw_error；

(4)计算横滚舵机控制量增量Δu_roll、俯仰舵机控制量增量Δu_pitch和偏航舵机控制量增量Δu_yaw：

Δu_roll＝roll_kp×(roll_error(k)-roll_error(k-1))+roll_ki×roll_error(k)

Δu_pitch＝pitch_kp×(pitch_error(k)-pitch_error(k-1))+pitch_ki×pitch_error(k)

Δu_yaw＝yaw_kp×(yaw_error(k)-yaw_error(k-1))+yaw_ki×yaw_error(k)；

其中roll_error(k)、pitch_error(k)和yaw_error(k)是本次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，roll_error(k-1)、pitch_error(k-1)和yaw_error(k-1)是上次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，roll_kp和roll_ki是横滚角控制的比例增益和积分增益，pitch_kp和pitch_ki是俯仰角控制的比例增益和积分增益，yaw_kp和yaw_ki是偏航角控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准；

(5)累加得横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw：

u_roll＝Δu_roll+u_roll

u_pitch＝Δu_pitch+u_pitch

u_yaw＝Δu_pitch+u_yaw

然后对横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw进行限幅；

(6)将横滚舵机控制量、俯仰舵机控制量和偏航舵机控制量写入FPGA内部横滚舵机控制量寄存器CR2、俯仰舵机控制量寄存器CR3和偏航舵机控制量寄存器CR4；

(7)将FPGA内部重装载寄存器置为0xFF，使写入FPGA的舵机控制量生效。

(六)机载下位机控制方法中的DSP的经纬度控制方法：

DSP响应FPGA发起的经纬度中断后，会执行经纬度控制方法。经纬度控制方法执行后，FPGA内经纬度中断标志位会被设为0。如图10所示，所述的DSP的经纬度控制方法包括如下步骤：

(1)从FPGA读取经纬度给定值、经纬度测量值和飞行模式；

(2)将表示经纬度测量值的字符串解算成对应的浮点数；

(3)将解算出的浮点数写入FPGA，将FPGA内标志位Update_Reg[1]置1，通知FPGA上传经纬度给定值和经纬度测量值；

(4)判断当前飞行模式是否为位置模式，是则进入下一步，否者直接结束；

(5)采用高斯克吕格投影方法，将经纬度给定值和经纬度测量值换算成两点间南北向距离偏差NS_error和东西向距离偏差EW_error；

投影公式为：

NS_error＝[Ay-By*cos(Φ+Φ0)]*(Φ-Φ0)

EW_error＝[Ax*cos(Φ/2+Φ0/2)-Bx*cos(3*Φ/2+3*Φ0/2)]*(λ-λ0)

其中Φ和λ为纬度测量值和经度测量值，Φ0和λ0为纬度给定值和经度给定值。各参数的取值为：

Ax＝111412.876m Ay＝111132.952m Bx＝93.503m By＝559.849m

(6)采用坐标变换，将南北向距离偏差NS_error和东西向距离偏差EW_error换算成机身纵向距离偏差x_error和横向距离偏差y_error；坐标变换公式为：

x_error＝NS_error*cos(Ψ)+EW_error*sin(Ψ)

y_error＝-NS_error*sin(Ψ)+EW_error*cos(Ψ)

其中Ψ为无人直升机的偏航角；

(7)计算横滚角给定增量ΔPhi_G和俯仰角给定增量ΔTheta_G：

ΔPhi_G＝y_kp×(y_error(k)-y_error(k-1))+y_ki×y_error(k)

ΔTheta_G＝x_kp×(x_error(k)-x_error(k-1))+x_ki×x_error(k)；

其中x_error(k)和y_error(k)是本次经纬度控制周期计算出的机身纵向距离偏差和横向距离偏差，x_error(k-1)和y_error(k-1)是上次经纬度控制周期计算出的机身纵向距离偏差和横向距离偏差。x_kp和x_ki是机身纵向距离控制的比例增益和积分增益，y_kp和y_ki是机身横向距离控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准；

(8)累加得横滚角给定值Phi_G和俯仰角给定值Theta_G：

Phi_G＝ΔPhi_G+Phi_G

Theta_G＝ΔTheta_G+Theta_G

然后将Phi_G和Theta_G限幅在-10°和10°之间。

(七)机载下位机控制方法中的DSP的高度控制方法：

DSP响应FPGA发起的大气压中断后，会执行高度控制方法。高度控制方法执行后，FPGA内大气压中断标志位会被设为0。如图11所示，所述的DSP的高度控制方法包括如下步骤：

(1)从FPGA读取大气压测量值、温度测量值、高度给定值和飞行模式；

(2)根据校正参数和温度测量值对大气压测量值进行温度补偿，得到校正大气压，然后利用大气压和海拔高度的关系，将校正大气压解算成高度测量值；

(3)将解算出的高度测量值写入FPGA，将FPGA内标志位Update_Reg[2]置1，通知FPGA上传高度信息；

(4)判断当前飞行模式是否为位置模式，是则进入下一步，否则直接结束；

(5)用高度给定值减去高度测量值得到高度偏差z_error；

(6)计算油门舵机控制量增量Δu_pow：

Δu_pow＝z_kp×(z_error(k)-z_error(k-1))+z_ki×z_error(k)；

其中z_error(k)是本次高度控制周期计算出的高度偏差，z_error(k-1)是上次高度控制周期计算出的高度偏差。z_kp和z_ki是高度控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准。

(7)累加得新油门舵机控制量u_pow＝Δu_pow+u_pow并对u_pow限幅；

(8)DSP内存储了油门舵机控制量和总距舵机控制量的对应关系表，根据油门舵机控制量u_pow查找该表，可得总距舵机控制量u_col；

(9)将油门舵机控制量和总距舵机控制量写入FPGA内部寄存器。

(八)机载上位机控制方法

所述机载上位机控制方法包括有转发飞行数据和命令字，实现飞行数据的存储。机载上位机控制方法包括如下步骤：

(1)设置串口，实现与机载下位机的通讯，转下一步。

(2)创建套接字，绑定IP地址和端口号，实现与地面站PC的通讯。

(3)判断是否收到新飞行数据，是则将其存储并转发至地面站PC，转下一步；否则直接转下一步。

(4)判断是否收到新命令字，是则将其转发至机载下位机，返回步骤3；否则直接返回步骤3)。

(九)地面站PC控制方法

所述的地面站PC控制方法包括飞行数据的接收和显示，以及向机载平台发布飞行命令。如图13所示，地面站PC控制方法包括如下步骤：

(1)创建套接字，绑定IP地址和端口号，实现与机载上位机的通讯。

(2)判断是否收到新飞行数据，是则将其显示给地面操控员。

(3)判断地面操控员是否发布新命令，是则将其转化成命令字后发送给机载上位机，返回步骤(2)；否则直接返回步骤(2)。

Claims (6)

1.一种用于基于DSP和FPGA的无人直升机飞行控制系统的控制方法，所述系统包括有由无人直升机机体(11)、机载舵机组(12)和无线电接收机(13)构成的模型直升机(1)，所述的模型直升机(1)分别连接机载控制平台(2)和地面站监控平台(3)，其中，所述的机载控制平台(2)包括有机载下位机(21)和分别通过RS232接口与所述的机载下位机(21)相连的姿态测量模块(22)、经纬度测量模块(23)和机载上位机(25)，以及通过SPI接口与所述的机载下位机(21)相连的气压传感器(24)，所述的地面站监控平台(3)设置有与所述的无线电接收机(13)无线连接的遥控器(31)和与所述的机载上位机(25)相连的地面站PC(32)，其特征在于，所述的方法包括有机载下位机控制方法、机载上位机控制方法和地面站PC控制方法三部分，其中，所述的机载下位机控制方法包括有用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据上传的控制方法，以及用于DSP的姿态、经纬度和高度的控制方法；所述机载上位机控制方法包括有转发飞行数据和命令字，实现飞行数据的存储；所述的地面站PC控制方法包括飞行数据的接收和显示，以及向机载控制平台发布飞行命令；

所述的机载下位机控制方法中的信息采集是指：FPGA通过串口采集姿态测量模块(22)和经纬度测量模块(23)测量的姿态测量值和经纬度测量值，通过SPI接口采集气压传感器测量的大气压和温度，然后发起中断通知DSP读取采集信息；所述的机载下位机控制方法中的命令字接收是指：FPGA通过串口接收并处理机载上位机转发的命令字；所述的机载下位机控制方法中的舵机驱动是指：FPGA根据飞行模式产生驱动舵机组的PWM信号；所述的机载下位机控制方法中的飞行数据上传是指：FPGA通过串口将飞行数据上传给机载上位机，所述的通过SPI接口采集气压传感器测量的大气压和温度，包括如下步骤：

1)复位：FPGA向设置在无人直升机上的气压传感器发送复位命令，延时3ms后进入下一步骤；

2)校正参数读取：FPGA向气压传感器发送校正参数读取命令，然后FPGA读取气压传感器中的校正参数，并存入内部寄存器供DSP读取；

3)主循环：FPGA先向气压传感器发送大气压转换命令，延时9ms后读取气压传感器中的大气压，并存入内部寄存器，再向气压传感器发送温度转换命令，延时9ms后读取气压传感器中的温度，并存入内部寄存器，然后，将FPGA大气压中断标志位置1，FPGA发起大气压中断通知DSP读取大气压测量值和温度测量值；

4)FPGA延时1s的采样周期后返回步骤3)。

2.根据权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其特征在于，所述的命令字接收包括接收飞行模式切换命令字、姿态给定命令字和位置给定命令字，具体包括如下步骤：

(1)接收命令字第一个字节；

(2)判断所接收字节的高4位是否都为0，若是，说明正在接收的命令字为飞行模式切换命令字，将所接收字节的低4位赋给飞行模式寄存器，上传飞行模式返回步骤(1)；若不是，说明正在接收的命令字为姿态给定命令字或位置给定命令字，进入下一步骤；

(3)接收命令字后四个字节；

(4)根据命令字类型将姿态给定值或者位置给定值存入FPGA内特定的寄存器供DSP读取，然后返回步骤(1)。

3.根据权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其特征在于，所述的舵机驱动是选用FPGA中5路PWM信号输出通道的前两路PWM信号输出通道驱动机载舵机组中的总距舵机和油门舵机，后三路PWM信号输出通道驱动横滚舵机、俯仰舵机和偏航舵机，具体包括如下步骤：

(1)将FPGA中的5个计数寄存器和5个初值寄存器的初始值设定为1500，重装载寄存器和周期寄存器的初始值设定为0；

(2)判断是否经过一微秒的计数周期，是则将计数寄存器中非零的寄存器减1，周期寄存器加1后进入下一步骤，否则直接进入下一步骤；

(3)判断DSP是否将重装载寄存器设为0xFF或者周期寄存器是否大于28000，是则说明DSP更新了初值寄存器或者本周期超过28了毫秒，将计数寄存器中的计数初值装载至计数寄存器，将重装载寄存器和周期寄存器设为0，产生下个周期的PWM信号后进入下一步；否则直接进入下一步；

(4)由飞行模式寄存器判断飞行模式，飞行模式寄存器等于0为手动模式，进入步骤(5)；飞行模式寄存器等于1为姿态模式，进入步骤(6)；飞行模式寄存器等于2为位置模式，进入步骤(7)；

(5)将与5个计数寄存器相对应的5路PWM信号输入通道接收的信号输出在同样与5个计数寄存器相对应的5路PWM信号输出通道上后返回步骤(2)；

(6)将前两路PWM信号输入通道接收的信号输出在前两路PWM信号输出通道上；5个计数寄存器中的后三个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤(2)；

(7)5个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤(2)。

4.根据权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其特征在于，所述的DSP的姿态控制方法包括如下步骤：

(1)从FPGA读取姿态给定值、姿态测量值和飞行模式；

(2)判断当前飞行模式是否为姿态模式或者位置模式，是则进入下一步，否者直接结束；

(3)将横滚角给定值、俯仰角给定值和偏航角给定值依次与横滚角测量值、俯仰角测量值和偏航角测量值相减，得横滚角偏差roll_error、俯仰角偏差pitch_error和偏航角偏差yaw_error；

(4)计算横滚舵机控制量增量Δu_roll、俯仰舵机控制量增量Δu_pitch和偏航舵机控制量增量Δu_yaw：

Δu_roll＝roll_kp×(roll_error(k)-roll_error(k-1))+roll_ki×roll_error(k)

Δu_pitch＝pitch_kp×(pitch_error(k)-pitch_error(k-1))+pitch_ki×pitch_error(k)

Δu_yaw＝yaw_kp×(yaw_error(k)-yaw_error(k-1))+yaw_ki×yaw_error(k)；

其中roll_error(k)、pitch_error(k)和yaw_error(k)是本次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，roll_error(k-1)、pitch_error(k-1)和yaw_error(k-1)是上次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，roll_kp和roll_ki是横滚角控制的比例增益和积分增益，pitch_kp和pitch_ki是俯仰角控制的比例增益和积分增益，yaw_kp和yaw_ki是偏航角控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准；

(5)累加得横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw：

u_roll＝Δu_roll+u_roll

u_pitch＝Δu_pitch+u_pitch

u_yaw＝Δu_pitch+u_yaw

然后对横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw进行限幅；

(6)将横滚舵机控制量、俯仰舵机控制量和偏航舵机控制量写入FPGA内部寄存器；

(7)将FPGA内部重装载寄存器置为0xFF，使写入FPGA的舵机控制量生效。

5.根据权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其特征在于，所述的DSP的经纬度控制方法包括如下步骤：

(1)从FPGA读取经纬度给定值、经纬度测量值和飞行模式；

(2)将表示经纬度测量值的字符串解算成对应的浮点数；

(3)将解算出的浮点数写入FPGA，通知FPGA上传经纬度给定值和经纬度测量值；

(4)判断当前飞行模式是否为位置模式，是则进入下一步，否者直接结束；

(5)采用高斯克吕格投影方法，将经纬度给定值和经纬度测量值换算成两点间南北向距离偏差NS_error和东西向距离偏差EW_error；

(6)采用坐标变换，将南北向距离偏差NS_error和东西向距离偏差EW_error换算成机身纵向距离偏差x_error和横向距离偏差y_error；坐标变换公式为：

x_error＝NS_error*cos(Ψ)+EW_error*sin(Ψ)

y_error＝-NS_error*sin(Ψ)+EW_error*cos(Ψ)

其中Ψ为无人直升机的偏航角；

(7)计算横滚角给定增量ΔPhi_G和俯仰角给定增量ΔTheta_G：

ΔPhi_G＝y_kp×(y_error(k)-y_error(k-1))+y_ki×y_error(k)

ΔTheta_G＝x_kp×(x_error(k)-x_error(k-1))+x_ki×x_error(k)；

其中x_error(k)和y_error(k)是本次经纬度控制周期计算出的机身纵向距离偏差和横向距离偏差，x_error(k-1)和y_error(k-1)是上次经纬度控制周期计算出的机身纵向距离偏差和横向距离偏差，x_kp和x_ki是机身纵向距离控制的比例增益和积分增益，y_kp和y_ki是机身横向距离控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准；

(8)累加得横滚角给定值Phi_G和俯仰角给定值Theta_G：

Phi_G＝ΔPhi_G+Phi_G

Theta_G＝ΔTheta_G+Theta_G

然后将Phi_G和Theta_G限幅在-10°和10°之间。

6.根据权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其特征在于，所述的DSP的高度的控制方法包括如下步骤：

(1)从FPGA读取大气压测量值、温度测量值、高度给定值和飞行模式；

(2)根据校正参数和温度对大气压测量值进行温度补偿，得到校正大气压，然后利用大气压和海拔高度的关系，将校正大气压解算成高度测量值；

(3)将解算出的高度测量值写入FPGA，通知FPGA上传高度给定值和高度测量值；

(4)判断当前飞行模式是否为位置模式，是则进入下一步，否则直接结束；

(5)用高度给定值减去高度测量值得到高度偏差z_error；

(6)计算油门舵机控制量增量Δu_pow：

Δu_pow＝z_kp×(z_error(k)-z_error(k-1))+z_ki×z_error(k)；

其中z_error(k)是本次高度控制周期计算出的高度偏差，z_error(k-1)是上次高度控制周期计算出的高度偏差，z_kp和z_ki是高度控制的比例增益和积分增益，它们的取值通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准，

(7)累加得新油门舵机控制量u_pow＝Δu_pow+u_pow并对u_pow限幅；

(8)根据油门舵机控制量u_pow和总距舵机控制量u_col的对应关系，计算出总距舵机控制量u_col；

(9)将油门舵机控制量和总距舵机控制量写入FPGA内部寄存器。