CN104699110A - 可编程飞行器控制引擎ip核 - Google Patents

Abstract

本发明公开了一种可编程飞行器控制引擎IP核，所述IP核包括：第一片上外设；第一DMA通道；根据飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数生成飞行器各电机控制信号，并经由第二DMA通道和第二片上外设传输至飞行器的统一处理引擎堆；第二DMA通道；第二片上外设；本发明采用超级流水线和超长指令集架构的硬件体系架构，通过硬件方式对飞行器进行姿态和航向控制，专用的优化硬件使得控制延迟大大降低。

Description

可编程飞行器控制引擎IP核

技术领域

本发明涉及飞行器控制领域，具体为一种可编程飞行器控制引擎IP核。

背景技术

飞行器运行时，为了完成所承担的任务,必须具有一定的控制系统，如四旋翼悬停飞行时，需要通过控制系统控制飞行器完成指定方向和指定姿态的飞行。对于飞行器而言，不仅需要进行姿态的控制和修正，还需要进行航向修正等其它控制行为，才能保证飞行器按照期望的方式飞行。

现有技术中的飞行器控制系统，一般采用通用CPU+通用A/D采样+通用D/A输出的架构，这种架构设计简单，且通用CPU和现有编程模型相同。采用这种方式意味着通用CPU需要时刻进行姿态控制，航向控制等操作，对于一般嵌入式处理器而言，这个运算量是十分庞大的，处理器往往无法再分配于其它应用之中，使得一些设计和架构不得不采用两个以上的通用CPU，其中一个用于姿态控制，另一个用于实现机载应用；同时，对于飞行器控制过程，其数据通路具有明显的生产者-消费者的数据特性，现有的通用CPU针对控制密集型应用，无法做到并行处理这些数据流，因而整个系统的处理效率大大降低；现有技术中的姿态控制系统，还有一部分采用通用CPU+硬件PID的设计，这种设计仅能够缓解PID部分的运算压力，对于改善整个飞行器控制系统的作用甚微。

发明内容

本发明针对以上问题的提出，而研制一种可编程飞行器控制引擎IP核。

本发明的技术手段如下：

一种可编程飞行器控制引擎IP核，所述IP核包括：

第一片上外设；所述第一片上外设接收飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数；所述飞行器控制数据至少包括姿态控制数据和航向控制数据；

连接第一片上外设和统一处理引擎堆，用于将飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数传输至统一处理引擎堆的第一DMA通道；

根据飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数生成飞行器各电机控制信号，并经由第二DMA通道和第二片上外设传输至飞行器的统一处理引擎堆；

第二DMA通道；

第二片上外设；

所述统一处理引擎堆包括：

根据飞行器三轴加速度和飞行器三轴角速度数据进行姿态融合计算确定飞行器当前姿态角的第一计算单元；

根据GPS数据和测距数据确定飞行器当前所处经纬度和高度的第二计算单元；

将飞行器控制数据、第一PID参数和第二PID参数传输给第四计算单元的第三计算单元；

连接第一计算单元、第二计算单元和第三计算单元，根据航向控制数据、第二计算单元确定的飞行器当前所处经纬度和高度、以及第一PID参数进行PID运算得出飞行器姿态角，并结合第一计算单元确定的飞行器当前姿态角、以及第二PID参数进行PID运算输出飞行器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量的第四计算单元；

连接第四计算单元，根据第四计算单元输出的飞行器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量，结合飞行器控制数据和飞行器模型生成飞行器各电机控制信号的第五计算单元；

另外，所述IP核还包括：

与飞行器外部设备实现数据交互的通用外设接口；所述统一处理引擎堆与通用外设接口相连接；

与通用外设接口相连接，用于运行用户自定义程序的主处理器；

与通用外设接口、统一处理引擎堆和主处理器相连接的仲裁器；所述仲裁器当主处理器和统一处理引擎堆同时申请访问通用外设接口时，根据预设优先级仲裁主处理器和统一处理引擎堆对通用外设接口的访问权；

另外，所述统一处理引擎堆还包括：

连接第一计算单元、第二计算单元、第三计算单元，用于将飞行器当前姿态角、飞行器当前所处经纬度和高度、飞行器控制数据回传至飞行器控制器的第六计算单元；

进一步地，所述统一处理引擎堆还包括：

用于暂存第一片上外设经由第一DMA通道传输过来的数据的输入寄存器；

用于暂存统一处理引擎堆将通过第二DMA通道输出的数据的输出寄存器；

用于暂存第一PID参数和第二PID参数的控制寄存器；

用于暂存各计算单元之间交互数据的共享数据存储器；

所述输入寄存器、输出寄存器与各计算单元之间通过快速互联总线连接；

第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均具有数据存取部件；

