波后送至数据选择器I， 写回级传输的运算结果还可以经过滤波器滤波后送至输出总线寄存器；所述滤波器包括滤 波运算单元；图9示出了本发明滤波运算单元的结构示意图；图10示出了本发明滑动窗口 滤波器的结构示意图；所述滤波运算单元包括FIR滤波器、IIR数字滤波器和滑动窗口滤波 器中的至少一种；通过交叉互联总线的作用，可以将三种滤波器并联或者串联对数据滤波； 所述滑动窗口滤波器包括具有8个存储单元的第一 FIFO存储器、加法器单元、加法选择器、 以及桶形移位寄存器；所述加法器单元用于计算出第一 FIFO存储器的前2个存储单元存放 的数据之和、前4个存储单元存放的数据之和、以及8个存储单元存放的数据之和；所述加 法选择器用于对上述三个数据之和进行选择并输出至桶形移位寄存器，桶形移位寄存器做 快速除法处理输出滤波值。
[0061] 本发明第一计算单元根据飞行器三轴加速度和飞行器三轴角速度数据进行 姿态融合计算确定飞行器当前姿态角，具体过程如下：①对三轴加速度数据进行单 位化；②根据四元数q〇, ql, q2, q3得到四元数中间流q0*q0、q0*ql、q0*q2、q0*q3、 ql*ql、ql*q2、ql*q3、q2*q2、q2*q3和q3*q3 ;③根据三轴加速度、三轴角速度数据，
【主权项】
1. 一种可编程飞行器控制引擎IP核，其特征在于，所述IP核包括： 第一片上外设；所述第一片上外设接收飞行器三轴加速度、飞行器三轴角速度、GPS数 据、测距数据、飞行器控制数据、第一PID参数和第二PID参数；所述飞行器控制数据至少包 括姿态控制数据和航向控制数据； 连接第一片上外设和统一处理引擎堆，用于将飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数传输至统一处理引擎堆 的第一DM通道； 根据飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第 一PID参数和第二PID参数生成飞行器各电机控制信号，并经由第二DMA通道和第二片上 外设传输至飞行器的统一处理引擎堆； 第二DM通道； 第二片上外设； 所述统一处理引擎堆包括： 根据飞行器三轴加速度和飞行器三轴角速度数据进行姿态融合计算确定飞行器当前 姿态角的第一计算单元； 根据GPS数据和测距数据确定飞行器当前所处经炜度和高度的第二计算单元； 将飞行器控制数据、第一PID参数和第二PID参数传输给第四计算单元的第三计算单 元； 连接第一计算单元、第二计算单元和第三计算单元，根据航向控制数据、第二计算单元 确定的飞行器当前所处经炜度和高度、以及第一PID参数进行PID运算得出飞行器姿态角， 并结合第一计算单元确定的飞行器当前姿态角、以及第二PID参数进行PID运算输出飞行 器的垂直速度控制量、翻滚输入控制量、俯仰控制量和偏航控制量的第四计算单元； 连接第四计算单元，根据第四计算单元输出的飞行器的垂直速度控制量、翻滚输入控 制量、俯仰控制量和偏航控制量，结合飞行器控制数据和飞行器模型生成飞行器各电机控 制信号的第五计算单元。
2. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，所述IP核还包 括： 与飞行器外部设备实现数据交互的通用外设接口；所述统一处理引擎堆与通用外设接 口相连接； 与通用外设接口相连接，用于运行用户自定义程序的主处理器； 与通用外设接口、统一处理引擎堆和主处理器相连接的仲裁器；所述仲裁器当主处理 器和统一处理引擎堆同时申请访问通用外设接口时，根据预设优先级仲裁主处理器和统一 处理引擎堆对通用外设接口的访问权。
3. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，所述统一处理引 擎堆还包括： 连接第一计算单元、第二计算单元、第三计算单元，用于将飞行器当前姿态角、飞行器 当前所处经炜度和高度、飞行器控制数据回传至飞行器控制器的第六计算单元。
4. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，所述统一处理引 擎堆还包括： 用于暂存第一片上外设经由第一DM通道传输过来的数据的输入寄存器； 用于暂存统一处理引擎堆将通过第二DM通道输出的数据的输出寄存器； 用于暂存第一PID参数和第二PID参数的控制寄存器； 用于暂存各计算单元之间交互数据的共享数据存储器； 所述输入寄存器、输出寄存器与各计算单元之间通过快速互联总线连接； 第一计算单元、第二计算单元、第三计算单元、第四计算单元和第五计算单元均具有数 据存取部件； 所述控制寄存器和共享数据存储器与各计算单元的数据存取部件直接连接。
5. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，第一计算单元、 第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包 括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄 存器、输出总线寄存器、取指译码处理级、多个数据选择器I、多个算术逻辑单元I、第一执 行级和写回级； 取指译码处理级控制程序计数器实现程序的顺序执行和跳转； 取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码， 按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算 单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存 器输出的数据经过数据选择器I的选择后进入算术逻辑单元I，各算术逻辑单元I和第一 执行级执行运算并向写回级输出运算结果，该运算结果通过写回级传输到上述各寄存器中 的至少一个寄存器。
6. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，第一计算单元、 第二计算单元、第三计算单元、第四计算单元和第五计算单元采用超长指令集架构，并均包 括：指令存储器、程序计数器、为各计算单元提供通用存储位置的本地寄存器、输入总线寄 存器、输出总线寄存器、取指译码处理级、多个数据选择器I、多个算术逻辑单元I、第一执 行级、多个数据选择器II、多个算术逻辑单元II和写回级； 取指译码处理级控制程序计数器实现程序的顺序执行和跳转； 取指译码处理级依据指令地址从指令存储器中取出指令，并对取出的指令进行译码， 按照超长指令集的指令格式译出控制信号；在控制信号的控制下，第一计算单元、第二计算 单元、第三计算单元、第四计算单元或第五计算单元包括的上述各寄存器的至少一个寄存 器输出的数据经过数据选择器I的选择后进入算术逻辑单元I，各算术逻辑单元I和第一 执行级在所述控制信号的控制下，执行运算并输出运算结果后，数据选择器II对第一执行 级输出的运算结果、上述各寄存器中的至少一个寄存器输出的数据进行选择后进入算术逻 辑单元II，各算术逻辑单元II和第二执行级执行运算并向写回级输出运算结果，该运算结 果通过写回级传输到上述各寄存器中的至少一个寄存器。
7. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，第一计算单元、第 二计算单元、第三计算单元、第四计算单元和第五计算单元均还包括： 实现PID运算的PID控制寄存器； 提供程序时钟的定时器； 控制第一计算单元、第二计算单元、第三计算单元、第四计算单元或第五计算单元暂停 或运行的程序同步寄存器。
8. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，本地寄存器、输入 总线寄存器和输出总线寄存器采用双时钟沿触发寄存器；所述双时钟沿触发寄存器包括第 一D触发器、第二D触发器和第一选择器；第一D触发器和第二D触发器的输出端均连接第 一选择器的输入端。
9. 根据权利要求1所述的可编程飞行器控制引擎IP核，其特征在于，算术逻辑单元I 和算术逻辑单元II均包括： 左寄存器、右寄存器、第二选择器、第三选择器和ALU;第二选择器的一输入端连接左 寄存器，另一输入端接收操作数，输出端连接ALU输入端；第三选择器的一输入端连接右寄 存器，另一输入端接收操作数，输出端连接ALU输入端。
【专利摘要】本发明公开了一种可编程飞行器控制引擎IP核，所述IP核包括：第一片上外设；第一DMA通道；根据飞行器三轴加速度、飞行器三轴角速度、GPS数据、测距数据、飞行器控制数据、第一PID参数和第二PID参数生成飞行器各电机控制信号，并经由第二DMA通道和第二片上外设传输至飞行器的统一处理引擎堆；第二DMA通道；第二片上外设；本发明采用超级流水线和超长指令集架构的硬件体系架构，通过硬件方式对飞行器进行姿态和航向控制，专用的优化硬件使得控制延迟大大降低。
【IPC分类】G05D1-10
【公开号】CN104699110
【申请号】CN201510066687
【发明人】夏广庆, 赵楠, 吕睿, 李辉, 王晓彤, 吴志刚
【申请人】大连理工大学
【公开日】2015年6月10日
【申请日】2015年2月5日