“翼计划”-无人机飞控开发平台实验讲义

翼计划——无人机飞控开发平台

实验讲义

北京中科浩电科技有限公司

联合编制

北京寒水教育科技有限公司

目录

实验0遥控器与飞控对码.............................................3

一、实验目的....................................................3

二、实验仪器和设备..............................................3

三、实验流程....................................................3

四、实验步骤....................................................4

五、结果分析....................................................7

实验1基于12c读取MPU6050数据....................................8

一、实验目的....................................................8

二、实验仪器和设备..............................................8

三、实验流程....................................................8

四、实验步骤....................................................9

五、结果分析...................................................12

实验2气压计读取高度..............................................12

一、实验目的...................................................12

二、实验仪器和设备.............................................12

三、实验流程...................................................13

四、实验步骤...................................................13

五、结果分析...................................................15

实验3无线通信的实现..............................................15

一、实验目的...................................................15

二、实验仪器和设备.............................................15

三、实验流程...................................................15

四、实验步骤...................................................16

五、结果分析...................................................19

实验4PWM驱动电机..............................................19

一、实验目的...................................................19

二、实验仪器和设备.............................................19

三、实验流程...................................................20

四、实验步骤...................................................20

五、结果分析...................................................22

实验5卡尔曼滤波..................................................22

一、实验目的...................................................22

二、实验仪器和设备.............................................23

三、实验流程...................................................23

四、实验步骤...................................................23

五、结果分析...................................................25

实验6陀螺仪姿态显示实验..........................................26

一、实验目的...................................................26

二、实验仪器和设备............................................26

三、实验流程...................................................26

四、实验步骤...................................................27

五、结果分析...................................................28

实验0遥控器与飞控对码

一、实验目的

将飞行器与遥控器无线设备配对。

二、实验仪器和设备

表0.1描述了实验所需要的设备或软件。

表0］实验所需设备

序号设备或软件名称类型设备或软件图示

1USB转串口USB-TTLYP-03硬件

2MSP430USB仿真器硬件

3MSP430-F150飞控开发板硬件

4MSP430-F150_GCS软件©

5IAREmbeddedWorkbench软件e

三、实验流程

在本次试验中，实验者将更改遥控器无线模块的发送地址和飞行器无线模块

的接收地址。图0.1给出了实验流程。

图0.1实验流程

四、实验步骤

1.打开附件目录\2.源码\1.飞控源码\MSP430_F150.eww,界面如图0.1所示;

>♦;>£<□>■e-■o

Enon0.Vlmmn0

图0.1出现标准界面

2.使用USB-TTL、仿真器和电源适配器以图0.2连接飞控板和开发板到电

脑端。

图0.2连接开发板到电脑端

注意，USB转串口模块连接开发板的方式按照图0.3所示连接。

"USB转串口

住模块

图0.3USB转串口模块连接开发板

3.下载程序到飞控板中

点击工具栏上的"播放"按钮---DownloadandDebug,如图0.4所示。

❷MSP430_F150-IAREmbeddedWorkbenchIDE-MSP4307.12.1

FileEditViewProjectEmulatorToolsWindowHelp