所述控制寄存器和共享数据存储器与各计算单元的数据存取部件直接连接；

进一步地，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄存器、输出总线寄存器、取指译码处理级、多个数据选择器Ⅰ、多个算术逻辑单元Ⅰ、第一执行级和写回级；

取指译码处理级控制程序计数器实现程序的顺序执行和跳转；

取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码，按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存器输出的数据经过数据选择器Ⅰ的选择后进入算术逻辑单元Ⅰ，各算术逻辑单元Ⅰ和第一执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中的至少一个寄存器；

进一步地，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄存器、输出总线寄存器、取指译码处理级、多个数据选择器Ⅰ、多个算术逻辑单元Ⅰ、第一执行级、多个数据选择器Ⅱ、多个算术逻辑单元Ⅱ和写回级；

取指译码处理级控制程序计数器实现程序的顺序执行和跳转；

取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码，按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存器输出的数据经过数据选择器Ⅰ的选择后进入算术逻辑单元Ⅰ，各算术逻辑单元Ⅰ和第一执行级在所述控制信号的控制下，执行运算并输出运算结果后，数据选择器Ⅱ对第一执行级输出的运算结果、上述各寄存器中的至少一个寄存器输出的数据进行选择后进入算术逻辑单元Ⅱ，各算术逻辑单元Ⅱ和第二执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中的至少一个寄存器；

进一步地，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均还包括：

实现PID运算的PID控制寄存器；

提供程序时钟的定时器；

控制第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元暂停或运行的程序同步寄存器；

进一步地，本地寄存器、输入总线寄存器和输出总线寄存器采用双时钟沿触发寄存器；所述双时钟沿触发寄存器包括第一D触发器、第二D触发器和第一选择器；第一D触发器和第二D触发器的输出端均连接第一选择器的输入端；

进一步地，算术逻辑单元Ⅰ和算术逻辑单元Ⅱ均包括：

左寄存器、右寄存器、第二选择器、第三选择器和ALU；第二选择器的一输入端连接左寄存器，另一输入端接收操作数，输出端连接ALU输入端；第三选择器的一输入端连接右寄存器，另一输入端接收操作数，输出端连接ALU输入端。

由于采用了上述技术方案，本发明提供的可编程飞行器控制引擎IP核，本发明采用超级流水线和超长指令集架构的硬件体系架构，通过硬件方式对飞行器进行姿态和航向控制，专用的优化硬件使得控制延迟大大降低，同时可编程的特点使得它具有良好的通用性，克服了现有技术中控制对主控CPU资源占用率过高，不能并行处理数据，控制延迟过大的问题，同时能够弥补现有技术中基于AISC的控制核心可编程性差、通用性不高的缺点。

附图说明

图1是本发明所述IP核的结构示意图；

图2是本发明所述第一DMA通道和第二DMA通道的结构示意图；

图3是本发明所述统一处理引擎堆的结构示意图；

图4是本发明各计算单元之间的连接示意图；

图5是本发明第一计算单元、第二计算单元、第三计算单元、第四计算单元、第五计算单元和第六计算单元各自的内部结构示意图；

图6是本发明所述双时钟沿触发寄存器的结构示意图；

图7是本发明所述算术逻辑单元Ⅰ、算术逻辑单元Ⅱ各自的结构示意图；

图8是本发明第四计算单元和第五计算单元的运算示意图；

图9是本发明滤波运算单元的结构示意图；

图10是本发明滑动窗口滤波器的结构示意图；

图11是本发明监视器的结构示意图；

图12是本发明程序同步寄存器的结构示意图。

图13是本发明快速互联总线的结构示意图。

具体实施方式

