一类无人机嵌入式飞行控制系统软件设计方法

一类无人机嵌入式飞行控制系统软件方法 摘 要:目前，小型无人机飞行控制多采用前后台系统实现。针对前后台系统功能简单，实 时性差等缺点，在uC/OS-II操作系统下，对以ARM处理器为核心的嵌入式小型无人机飞行 控制系统软件进行了完整设计。首先阐述了系统的基本原理并引入实时内核，接着对系统任 务进行了划分，并对任务的调度管理和通信机制给出了详细设计和分析，最后通过地面测试 和试飞实验对所设计的系统软件进行了验证。验证明，该系统软件符合飞控系统设计。 关键词:无人机;飞行控制系统; uC/OS-II;消息邮箱 A method of UAV embedded flight control system design Abstract: At present, most flight control systems for suav adopt fore-background system. As fore- background system’s simple function and weak real-time, an embedded suav flight control system is designed based on ARM core and uC/OS-II operating system. First, the basic system principle and real time kernel are elaborated. Then, the system tasks are divided and task schedule and communication mechanisms are analyzed in detail. Last, the design is verified by ground test and flight experiment. The result shows, this design meets the flight control system requirement. Key words: SUAV; FCS; uC/OS-II; mailbox 0 引言 小型无人机的飞行控制系统是一个多任务系统，功能较为复杂，同时要求具有很强的实时性和较高的可靠性。目前，大多数飞控软件仍采用前后台系统，后台程序是一个无限循环，前台则通过中断来处理事件。由于没有操作系统，前后台系统在结构和功能简单的嵌入式系统中有很广泛地应用。但前后台系统的实时性不高，任务级响应时间在最坏情况下将取决于 [1]整个循环的执行时间。uC/OS-II是具有高实时性的操作系统内核，且简单高效，可移植性好。故本文采用uC/OS-II操作系统来对一类小型无人机飞行控制系统软件进行设计。 1 uC/OS-II内核 uC/OS-II是规模最小的嵌入式操作系统软件之一，是Jean J. Labrosse在1992年推出的uC/OS系统内核的第二个版本。其源码公开且移植性好，是基于优先级的可剥夺型内核，系统中所有任务都有一个唯一的优先级别，适合于实时性要求较高的场合，且内核较小，具有可裁剪性，可固化到嵌入式产品中，具有稳定性和可靠性。 2 系统总体设计 2.1 系统结构 为使无人机在飞行过程中能按照预定的航点飞行，应对无人机的姿态进行实时测量与控制，为此需要获取无人机的实时参数信息，经解算后，输出到各路任务舵机进行控制。同时，飞行控制计算机可通过CAN总线收发与导航模块及地面站的通信数据，获取相应的航线信 [2-3]息和控制命令信息，同时地面站也可实时显示无人机的飞行状态信息。该型飞行控制系统的核心部件采用一块ARM芯片，系统的原理框图如图1所示: “十一五”XX基础科研项目(A212006XXX);罗伟(1987-)， 男，重庆市忠县人，硕士，研究方向为嵌入式飞行控制技术;吴森堂(1963-)，男，山东平度人，教授，博士生导师，研究方向为飞行器信息处理与控制等。 导航模陀螺块和 AD CAN地面站飞行 采 控制 集加速度计计算机 ARM 各路任电子罗盘务舵机 图1 系统原理框图 2.2 功能模块 一套完整的飞控系统软件应该具有如下功能: 1)飞行模式切换功能，无人机有不同的飞行方式，并且在起飞和降落过程中也应设有相应的模式。 2)数据采集功能，要能够采集陀螺，加计等传感器和各路任务舵机的信号。 3)姿态解算与控制计算功能，根据传感器信号，解算出当前无人机的飞行姿态，并由目标位置或姿态，形成相应的控制量。 4)与导航及地面站的通信功能，接收GPS等导航信息，航点信息以及地面站控制命令，同时回传当前无人机的飞行信息。 5)控制输出功能，将控制量信息输出至各路任务舵机，驱动舵机转动。 该型飞行控制系统的软件流程如图2所示: 初始化 飞行模式切换 传感器数据采集 捷联姿态解算 无人机控制计算 各路任务舵机输出 接收导航信息 接收地面站信息 向地面站发送数据 N结束 Y 退出 图2 系统软件流程图 3 系统任务划分与优先级确定 uC/OS-II总共可管理64个任务，除保留任务外，用户应用程序可使用的任务最多为56个。每个任务都具有不同的优先级，uC/OS-II采用优先级调度策略，总是先运行就绪状态下的拥有最高优先级的任务。在飞控软件中，根据软件流程图和系统工作原理，可将系统划分为如下任务: 1)系统起始任务TaskStartup()。起始任务初始化系统时钟，并创建其他几个任务，同时进行设备参数初始化。主要包括定时器，电子罗盘，AD采样，存储芯片，CAN总线，输出舵机控制初始化和捷联解算初始化。初始化任务具有最高优先级0，当系统初始化完毕后，调用函数OSTaskDel()删除任务，让其处于休眠状态。系统示意性代码如下: void TaskStartup(void *pdata) { OS_ENTER_CRITICAL(); //关中断 //安装uC/OS-II时钟中断向量 PC_VectSet(0x08 , OSTickISR); //设置uC/OS-II时钟频率 PC_SetTickRate(OS_TICKS_PER_SEC); OS_EXIT_CRITICAL(); //开中断 OSStatInit(); //初始化统计任务 CreateOtherTasks(); //创建其他任务 //硬件参数初始化 TimerInit(); //定时器 CompassInit(); //罗盘 ADInit(); //AD采集 FlashInit (); // Flash存储 CANInit(); //CAN总线 ServoInit(); //舵机控制 StrapdownInit();//捷联解算 //删除自身任务 OSTaskDel(OS_PRIO_SELF); } 在CreateOtherTasks()函数中创建其他任务。用函数OSTaskCreate()创建的任务包括: AD 采集任务 TaskADC()，数据通信任务 TaskDataComm()，姿态解算与控制计算任务TaskCal() 和执行机构输出任务 TaskServoOut()。 2)AD数据采集任务TaskADC()。采集任务包括空间坐标各方向上陀螺的角速度，加 计的加速度数据。采集任务需要用到CPU较频繁，将其优先级置低，定义为4。任务示意 性代码为: void TaskADC(void* pdata) { INT8U t; //循环计数变量t INT16U GyroX, GyroY, GyroZ; INT16U AccelX, AccelY; //依次采集陀螺仪和加速度计各通道数据 t= (t+1) %5; if(t==0) GyroX=ADCal (GX_CH); else if(t==1) GyroY=ADCal (GY_CH); else if(t==2) GyroZ=ADCal (GZ_CH); else if(t==3) AccelX=ADCal(AX_CH); else if(t==4) AccelY=ADCal(AY_CH); } 3)姿态解算与控制计算任务TaskCal()。首先对传感器的采集值进行FIR滤波，然后采用旋转四元数法计算无人机当前姿态信息，并采用PID控制算法对各回路进行控制。由于解算任务需要用到大量CPU时间，可考虑将优先级置低，设置为3。任务的示意性代码为: void TaskCal(void*pdata) { Temp=FIRCal(INT8U temp, INT8U *p, INT8U len) //滤波计算 FP64 q[4]; //定义四元数 //俯仰角Pitch，滚转角Roll，偏航角Yaw Pitch = asin( -2*(q[3]*q[1]-q[0]*q[2]) ) *180/PI; Roll = atan2( 2*(q[2]*q[3] + q[0]*q[1]), 1-2*(q[1]*q[1]+ q[2]*q[2]) ) *180/PI ; Yaw = atan2( 2*(q[1]*q[2] + q[0]*q[3]), 1-2*(q[2]*q[2] + q[3]*q[3]) ) *180/PI; //系统各回路均采用PID控制 PID(); } 4)数据通信任务TaskDataComm()。数据通信包括串口和CAN总线的数据收发。 UART串口可用来接收电子罗盘数据，CAN总线负责与地面站和导航部分通信。通信任务处理数据收发且要处置串口等异步事件，将其优先级设为次高级别2。 void TaskDataComm(void*pdata) { //UART接收罗盘数据 temp=U0RBR; while(U0BPCANData; } RecvCAN(INT8U SourAddr, structRvBuf * RvBufP) { SourAddr=CANFrameAddr; RvBufP->CANData=CANFrameData; } } 5)任务舵机输出TaskServoOut()。该任务是飞控系统的执行机构，经控制计算机解算后，各通道输出相应占空比的PWM波，驱动舵机旋转相应角度，产生舵偏。该任务占用的CPU时间频率相对较低，且出于安全考虑，一旦出现故障，可由遥控器直接控制舵机的舵量输出，因此将优先级设为1。起始任务删除后，此任务的优先级最高。示意性代码: void TaskServoOut() { //输出PWM方波驱动各路任务舵机 PWMOut(INT8U CHA, INT8U DutyInt); } 4 任务调度与通信 初始任务完成并删除自身后，其余4个任务相继运行。本文任务间的通信采用消息邮箱 [4]机制。任务TaskADC()将采集到的数据发送给任务TaskCal()计算处理， 任务TaskDataComm()也将导航数据，地面站命令和串口数据发送给任务TaskCal()。任务TaskCal()还将当前状态信息发送给任务TaskDataComm()在地面站中显示相应状态，同时还将计算的控制信息发送给任务TaskServoOut()驱动舵机产生舵偏。任务TaskDataComm()和任务TaskCal()有一个相互收发数据的环节，若将两者结合为一个任务，可以少用2个消息邮箱，但此时该任务变得很复杂，CPU执行该长任务耗时增加，实时性将有所降低，故将长任务 分割加以简单的消息邮箱通信来提高实时性。任务间的通信如图3所示: MBoxDataCal1 TaskADCTaskCal 优先级:4优先级:3 MBoxDataCal2MBoxDataExe TaskDataCommTaskServoOutMboxDataBack优先级:2优先级:1 图3 任务间的通信 在Main函数中，用函数OSMboxCreate()建立邮箱:MBoxDataCal1用于发送AD采集数据;MBoxDataCal2发送高优先级的命令字及电子罗盘数据;MBoxDataBack回传信息给地面监控站;MBoxDataExe发送控制输出给高优先级的任务执行机构。消息邮箱用函数OSMboxPost()发送，函数OSMboxPend()等待接收，等待时限为任务运行的最大时间。 系统运行过程分析:初始化完成后，运行优先级最高的任务TaskServoOut()，查询邮箱MBoxDataExe，因为此时消息邮箱为空，故任务为等待态，并根据其他任务正常运行的最大时间设定等待时间。此时任务TaskDataComm()得以运行，该任务也要查询邮箱MBoxDataCal2，由于邮箱为空，该任务也处于等待状态，以此类推，优先级最低的任务TaskADC()因为不需要等待邮箱，故可以获得CPU控制权运行，在等待时间内，发送消息至邮箱MBoxDataCal1，高优先级任务TaskCal()获得消息后运行，该任务发送消息至邮箱MBoxDataBack，任务TaskDataComm()继续运行，在等待时间内，任务TaskCal()再发送消息至邮箱MBoxDataExe，舵机执行机构TaskServoOut()获得消息后继续运行。然后系统进入下一周期的运行。 5 实验验证 基于上述系统软件设计方法，采用40级无人机模型，动力装置采用两冲程甲醇内燃发动机，进行了试飞实验验证。首先在地面进行测试，各舵面均有正确逻辑偏转响应。试飞验证实验让无人机进行直线平飞和简单的转弯飞行，无人机有较好的响应，地面站也能够正常接收显示数据，取得了较为满意的效果，实验无人机模型及地面站显示信息如图4所示: 图4 实验模型及地面监控 6 结束语 本文探讨了uC/OS-II实时操作系统在一类小型无人机飞行控制方面的应用，为嵌入式飞行控制系统设计提供了新的思路。实时内核的引入，便于在系统级平台下实现多任务的管理调度，提高了系统的实时性，缩短了产品的开发周期。随着嵌入式技术的不断发展，以微处理器和嵌入式实时操作系统相结合的嵌入式体系结构必将在飞行控制系统设计中得到越来越广泛的应用。 参考文献: [1] Jean J Lacrosse著，邵贝贝译 嵌入式实时操作系统uC/OS-II[M]. 北京:北京航空航天大学出版社, 2003. [2] 毛伟云等.嵌入式实时操作系统uC/OS-II在弹载计算机上的应用[J]. 弹箭与制导学报，2007，27(4):349-356. [3] 吴森堂，费玉华. 飞行控制系统[M].北京:北京航空航天大学出版社, 2005. [4] 张太平.仪器仪表中基于邮箱监控的嵌入式软件系统设计与应用[J].PLC&FA 2006, 8:102-105.