人□El◎mX®QL>C▼<Q>%射芭<g>BQ・s|Q[.

图0.4下载飞控程序

Busy

Startingdebuggersession:Loadingdebugfile

Emulator:Downloadingapplication...

图0.5下载中

等待片刻后下载完成，如图0.5所示。下载完成后，点击图0.6中的播放按

键（或按下F5）,即可开始运行程序。

■•哪©c。_:c1r“["Ho11丁:*"昵拼&卷/♦&®,

图0.6全速运行

4.读取飞控当前接收地址

打开附件目录中的MSP430-F150_GCS.exe程序，如图0.7所示。

❸零计一一无人机例开发二一

v|状态：Close寐索衮们

图0.7上位机界面

如图1.8点击此处下拉列表，即可选择插入的串口端口，选择好后点击''连

接”按钮，当“连接”按钮背景色变为绿色时，表示端口连接成功。

图0.8连接端口

点击更新信息按钮，即可查看当前运行时间和飞控的接收地址，如图0.9所

示，运行时间单位为秒，“更细信息”按钮为单次查询，如果需要多次更新信息，

需要多次点击“更新信息”按钮，变化的运行时间表明当前飞控连接正常。

飞控数据遥控器数据电机数据

PitchY:awI

加速度计陀螺仪配置飞控接收地址

Gyro_X：0

0ACC_X：0更新信息

Gyro_Y：0运行时间：110

ACC_Y：0接收地址：E1,E2,E3,E4,E5

p设置接收地址

0ACC_Z：0Gyro_Z：0ElE2E3E4E5|

默认接收地址：E1,E2,E3,E4,E5

i连接]C0M15-状态：1Open联家我们

图0.9连接后效果图

5.更改飞控接收地址

更改接收地址框中的数据，此处示范将接收地址更改为AA,AA,AA,CC,DDO

注意，5个地址空需要填满，并且内容为十六进制数据，不可用10进制数据填

写。所以，填写GG是错误的填法。

填写完毕以后，点击设置接收地址，再点击更新信息即可查看到配置后的地

址，效果如图0.10所示。

飞控数据遥控器数据电机数据

加速度计陀螺仪配置飞控接收地址

ACC_X：0Gyro_X：0更新信息

ACC_Y：0Gyro_Y：0运行时间：18

接收地址：AA,AA,AA,CC,DD

设置接收地址

ACCZ：0GyroZ：0AA|AA|AA叩忸|

默认接收地址:El,E2,E3,E4,E5

图0.10设置接收地址

6.遥控器校准

遥控器的校准为校准遥控器摇杆机械量程与发送量的对应关系。

操作为：按下遥控器上复位按钮不松手，按下一键起飞按钮不松手，先松开

复位按钮，再松开一键起飞按钮，此时STU、RF、电量灯会全部亮起，表示进入

校准状态。

进入校准状态以后各个方向推动遥控器摇杆，将各个端点位置都需要推到。

确保量程照顾到以后，将摇杆回中，注意左边的摇杆需要手动回中。按下一键起

飞按钮后，RF灯闪烁表示校准完成，连接的无人机飞控开发平台控制终端上会

出现遥控器各个通道量，推动遥控器摇杆，可看到摇杆量在终端上的显示。

7.读取遥控器的发送地址

同刚才一样，使用USB转串口模块连接遥控器左端的串口模块，连接上位

机，电机更新信息，即可查看当前的固件号和发送地址和运行时间，如图0.10所

图0.10遥控器更新信息

8.更改遥控器的发送地址

填写新的发送地址到设置框中，再点击设置发送地址，此时地址已经设置,

但是此时并无显示，需要再次更新信息，点击更新信息。如图o.n所示，即可查

看到当前的发送地址已经更改。

当遥控器的发送地址和飞控的接收地址一致时，遥控器与飞控的连接成功。

飞控数据遥控器数据电机数据

图0.11更改遥控器的发送地址

五、结果分析

每一个发送和接收地址都需要是独一无二的，这样才能将精准的操作到每一

架飞机。

实验1基于I2C读取MPU6050数据

一、实验目的

基于I2C通信协议读取MPU6050的数据，其中包括陀螺仪传感器检测到

的旋转角度X轴、Y轴和Z轴值，以及三轴加速度传感器检测到的X轴、Y轴

和Z轴上的加速度分量。

二、实验仪器和设备

表1.1描述了实验所需要的设备或软件。

表1.1实验所需设备

序号设备或软件名称类型设备或软件图示

1USB转串口USB-TTLYP-03硬件

2MSP430USB仿真器硬件

3MSP430-F150开发板飞控硬件

4MSP430-F150_GCS软件

5IAREmbeddedWorkbench软件踏

三、实验流程

在本次实验中，实验者将调用相应函数基于I2C通信协议读取MPU6050的

加速度计和陀螺仪传感器数据，根据约定的协议格式封装数据帧并通过串口发送

给上位机（GCS）显示。此过程中我们需要使用IAR软件编译并下载实验程序，

使用上位机GCS查看MPU6050采集到的数据，以及下载器、USB转串口等硬

件设备。图1.1给出了本实验的实验流程。

图1.1实验流程

四、实验步骤

1.打开附件目录:\4.实验代码'实验1基于I2C读取MPU6050数据

\MSP430_F150.eww；

2.本实验定时读取MPU6050数据并发送给上位机，这部分代码在timer.c

的定时器中断处理函数TIMER_TBO_AO」RQ_Handler()中实现，如图1.2所示:

if(cnt_3ms=2)//3ms更新一次

(-

cnt_3ms=0;

MpuGetDataO;//获取MPU6050数据

RC-Analy();〃遥控数据获取与解析

FlightPidControKO.003f);//姿态控制

MotorControl0;,'电机输出

if(cnt_100ms=99)

(-

Buff[0]=OxAA;〃帧头

Bufftl]=12;〃长度低八位

Buff[2]=0：//长度高八位

Buff[3]=1;〃发送者ID

Buff[4]=0;〃接收者ID

Buff[5]=2;//消息ID

memcpy(Buff+6,&MPU6050,12);

for(inti=0;i<((Buff[1]IBuff[2]«8)+6);i++)

(

sum+=Buff[i];

Buff[(Buff[l]|Buff[2]«8)+6]=sum&OxFF;

sum=0;

UART_PutNChar(lARTO,(char*)Buff,(Buff[1]IBuff[2]«8)+7)：

)一

LED_Update();

图1.2

定时器每3ms调用函数MpuGetData。,读取加速度和陀螺仪采集到的数据。

图1.3是MpuGetData。具体实现：

staticvolatileint!6_t*pMpu-(intl6_t*)&MPU6050:

voidMpuGetData(void)&取陀螺仪收据加一滤波

uint8_ti:

uint8_tbuffer[12]:

Acc_Read(buffer):

Gyro_Read(buffer)：

for(i=0：i<6;i++)

pMpu[i]=(((intl6_t)buffer[i«l]«8)buffer[(i«l)+l])-MpuOffset[i]:

if(i<3)此处对加速度做一维卡尔曼滤波

staticstruct1ekf_filterekf[3]={{0.02.0,0,0,0.001,0..M?.},(0.02,0,0,0,0.001,0.543},(0.02,0,0,0,0.001,0.543})：

ka1man^1(&ekf[iT,7float)pMpuLi]);维卡尔曼