如图1、图3、图4、图5、图6、图7和图8所示，一种可编程飞行器控制引擎IP核，所述IP核包括：第一片上外设；所述第一片上外设接收飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数；所述飞行器控制数据至少包括姿态控制数据和航向控制数据；飞行器控制数据、第一PID参数和第二PID参数来自于遥控器和飞行器控制器；连接第一片上外设和统一处理引擎堆，用于将飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数传输至统一处理引擎堆的第一DMA通道；根据飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数生成飞行器各电机控制信号，并将飞行器各电机控制信号经由第二DMA通道和第二片上外设传输至飞行器的统一处理引擎堆；第二DMA通道；第二片上外设；所述统一处理引擎堆包括：根据飞行器三轴加速度和飞行器三轴角速度数据进行姿态融合计算确定飞行器当前姿态角的第一计算单元；根据GPS数据和测距数据确定飞行器当前所处经纬度和高度的第二计算单元；将飞行器控制数据、第一PID参数和第二PID参数传输给第四计算单元的第三计算单元；连接第一计算单元、第二计算单元和第三计算单元，根据航向控制数据、第二计算单元确定的飞行器当前所处经纬度和高度、以及第一PID参数进行PID运算得出飞行器姿态角，并结合第一计算单元确定的飞行器当前姿态角、以及第二PID参数进行PID运算输出飞行器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量的第四计算单元；连接第四计算单元，根据第四计算单元输出的飞行器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量，结合飞行器控制数据和飞行器模型生成飞行器各电机控制信号的第五计算单元；所述飞行器模型可以为四旋翼飞行器模型；四旋翼飞行器是一种实现垂直起降的非共轴式多旋翼飞行器，只需通过调节蝶形分布四个旋翼的转速，实现对飞行姿态的控制。另外，所述IP核还包括：与飞行器外部设备实现数据交互的通用外设接口；所述统一处理引擎堆与通用外设接口相连接；与通用外设接口相连接，用于运行用户自定义程序的主处理器；与通用外设接口、统一处理引擎堆和主处理器相连接的仲裁器；所述仲裁器当主处理器和统一处理引擎堆同时申请访问通用外设接口时，根据预设优先级仲裁主处理器和统一处理引擎堆对通用外设接口的访问权；另外，所述统一处理引擎堆还包括：连接第一计算单元、第二计算单元、第三计算单元，用于将飞行器当前姿态角、飞行器当前所处经纬度和高度、飞行器控制数据回传至飞行器控制器的第六计算单元；进一步地，所述统一处理引擎堆还包括：用于暂存第一片上外设经由第一DMA通道传输过来的数据的输入寄存器；用于暂存统一处理引擎堆将通过第二DMA通道输出的数据的输出寄存器；用于暂存第一PID参数和第二PID参数的控制寄存器；用于暂存各计算单元之间交互数据的共享数据存储器；所述输入寄存器、输出寄存器与各计算单元之间通过快速互联总线连接；第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均具有数据存取部件；所述控制寄存器和共享数据存储器与各计算单元的数据存取部件直接连接；进一步地，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄存器、输出总线寄存器、取指译码处理级、多个数据选择器Ⅰ、多个算术逻辑单元Ⅰ、第一执行级和写回级；取指译码处理级控制程序计数器实现程序的顺序执行和跳转；取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码，按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存器输出的数据经过数据选择器Ⅰ的选择后进入算术逻辑单元Ⅰ，各算术逻辑单元Ⅰ和第一执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中的至少一个寄存器；进一步地，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄存器、输出总线寄存器、取指译码处理级、多个数据选择器Ⅰ、多个算术逻辑单元Ⅰ、第一执行级、多个数据选择器Ⅱ、多个算术逻辑单元Ⅱ和写回级；取指译码处理级控制程序计数器实现程序的顺序执行和跳转；取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码，按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存器输出的数据经过数据选择器Ⅰ的选择后进入算术逻辑单元Ⅰ，各算术逻辑单元Ⅰ和第一执行级在所述控制信号的控制下，执行运算并输出运算结果后，数据选择器Ⅱ对第一执行级输出的运算结果、上述各寄存器中的至少一个寄存器输出的数据进行选择后进入算术逻辑单元Ⅱ，各算术逻辑单元Ⅱ和第二执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中的至少一个寄存器；所述控制信号对输出的数据的寄存器设定、各数据选择器Ⅰ的选择操作、各算术逻辑单元Ⅰ的运算、数据选择器Ⅱ的选择操作、各算术逻辑单元Ⅱ的运算、接收写回数据的寄存器设定等进行控制；进一步地，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均还包括：实现PID运算的PID控制寄存器；提供程序时钟的定时器；控制第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元暂停或运行的程序同步寄存器；进一步地，本地寄存器、输入总线寄存器和输出总线寄存器采用双时钟沿触发寄存器；所述双时钟沿触发寄存器包括第一D触发器、第二D触发器和第一选择器；第一D触发器和第二D触发器的输出端均连接第一选择器的输入端；进一步地，算术逻辑单元Ⅰ和算术逻辑单元Ⅱ均包括：左寄存器、右寄存器、第二选择器、第三选择器和ALU；第二选择器的一输入端连接左寄存器，另一输入端接收操作数，输出端连接ALU输入端；第三选择器的一输入端连接右寄存器，另一输入端接收操作数，输出端连接ALU输入端。所述控制寄存器还用于统一处理引擎堆和主处理器之间的控制数据交互；算术逻辑单元Ⅰ和算术逻辑单元Ⅱ包括的ALU所接收的操作数可以为第一数据选择器或第二数据选择器的输出结果；算术逻辑单元Ⅰ和算术逻辑单元Ⅱ均还包括：第四选择器和第五选择器；ALU的运算结果在送至第一执行级或第二执行级的同时，还送至第四选择器和第五选择器，第四选择器对ALU的运算结果和来自上部ALU的结果数据进行选择，选择后的结果暂存至左寄存器，第五选择器对ALU的运算结果和来自下部ALU的结果数据进行选择，选择后的结果暂存至右寄存器，其中任一ALU的上部ALU和下部ALU由图5中的各算术逻辑单元Ⅰ的上下顺序、或各算术逻辑单元Ⅱ的上下顺序确定，这样便于进行类似乘累加运算时上一周期的计算结果能够立即被下一周期使用或者使编译器编译的时候进行软件流水的相关优化。