pMpu[iJ=(int16_t)ekf[i].out：

if(i>2)此处对角速度做一介低通滤波

uint8_tk=i-3:

constfloatfactor=0.15f;滤波因素

staticfloattBuff[3];

pMpu[i]=tBuff[k]♦(I-factor)+pMpuLi]♦factor:

tBuff[k]=tBuff[k]♦(1-factor)+pMpu[i]*factor:

图1.3

该函数分别调用Acc_Read(buffer)和Gyro_Read(buffer)读取力口速度Acc和角

速度Gyro在三个方向x轴、y轴和z轴上的分量并存放在数组buffer中，然后

在for循环中将数据进行滤波并存放在指针pMpu指向的存储空间，pMpu指向

_st_Mpu类型的结构体MPU6050,该结构体用于保存读取到的加速度和角速度

值，其定义和声明保存在ALL_Atruct.h中，如图1.4所示。

typedefstruct

intl6_taccX;

intl6_taccY;

intl6_taccZ;

intl6_tgyroX;

intl6_tgyroY;

intl6_tgyroZ;

}_st_Mpu;

图1.4

MpuGetData。调用结束后，结构体MPU6050中已经保存了读取至U的数据，

我们接下来需要封装数据帧并将其发送给上位机显示。

3.在MSP430的各个实验中，飞控和上位机(GCS)之间经常需要收发数据，而

收发过程又必须遵循相应协议(即数据帧格式)，以保证数据可以被正确读取和

解析。因此，我们为各个实验约定了统一的数据帧格式并通过数组Buff进行封

装。

表1.2给出了约定的格式及各部分对应的字节长度，我们将采集到的数据通

过如下方式封装并发送，上位机即可成功接收并解析。

表1.2数据帧格式

发送者接收者

帧头长度消^息ID数据校验和

IDID

Ibyte2byteIbyteIbyteIbyteNIbyte

>帧头：帧头固定为OxAA

>长度：长度单指数据的长度N。本实验中需要发送的数据是结构体

MPU6050中的内容，包括6个intl6_t类型的成员变量，共12字节，

故长度为12。协议中长度用2个字节描述，而数组Buff是uint8_t类

型，每个元素1个字节，所以需要用两个元素来描述长度。

>发送者ID：约定为0x01

>接收者ID：约定为0x00

>消息ID：约定为0x02

>数据：指要接收/发送的实际数据。各实验需要收发的是加速度计和陀

螺仪传感器采集到的数据。实验中通过内存拷贝函数memcpy()将结构

体MPU6050保存的数据拷贝到数组中。

>校验和：全帧累加，取低8位。

数据帧(Buff)封装完成后，调用UART_PutNChar()函数通过串口1发送给

上位机。

4.按下“F7”编译当前程序，当出“OError(s),OWaming(s)”时,表示编译成功，

如图1.5：

Build

Messages

Buildingconfiguration:msp430_drone_fmu-Debug

Updatingbuildtree..

Totalnumberoferrors:0

Totalnumberofwarnings:0

BuildDeclarationsFindinFilesAmbiguousDefinitions

图1.5

5.使用USB-TTL,仿真器和电源适配器连接飞控板和开发板到电脑端，连接方式

见实验0的步骤2o

6.下载程序到飞控板中，下载方式见实验0的步骤3。

7.打开上位机GCS,选择对应串口并点击“connecting”按钮进行连接，当按钮变

为绿色且“status”为“open”时表示连接成功。点击图1.7中菜单栏的播放按钮

「开始运行程序。移动开发板，可以看到变化的ACC和Gyro数据，如图1.8所

Zj\O

O'SS"«'-tMVUHK单

图1.6实验现象

五、结果分析

平缓移动开发板时，并没有产生加速度，故ACC值为0,骤然改变开发板

状态时加速度计才会采集到数据；转动开发板时，陀螺仪传感器会采集到旋转角

度在实时变化。

实验2气压计读取高度

一、实验目的

理解气压计原理，获取气压计数据并在上位机实时显示。

二、实验仪器和设备

表2.1描述了实验所需要的设备或软件。

表2.1实验所需设备

序号设备或软件名称类型设备或软件图示

1USB转串口USB-TTLYP-03硬件

2MSP430USB仿真器硬件

3MSP430-F150飞控开发板硬件

4MSP430-F150_GCS软件

5IAREmbeddedWorkbench软件

三、实验流程

在本次试验中，实验者将读取气压计SPL06_001采集到的温度、压力等数

据并计算当前所处高度，将其发送给上位机显示高度变化。图2.1给出了实验流

程。

图2.1实验流程

四、实验步骤

1.打开附录下fitl/msp430_drone_fmu/msp430_drone_fmu.eww；

2.与实验1相同，本实验主要£能即高度值的获取、计算和发送等功能同样都

在timer.c的定时器中断处理函数TIMER_TBO_AO」RQ_Handler()中实现。如

图2.2所示：

if(cnt_100ms=99)