本发明输入总线寄存器输出的数据还可以经过滤波器滤波后送至数据选择器Ⅰ，写回级传输的运算结果还可以经过滤波器滤波后送至输出总线寄存器；所述滤波器包括滤波运算单元；图9示出了本发明滤波运算单元的结构示意图；图10示出了本发明滑动窗口滤波器的结构示意图；所述滤波运算单元包括FIR滤波器、IIR数字滤波器和滑动窗口滤波器中的至少一种；通过交叉互联总线的作用，可以将三种滤波器并联或者串联对数据滤波；所述滑动窗口滤波器包括具有8个存储单元的第一FIFO存储器、加法器单元、加法选择器、以及桶形移位寄存器；所述加法器单元用于计算出第一FIFO存储器的前2个存储单元存放的数据之和、前4个存储单元存放的数据之和、以及8个存储单元存放的数据之和；所述加法选择器用于对上述三个数据之和进行选择并输出至桶形移位寄存器，桶形移位寄存器做快速除法处理输出滤波值。

本发明第一计算单元根据飞行器三轴加速度和飞行器三轴角速度数据进行姿态融合计算确定飞行器当前姿态角，具体过程如下：①对三轴加速度数据进行单位化；②根据四元数q0,q1,q2,q3得到四元数中间流q0*q0、q0*q1、q0*q2、q0*q3、q1*q1、q1*q2、q1*q3、q2*q2、q2*q3和q3*q3；③根据三轴加速度、三轴角速度数据，结合四元数中间流得出机身坐标系修正量，具体利用公式 $\left\{\begin{array}{c}{e}_x=(a_y*v_z-a_z*v_y)\\ e_y=(a_z*v_x-a_x*v_z)\\ e_z=(a_x*v_y-a_y*v_x)\end{array}\right.$ 计算出机身坐标系修正量(e_x,e_y,e_z)，其中(a_x,a_y,a_z)为单位化后的三轴加速度数据、 $\left\{\begin{array}{c}{v}_x=2*(q1*q3-q0*q2)\\ v_y=2*(q0*q1+q2*q3)\\ v_z=q0*q0-q1*q1-q2*q2-q3*q3\end{array}\right.;$ ④利用机身坐标系修正量对三轴角速度数据执行比例积分修正，具体为利用公式 $\left\{\begin{array}{c}{u}_x=g_x+K_p*e_x+{e_x}^{\′}\mathrm{Int}\\ u_y=g_y+K_p*e_y+{e_y}^{\′}\mathrm{Int}\\ u_z=g_z+K_p*e_z+{e_z}^{\′}\mathrm{Int}\end{array}\right.$ 得到PI修正后的三轴角速度数据(u_x,u_y,u_z)，其中(g_x,g_y,g_z)为单位化后的三轴角速度数据、K_p为设定的比例系数、(e_x,e_y,e_z)为四元数误差量、 $\left\{\begin{array}{c}{e_x}^{\′}\mathrm{Int}=e_x\mathrm{Int}+e_x*K_i\\ {e_y}^{\′}\mathrm{Int}=e_y\mathrm{Int}+e_y*K_i\\ {e_z}^{\′}\mathrm{Int}=e_z\mathrm{Int}+e_z*K_i\end{array}\right.,$ (e_xInt,e_yInt,e_zInt)为对上一积分周期中的(e_x,e_y,e_z)的各自积分、K_p为设定的积分系数；⑤利用公式 $\left\{\begin{array}{c}{q0}^{\′}={q0}_b+(-{q1}_b*u_x-{q2}_b*u_y-q{3}_b*u_z)*\mathrm{halfT}\\ {q1}^{\′}={q1}_b+({q0}_b*u_x+{q2}_b*u_z-q{3}_b*u_y)*\mathrm{halfT}\\ {q2}^{\′}={q2}_b+({q0}_b*u_y-{q1}_b*u_z+q{3}_b*u_x)*\mathrm{halfT}\\ {q3}^{\′}={q3}_b+({q0}_b*u_z-{q1}_b*u_y-{q2}_b*u_x)*\mathrm{halfT}\end{array}\right.$ 得出当前积分周期的四元数q0′,q1′,q2′,q3′，其中q0_b,q1_b,q2_b,q3_b为上一积分周期的四元数、(u_x,u_y,u_z)为PI修正后的三轴角速度数据、halfT为设定参数；在生成当前积分周期四元数q0′,q1′,q2′,q3′后还对q0′,q1′,q2′,q3′进行单位化并得到q0,q1,q2,q3；⑥利用公式 $\left\{\begin{array}{c}\tan \left(\mathrm{Roll}\right)=\frac{2*(q0*q1+q3*q2)}{(q3*q3+q0*q0-q1*q1-q2*q*)}\\ \sin \left(\mathrm{Pitch}\right)=-2*(q0*q2-q3*q1)\\ \tan \left(\mathrm{Yaw}\right)=\frac{2*(q1*q2+q3*q0)}{(q3*q3-q0*q0-q1*q1+q2*q2)}\end{array}\right.$ 计算出滚转角Roll的正切三角函数值tan(Roll)、俯仰角Pitch的正弦三角函数值sin(Pitch)、偏航角Yaw的正切三角函数值tan(Yaw)，进而确定滚转角Roll、俯仰角Pitch和偏航角Yaw；

本发明通用外设接口包括UART接口、GPIO接口、I²C接口等，它们被安排在主处理器的数据总线上和统一处理引擎堆的数据总线上；第一片上外设和第二片上外设是供给统一处理引擎堆利用的专属输入输出外设；第一片上外设具有A/D转换器和UART输入部分，第二片上外设具有D/A转换器、PWM发生器和UART输出部分，第一片上外设连接加速度传感器、角速度传感器、GPS、测距仪和飞行器控制器，进而接收加速度传感器获取的飞行器三轴加速度、角速度传感器获取的飞行器三轴角速度、GPS获取的GPS数据、测距仪获取的测距数据、飞行器控制数据、以及设定的第一PID参数和第二PID参数，第一PID参数和第二PID参数可通过PID矩阵记载，PID参数包括比例系数、积分系数和微分系数；第一片上外设和第二片上外设还具有与第一DMA通道或第二DMA通道连接的DMA交换接口；第一DMA通道和第二DMA通道在片上外设和统一处理引擎堆之间搬运数据；图2示出了本发明所述第一DMA通道和第二DMA通道的结构示意图，所述第一DMA通道和第二DMA通道均具有n个输入通道、输入端连接n个输入通道的输入选择器、连接输入选择器输出端的第二FIFO存储器、连接第二FIFO存储器的输出选择器、以及连接输出选择器的输出通道。如图2所示，第一DMA通道从多种外设中选择n个有效位和数据搬运到输入通道上，输入选择器选择出一组有效位和数据，当有效位有效时，触发第二FIFO存储器读入数据；同样地，对于输出通道，输出通道的信号标志(是否忙)经过输出选择器给第二FIFO存储器，将忙位处于非有效时，数据由第二FIFO存储器经由输出通道输出至各外设，当忙位处于有效时，置位忙状态标志位并拒绝来自第二DMA通道的数据。

本发明所述输入寄存器、输出寄存器与各计算单元之间通过快速互联总线连接；快速互联总线是整个统一处理引擎堆的链接桥梁，其相关寄存器被映射在主处理器对控制寄存器的RAM区域内，主处理器在系统启动时对其进行编程，通过对快速互联总线的编程，统一处理引擎堆中各个独立的计算单元被依次首尾连接起来组成了一个巨型超级运算流水线，快速互联总线工作原理如下：各个计算单元均通过硬连线与每一个其它计算单元相连，连线的输入输出端均有MOS管控制导通，所述控制寄存器还暂存有快速互联总线连通参数，图13示出了本发明快速互联总线的结构示意图，如图13所示，根据快速互联总线连通参数(图13中的控制位)控制各通道MOS管的开关，进而控制各计算单元的输入寄存器的FIFO通道和输出寄存器的FIFO通道之间的连接，从而实现各计算单元之间选择性互联，达到互联或者级联的目的，依靠此种方法，实现了各个计算单元之间的并行/串行灵活配置。

图5示出了本发明第一计算单元、第二计算单元、第三计算单元、第四计算单元、第五计算单元和第六计算单元各自的内部结构示意图，如图5所示，算术逻辑单元Ⅰ之间还具有传输运算结果的数据通路，同样地，算术逻辑单元Ⅱ之间也具有传输运算结果的数据通路，便于实现软件流水循环展开。

图8示出了本发明第四计算单元和第五计算单元的运算示意图；第四计算单元采用增量数字PID算法进行PID运算；流水线上分别储存最近的三个误差采样值，三个误差采样值通过乘法器与PID参数相乘，并送入加法器得到PID加和数据。

本发明统一处理引擎堆是一个含有多个计算单元的流数据处理集群；每一个计算单元均基于VLIW(超长指令集架构)设计，同时采用灵活的可变长指令码，指令预先被写入到指令存储器中，运行时，指令以64*3位、128*3位被取出解码，每一个计算单元有独立的输入寄存器和输出寄存器，分别接收的来自前一个计算单元的数据和对发送到下一个计算单元的数据进行暂存；为了能够更好地适应飞行器的相关控制，计算单元之间可被设计成非对称的，如一部分计算单元中的各寄存器采用双时钟沿触发寄存器，使之能以更高速逻辑主频运行，一部分计算单元具有独享的辅助外设如PID控制寄存器或滤波器，一部分计算单元具有更宽的执行宽度(6并行)，还有一部分则具有两级执行流水线，上述非对称设计取决于用户对计算单元的设定。

计算单元中的本地寄存器、输入总线寄存器和输出总线寄存器采用双时钟沿触发寄存器，图6示出了本发明所述双时钟沿触发寄存器的结构示意图，具体采用了两个D触发器和第一选择器，第一D触发器和第二D触发器的D输入端相互连接作为双时钟沿触发寄存器的D输入端，第一D触发器和第二D触发器的CP输入端相互连接作为双时钟沿触发寄存器的CP输入端，第一选择器的输出端作为双时钟沿触发寄存器的Q输出端；双时钟沿触发寄存器能够同时对上升沿时钟和下降沿时钟敏感，因此在同等时钟下单周期可以完成2倍的任务，可以使得某个任务量大的计算单元具有比其它计算单元更快的响应能力。

本发明所述控制寄存器还用于统一处理引擎堆和主处理器之间的控制数据交互；具体地，控制寄存器由两个具有同时读写的总线，其中一个来自主处理器，另一个来自统一处理引擎堆，它的内部寄存器可以被同时读和写，仲裁器还对此时哪条总线信号有效进行判断，所有计算单元均被连接到来自统一处理引擎堆的这个总线上。

统一处理引擎堆包括的各寄存器还可以连接有监视器，所述寄存器可以为控制寄存器和各计算单元包括的本地寄存器，图11示出了本发明监视器的结构示意图；当某一监视数据即寄存器中暂存的数据出现异常值，或者前后两次数据相差极大，中断电平会产生，根据中断掩码，对应中断被选择性输入至主处理器；具体地，所述监视器包括复制具体寄存器暂存内容的一级数据缓冲寄存器、连接一级数据缓冲寄存器的二级数据缓冲寄存器、连接一级数据缓冲寄存器和接收参考限值的第一比较单元、连接一级数据缓冲寄存器和二级数据缓冲寄存器的第二比较单元、以及连接第一比较单元和第二比较单元的第一结果选择器；所述第一比较单元用于将一级数据缓冲寄存器中存放的当前数据与参考限值进行比较，并在当前数据超过参考限值时输出异常信号；所述第二比较单元用于将二级数据缓冲寄存器中存放的当前数据的上一数据与一级数据缓冲寄存器中存放的当前数据进行比较，在当前数据与其上一数据的差值超出一定范围时第二比较单元输出异常信号；所述第一结果选择器对上述两个异常信号进行选择并发送给中断事件生成器；所述主处理器连接中断事件生成器，用于接收中断事件生成器发出的中断信号。

本发明程序同步寄存器控制第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元暂停或运行，本发明各计算单元(第一计算单元、第二计算单元、第三计算单元、第四计算单元、第五计算单元、第六计算单元)的流水线运作状态由各自包括的程序同步寄存器设定，若程序同步寄存器给出暂停逻辑，则相应计算单元的流水线进入停止运行状态，直到程序同步寄存器给出继续运行逻辑。图12示出了本发明程序同步寄存器的结构示意图，如图12所示，具体地，每个计算单元包括的程序同步寄存器收集了其余计算单元的输入寄存器或输出寄存器各个FIFO通道的满空状态，通过1表示FIFO通道为满状态，通过0表示FIFO通道为空状态，并通过屏蔽位表示其它计算单元与当前计算单元相连接的状态，例如假设第一计算单元的输出寄存器的第一FIFO通道和第二FIFO通道，对应与第三计算单元的输入寄存器的第三FIFO通道，第四计算单元的输入寄存器的第一FIFO通道相连接，则第一计算单元将各屏蔽位具体设置为：第三计算单元的输入寄存器的第三FIFO通道对应的屏蔽位为1，第四计算单元的输入寄存器的第一FIFO通道对应的屏蔽位为1，其余屏蔽位设置为0，当第三计算单元的输入寄存器的第三FIFO通道为满状态、和/或第四计算单元的输入寄存器的第一FIFO通道为满状态，则将这两个满状态的信号与各自对应的屏蔽位执行与操作，若这两个通道均为满状态，则得到与操作结果为1,1；然后再对多个与操作结果执行或操作，在这里会得到或操作结果1，则第一计算单元的程序同步寄存器控制第一计算单元暂停，若这两个通道均未达到满状态时，则得到与操作结果为0,0；此时经过或操作得到结果为0，则程序同步寄存器使第一计算单元继续运行。

本发明各计算单元具有的数据存取部件(Load/Store部件)用于各计算单元读写控制寄存器和共享数据存储器，本发明数据存取部件(Load/Store部件)含有8个内部寄存器，具体为：读数据地址寄存器、读数据数据寄存器、读数据数据有效寄存器、读数据开始寄存器；写数据地址寄存器、写数据数据寄存器、写数据成功寄存器和写数据开始寄存器；向数据存取部件(Load/Store部件)中的寄存器读写数据是单周期的，当读取控制寄存器和共享数据存储器中的数据时，计算单元向数据存取部件中的读数据地址寄存器写入数据，同时将读数据开始寄存器置位，然后读取读数据数据有效寄存器直到内容变成1，此时读数据数据寄存器的内容即为对应控制寄存器和共享数据存储器中的数据；写数据时同理，向写数据地址寄存器、写数据数据寄存器写入数据，而后置位写数据开始寄存器，当读取写数据成功寄存器内容为1时，写入成功。

本发明输入寄存器被分为三个寄存器区，每个区储存来自DMA通道输送的数据，输入寄存器的三个输出通道被通过互连总线的方式连接在所有计算单元的输入总线上，通过控制寄存器决定它实际被哪个运算单元所使用；输出寄存器也被分为三个寄存器区，输出寄存器的三个输出通道被通过互连总线的方式连接在所有计算单元的输出总线上，通过控制寄存器决定它实际被哪个计算单元所使用；计算单元以环形结构围成一个正方形排布在一起，中间通过互联总线连接。

指令存储器在输入指令地址后，输出所得指令，并根据地址线的第一位的逻辑值决定输出64*3位或128*3位指令；本地寄存器可以一共有32个64位的，为计算单元提供通用的储存位置，输入总线寄存器可以一共有三个，分别接受计算单元三个输入总线上的数据，同理输出总线寄存器可以一共三个，分别向三个输出总线上发送数据；PID控制寄存器被接入计算单元的写回级上，可以被指令改写；定时器由一个加法器和一个寄存器组成，其寄存器能够被计算单元的指令改写、提供简单的计时功能；取指译码处理级内含程序计数器，每周期取指译码处理级从指令存储器读出指令，并根据地址线的第一位的逻辑值增加程序计数器的值，取指译码处理级同时送给指令解码逻辑，解码逻辑将数据选择器Ⅰ选择从各个寄存器中进入的数据并输入到EX1执行级即第一执行级中，在EX1执行级中，数据按照指令定义的内容经过算术逻辑运算，EX1级的计算结果将被输出，并与经过数据选择器Ⅱ的数据共同输入到下一级算术逻辑运算中，在EX2执行级即第二执行级中，数据被按照指令执行了第二个运算操作，而后被输送到写回级上，在写回级上，根据指令的内容，数据将被写回到指定的寄存器；计算单元使用的算术逻辑单元经过了特殊优化，不仅能够完成普通的ALU整数计算功能，还可以内置了一个预先存储各角度与对应各角度的正切三角函数值和正弦三角函数值的查找表的ROM，故它可以快速将输入的tan或者sin函数的值转化成角度输出，除此之外，每个算术逻辑单元配备了两个本地寄存器，称为左寄存器和右寄存器，进一步地，可通过指令控制选择使用左右寄存器的数据还是来自数据选择器输出的数据，同时也可通过指令控制是否将数据写回左右寄存器，这里的数据可以来自本ALU的结果，也可以来自本ALU的上部ALU或下部ALU的计算结果；根据用户的配置，滤波器、第二执行级、流水线的算术逻辑单元执行宽度均可在IP核中进行配置，用户可以自行决定使用功能更加强大，执行带宽更高的计算单元或者是使用更精简的配置。

本发明提供的可编程飞行器控制引擎IP核，本发明采用超级流水线和超长指令集架构的硬件体系架构，通过硬件方式对飞行器进行姿态和航向控制，专用的优化硬件使得控制延迟大大降低，同时可编程的特点使得它具有良好的通用性，克服了现有技术中控制对主控CPU资源占用率过高，不能并行处理数据，控制延迟过大的问题，同时能够弥补现有技术中基于AISC的控制核心可编程性差、通用性不高的缺点。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (9)

1.一种可编程飞行器控制引擎IP核，其特征在于，所述IP核包括：

第一片上外设；所述第一片上外设接收飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数；所述飞行器控制数据至少包括姿态控制数据和航向控制数据；

连接第一片上外设和统一处理引擎堆，用于将飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数传输至统一处理引擎堆的第一DMA通道；

根据飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数生成飞行器各电机控制信号，并经由第二DMA通道和第二片上外设传输至飞行器的统一处理引擎堆；

第二DMA通道；

第二片上外设；

所述统一处理引擎堆包括：

根据飞行器三轴加速度和飞行器三轴角速度数据进行姿态融合计算确定飞行器当前姿态角的第一计算单元；

根据GPS数据和测距数据确定飞行器当前所处经纬度和高度的第二计算单元；

将飞行器控制数据、第一PID参数和第二PID参数传输给第四计算单元的第三计算单元；

连接第一计算单元、第二计算单元和第三计算单元，根据航向控制数据、第二计算单元确定的飞行器当前所处经纬度和高度、以及第一PID参数进行PID运算得出飞行器姿态角，并结合第一计算单元确定的飞行器当前姿态角、以及第二PID参数进行PID运算输出飞行器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量的第四计算单元；

连接第四计算单元，根据第四计算单元输出的飞行器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量，结合飞行器控制数据和飞行器模型生成飞行器各电机控制信号的第五计算单元。

2.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，所述IP核还包括：

与飞行器外部设备实现数据交互的通用外设接口；所述统一处理引擎堆与通用外设接口相连接；

与通用外设接口相连接，用于运行用户自定义程序的主处理器；

与通用外设接口、统一处理引擎堆和主处理器相连接的仲裁器；所述仲裁器当主处理器和统一处理引擎堆同时申请访问通用外设接口时，根据预设优先级仲裁主处理器和统一处理引擎堆对通用外设接口的访问权。

3.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，所述统一处理引擎堆还包括：

连接第一计算单元、第二计算单元、第三计算单元，用于将飞行器当前姿态角、飞行器当前所处经纬度和高度、飞行器控制数据回传至飞行器控制器的第六计算单元。

4.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，所述统一处理引擎堆还包括：

用于暂存第一片上外设经由第一DMA通道传输过来的数据的输入寄存器；

用于暂存统一处理引擎堆将通过第二DMA通道输出的数据的输出寄存器；

用于暂存第一PID参数和第二PID参数的控制寄存器；

用于暂存各计算单元之间交互数据的共享数据存储器；

所述输入寄存器、输出寄存器与各计算单元之间通过快速互联总线连接；

第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均具有数据存取部件；

所述控制寄存器和共享数据存储器与各计算单元的数据存取部件直接连接。

5.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄存器、输出总线寄存器、取指译码处理级、多个数据选择器Ⅰ、多个算术逻辑单元Ⅰ、第一执行级和写回级；

取指译码处理级控制程序计数器实现程序的顺序执行和跳转；

取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码，按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存器输出的数据经过数据选择器Ⅰ的选择后进入算术逻辑单元Ⅰ，各算术逻辑单元Ⅰ和第一执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中的至少一个寄存器。

6.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄存器、输出总线寄存器、取指译码处理级、多个数据选择器Ⅰ、多个算术逻辑单元Ⅰ、第一执行级、多个数据选择器Ⅱ、多个算术逻辑单元Ⅱ和写回级；

取指译码处理级控制程序计数器实现程序的顺序执行和跳转；

取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码，按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存器输出的数据经过数据选择器Ⅰ的选择后进入算术逻辑单元Ⅰ，各算术逻辑单元Ⅰ和第一执行级在所述控制信号的控制下，执行运算并输出运算结果后，数据选择器Ⅱ对第一执行级输出的运算结果、上述各寄存器中的至少一个寄存器输出的数据进行选择后进入算术逻辑单元Ⅱ，各算术逻辑单元Ⅱ和第二执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中的至少一个寄存器。

7.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均还包括：

实现PID运算的PID控制寄存器；

提供程序时钟的定时器；

控制第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元暂停或运行的程序同步寄存器。

8.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，本地寄存器、输入总线寄存器和输出总线寄存器采用双时钟沿触发寄存器；所述双时钟沿触发寄存器包括第一D触发器、第二D触发器和第一选择器；第一D触发器和第二D触发器的输出端均连接第一选择器的输入端。

9.根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，算术逻辑单元Ⅰ和算术逻辑单元Ⅱ均包括：

左寄存器、右寄存器、第二选择器、第三选择器和ALU；第二选择器的一输入端连接左寄存器，另一输入端接收操作数，输出端连接ALU输入端；第三选择器的一输入端连接右寄存器，另一输入端接收操作数，输出端连接ALU输入端。