Buff[O；=OxAA;“帧头

BufftL—n.长度低八位

Buff[2：=o；'/艮度高八也

Buff[3：=1;发送若ID

Buff[4=o；接收者ID

Buff。=4;消息ID

intLength=Buff[1]Buff[2]«8;

SPL06_get_raw_pressure();

pressure=SPL06_get_pressure();

uint16_tAlt=(uint16_t)(44330*pow(l-pressureSPL06.G_Press,0.191f));

memcpy(Buff+6,ftAlt,Length);

for(inti=0;i<((Buff[i]Buff[2]«8)+6);i++)

1

sum+=Buff[i];

}

Buff[(Buff[1]Buff[2]«8)+6]=sum&OxFF;

sum=0;

l：ART_Put\Char(UARTO,(char*)Buff,(Buff[1]Buff[2]«8)+7);

图2.2功能实现

我们需要将数据发送给上位机显示，所以同样需要封装成约定格式的数

据帧，数据帧格式在实验1中有详细描述，封装方式也与实验1相同，在此不

再赘述。不同的是，本实验需要发送的实际数据只有uintl6_t类型的高度值Alt,

占2个字节，所以数组Buff中的数据长度为2；另外，消息ID约定为4.

接下来需要通过气压计获取并计算当前的海拔高度Alt,这部分内容在“硬

件和软件一气压计”文件中有详细描述，在此不再赘述。

计算出Alt值后，计算校验和并调用UART_PutNChar()将数据发送

给上位机显示。

3.按下“F7”编译当前程序，当出“OError(s),OWarning(s)”时，表示编译成

功；

4.使用USB-TTL、仿真器和电源适配器连接飞控板和开发板到电脑端，连接

方式见实验0的步骤2。

5.下载程序到飞控板中，下载方式见实验0的步骤3。

6.打开上位机GCS,选择对应串口并点击“connecting”按钮进行连接，当按钮

变为绿色且“status”为“open”时表示连接成功。点击图2.5中菜单栏的播

放按钮二开始运行程序。上下移动底板，观察上位机高度值的变化。如图2.7

所示：

图2.3实验现象

五、结果分析

上下移动底板，上位机显示的海拔高度值Alt会随之实时变化。

实验3无线通信的实现

一、实验目的

了解F150无线通信的实现和相关编程

二、实验仪器和设备

表3.1描述了实验所需要的设备或软件。

表3.1实验所需设备

序号设备或软件名称类型设备或软件图示

1USB转串口USB-TTLYP-03硬件

2MSP430USB仿真器硬件

3MSP430-F150开发板飞控硬件

4MSP430-F150_GCS软件©

5IAREmbeddedWorkbench软件©

三、实验流程

在本次试验中，实验者将主动配置F150遥控器的发送硬件并使用F150遥

控器发送一个固定信息到F150的飞机上，飞机会对该信息作出反应，图5.1给

出了实验流程。

图3.1实验流程

四、实验步骤

1.打开附件目录:\4.实验代码'实验3无线通信\MSP430_F150.eww；

2.与实验1相同，本实验主要功能即遥控器数据的获取与解析、电机控制等都

在timer.c的定时器中断处理函数TIMER_TBO_AO」RQ_Handler()中实现。如

图3.2

if(cnt_3ms=2)/3ms更新一次

(~

cnt__3ms=0;

MpuGetDataO;茯取MP16030数据

RC_Analy()：/遥控数据佚取与解析

FlightPidControl(0.003f);姿态控制

MotorControl0电极瑞曲

)

图3.2

RC_Analy()函数负责遥控器数据的获取与解析，包括油门(Thr)、俯仰角

度(Pitch)、横滚值(Roll)、航向(Yaw)以及两个开关的值，并根据开关值AUX1

和AUX2控制飞控和底板上各LED灯的亮灭。

FlightPidControl。和MotorControl()负责通过PID算法计算四个电机值并控

制电机转动。实验4会对两个函数进行详细介绍，本实验只介绍负责通信的

RC_Analy()函数。

3.RC_Analy()存在于remote.c文件中。以下是其具体实现：

>NRF2401_RecData()函数负责接收遥控器发送的数据保存在指定数组

RC_rxData中；

if(NRF240l_RecData(RC_rxData)==RX_OK)

>将数组RC_rxData的数据保存在send_Data_t类型的结构体send,通过该结

构体解析接收到的数据。

memcpy((uint8_t*)&send_Data,RC_rxData,sizeof(send_Data_t))；

>判断数据帧的帧头和帧尾，如果与符合约定的协议，说明接收到的遥控数据

正确，将数据拷贝到结构体Remote中。Remote.AUX2描述遥控器测二段开

关的值，取值为1000或2000(在遥控器文档的通信协议部分有详细说明),

根据Remote.AUX2的值控制飞控和底板上个LED灯的亮灭。

if((send_Data.Start==OxAAAA)&&(send_Data.Stop==0x5555))

(

Remote.roll=send_Data.ROLL;

Remote.pitch=send_Data.PITCH;

Remote.thr=send_Data.THROTTLE-100;

Remote.yaw=send_Data.YAW;

Remote.AUX1=send_Data.SW_THREE;

Remote.AUX2=send_Data.SW_TWO;

if(Remote.AUX2==2000)

LED_UNLOCK_ON;

LED_STATUS_ON;

LED_M1_ON;

LED_M2_0N;

LED_M3_0N;

LED_M4_0N;

}else

(

LED_UNLOCK_OFF;

LED_STATUS_OFF;

LED_M1_OFF;

LED_M2_0FF;

LED_M3_0FF;

LED_M4_0FF;

)

>pidPitch>pidRoll和pidYaw分别描述无人机的Pitch>Roll和Yaw,将Remote

中对应的数据拷贝到这三个结构体的成员变量desired(描述目标值)，

FlightPidControl()函数会根据这个目标值及采集到的测量值计算出电机值进

行控制，在此不详述。

4.按下“F7”编译当前程序，当出“OError(s),OWaming(s)”时，表示编译成功；

5.使用USB-TTL、仿真器和电源适配器连接飞控板和开发板，连接方式见实验

0的步骤2。

6.下载程序到飞控，下载方式见实验0的步骤3。

7.打开上位机GCS,选择对应串口并点击“connecting”按钮进行连接，当按钮

变为绿色且“status”为“open”时表示连接成功。点击图1.7中菜单栏的播

放按钮；开始运行程序。

8.打开遥控器开关，如图3.3,电源指示灯长亮，绿色LED灯闪烁说明遥控器

可以正常工作，长时间使用请注意充电。

图3.3

9.拨动左侧二段开关，摇动摇杆，观察现象。

注意:重新烧写飞控代码后，飞控的数据接收地址即为默认的E1,E2,E3,E4,E5,

飞控与遥控器需要重新对码进行通信。

五、结果分析

拨动左上角二段开关时飞控和底板上共有6个LED指示灯随之量灭，移动

摇杆电机会随之转动。如图3.4,则表示已经接收到当前遥控器发出的信息，通

信成功。若实验不成功，请检查遥控器是否烧写了控制程序、是否没电；飞控与

遥控器是否对码成功。

图3.4实验现象

实验4PWM驱动电机

一、实验目的

通过上位机显示PWM值控制电机转速，了解PWM波与电机转速的关系。

二、实验仪器和设备

表4.1描述了实验所需要的设备或软件。

表4.1实验所需设备

序号设备或软件名称类型设备或软件图示

1USB转串口USB-TTLYP-03硬件

2MSP430USB仿真器硬件

3MSP430-F150开发板飞控硬件

4MSP430-F150_GCS软件©

5IAREmbeddedWorkbench软件

三、实验流程

在本次实验中，实验者将通过上位机设置4个PWM值，飞控通过串口接收

上位机设置的PWM值来控制四个电机的转速，同时将接收到的pwm值发送给

上位机显示。图4.1给出了实验流程。

图4.1实验流程

四、实验步骤

1.打开附件目录:\4.实验代码'实验4PWM驱动电机\MSP430_F150.eww；

2.实验3介绍了函数RC_Analy(),获取并解析得到了遥控器数据。本实验介绍如

何计算电机值。我们通过PID算法进行计算，本实验中分为内环(pidRateN,

单位：角度/秒)和外环(pidXXX,单位：角度)，均有一个输出(out)和两个输

入(测量值Measured＞目标值desired),外环的输出out作为内环的目标值

desired来用。FlightPidControl。函数最终计算得到内环的输出，MotorControl()

负责通过内环的输出和遥控器发送的油门值计算电机值并控制电机转速。

3.图4.2给出了FlightPidControl()在遥控器解锁后的正式控制代码

casePR0CESS_31:正式进入控制

pidRateX.measured=\(PU6050.gyroX*Gyro_G;内环测量值角度秒

pidRateY.measured=MPU6050.gyroY♦Gyro_G;

pidRateZ.measured=MPU6050.gyroZ*Gyro_G;

pidPitch.measured=Angle,pitch;外环测量值单位:角度

pidRoll.measured=Angle,roll;

pidYaw.measured=Angle,yaw;

pidUpdate(&pidRollTdt);

pidRateX.desired=pidRoll.out;

pidUpdate(&pidRateX,dt);

pidl'pdate(ipidPitch,dt);调用PID处理函数来处理外环俯仰角PID

pidRateY.desired=pidPitch.out;

pidUpdate(ApidRateY,dt);

CascadePID(&pidRateZfftpidYaw,dt);也可以直接调用串级PID函数来处理

break;

图4.2

MpuGetData。函数读取到了MPU6050的数据加速度和角度值(实验1),

Gyro_G为陀螺仪初始化量程，根据两者计算得到内环的测量值

pidRateN.Measured。

姿态解算函数GetAngle(),根据结构体MPU6050计算得到了无人机姿态保

存在Angle中，将其中的俯仰(Pitch)、横滚值(Roll)和航向(Yaw)赋值给外

环的测量值PidXXX.Measured.

调用pidUpdate。函数进行PID运算，可以得到PID最终输出。RC_Analy()

获取至U了外环的目标值PidXXX.desired,FlightPidControl。获取至U了外环的测量

值PidXXX.Measured,调用pidUpdate(&pidXXX,dt)即可得至U外环的输出

PidXXX.outo

外环的PidXXX.out作为内环的目标值来用，内环的测量值已经计算得到，

调用pidUpdate(&pidRateN,dt)即可得到内环的输出pidRateN.outo

4.图4.3给出了MotorControl()函数在遥控器解锁后的正式控制代码。

casePR0CESS_31:

intl6_ttemp;

if(ALL_flag.heightJock)

temp=pidHeightRate.out;

)

else

if(Remote.thr<1040)

MOTOR1=MOTOR2=MOTOR3=MOTOR4=0;清0

break;

temp=Remote,thr-1000;

)

将油门值作为基础值给PC!

MOTOR1=M0T0R2=M0T0R3=M0T0R4=LIMIT(temp,0,700);

MOTORI+=+pidRateX.out-pidRateY.out-pidRateZ.out;

M0T0R2+=+pidRateX.out+pidRateY.out+pidRateZ.out;

M0T0R3+=pidRateX.out+pidRateY.out-pidRateZ.out;

M0T0R4+=pidRateX.outpidRateY.out+pidRateZ.out;

break:

图4.3

获取油门值temp作为基础值给PWM,在此基础上，根据内环的输出

pidRateN.out通过不同的计算方式得到四个PWM值M0T0R1-4,将其作为电机

值控制电机转速。

5.将四个PWM值封装成约定格式的数据帧并通过串口1发送给上位机显示,

数据帧格式及具体发送方式在实验1中有详细描述，在此不再赘述。需要注

意的是，本实验中需要发送的实际数据为接收到的四个PWM值，他们保存

在数组motor中，数据类型为intl6_t,共8个字节，故长度为8；另外,消

息ID约定为0x01。

6.按下“F7”编译当前程序，当出“0Error(s),0Waming(s)”时，表示编译成功；

7.下载程序到飞控，下载方式见实验0的步骤3。

8.打开上位机GCS,选择对应串口并点击“connecting”按钮进行连接，当按钮

变为绿色且“status”为“open”时表示连接成功。点击菜单栏的播放按钮-

开始运行程序。

9.遥控器经过对码后控制飞控；利用上位机(GCS)和示波器观察电机转速和

PWM波形的关系。

五、结果分析

PWM波形显示的占空比越大，电机转速越快。

实验5卡尔曼滤波

一、实验目的

使用卡尔曼滤波器滤除MPU6050数据中的噪声，比较卡尔曼滤波前

后的波形，了解卡尔曼滤波的工作。

二、实验仪器和设备

表5］描述了实验所需要的设备或软件。

表5.1实验所需设备

序号设备或软件名称类型设备或软件图示

1USB转串口USB-TT