西华大学毕业设计说明书 开始 初始化飞控板硬件设置 通信初始化 中断初始化 驱动初始化 ··· 飞控系统初始化 初始化系统参数 初始化传感器 通信数据处理 串级PID控制 调节电机PWM波输出
图5.5 主程序流程图
根据主程序流程图底层驱动主要使用到的部分如下: (1) USART1:外接扩展的传感器模块。
(2) USART2:用于调试程序、发送和接收上位机命令的通信。 (3) IIC:用于读取 GY-86 模块的值,此处使用GPIO模拟IIC。
(4) GPIO:用于控制LED,通过不同的闪烁以及亮灭提示飞行器的运行状态。 (5) SYSTICK:滴答定时器,用于系统的精确定时以及延时。
(6) TIM定时器的PWM通道:使用TIM3和TIM4的PWM功能输出4路不同占空比的
PWM波给电子调速器从而控制无刷电机的转动。
(7) 外部中断:利用单片机的外部中断,可以及时的获取当前的飞行姿态。 (8) 独立看门狗:溢出时间为2s,防止系统死机停止工作。
第 19 页
西华大学毕业设计说明书
6系统调试
6.1 通信系统
飞行器不同于一般的控制系统,由于带有四个螺旋桨,并且电机转速很高,近距离调试意味着伴随着安全隐患。需要构建一个通信系统对飞行器进行远距离调试。
用上位机对飞行器实时控制,上位机发送到飞控系统的数据很多,比如想要加减油门,让飞行器往哪飞,在调试的时候需要随时更改PID参数,这也是上位机要发送命令。对于飞行控制系统,也需要发送数据到上位机,比如,当前四个电机的转速,当前飞行器的姿态,以及正在使用的PID参数。为保证通信的准确性,及时性,对于数据传输编写一个通信协议很有必要。上位机如图6.1所示。
图6.1 上位机界面图
因为使用的PID参数有小数,所以协议传递的PID参数都是先放大了1000倍在进行传输的。为了保证每种类型的数据都正确无误,在数据末尾增加了和校验SUM,比如飞控系统在接收到RCDATA后会在单片机里求和,并将结果同上位机发送来的SUM做对比,只有当两者相等时,数据才会真正保存到飞控系统。并且在接收到重要的数据时,飞控系统还会返回一模一样的数据给上位机,表示已经正确的收到了这些重要数据,大大增加了飞控系统的安全性。
第 20 页
西华大学毕业设计说明书 6.2 姿态传感器调试
6.2.1 传感器数据分析与处理
四旋翼飞行器保持平稳状态的关键是正确对姿态传感器的数据进行处理。首先,通过操作MPU6050的寄存器可以获得飞行时的线加速度和转动角速度原始值。加速度计测量值存储寄存器如表6.1所示。
表6.1 加速度计测量值存储寄存器
Register(Hex) 3B 3C 3D 3E 3F 40 Register(Decimal) 59 60 61 62 63 64 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 ACCEL_XOUT[15:8] ACCEL_XOUT[7:0] ACCEL_YOUT[15:8] ACCEL_YOUT[7:0] ACCEL_ZOUT[15:8] ACCEL_ZOUT[7:0] 每个16位加速度计测量值的满量程定义在MPU6050的CCEL_FS寄存器(寄存器28)。 对于每个满量程的设置,加速度计测量值的灵敏度最低分辨率(LSB)如下表6.2所示:
表6.2 加速度计灵敏度最低分辨率
AFS_SEL 0 1 2 3 Full Scale Range ±2g ±4g ±8g ±16g LSB Sensitivity 16384 LSB/g 8192 LSB/g 4096 LSB/g 2048 LSB/g
陀螺仪测量值存储寄存器如表6.3所示。
表6.3 陀螺仪测量值存储寄存器
Register(Hex) 43 44 45 46 47 48 Register(Decimal) 67 68 69 70 71 72 Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0 GYRO_XOUT[15:8] GYRO_XOUT[7:0] GYRO_YOUT[15:8] GYRO_YOUT[7:0] GYRO_ZOUT[15:8] GYRO_ZOUT[7:0] 每个16位陀螺仪测量值的满量程定义在MPU6050的FS_SEL寄存器(寄存器 27)。对于每个满量程的设置,陀螺仪测量值的灵敏度最低分辨率(LSB)如下表6.4所示。
第 21 页
西华大学毕业设计说明书 表6.4 陀螺仪灵敏度最低分辨率
FS_SEL 0 1 2 3 Full Scale Range ±250°/s ±500°/s ±1000°/s ±2000°/s LSB Sensitivity 131 LSB/°/s 65.5 LSB/°/s 32.8 LSB/°/s 16.4 LSB/°/s MPU6050的数据是为int16型,数据范围是-32768到+32767。即正数是0-32767对应16进制即是0-7FFF。负数是-32768- -1对应16进制即是8000-FFFF。
就MPU6050而言,比如数据为FFFF,原码是8001,即-1。数据为FF00,原码是8100,即-256
先让单片机接收MPU6050原始数据,并把数据除以对应的分辨率,可将数据加速度值单位转换为g,角速度单位转换为°/sec。采集数据并取100次平均值后,把ACCEL_OUT放大100倍,再将全部数据发送到串口助手6次,这样便于观察和分析。具体数值如表6.5和6.6所示。
表6.5 静止时MPU6050原始数据
状态 MPU6050静止Z轴垂直向上 GYRO_X 1 1 1 1 1 1 MPU6050静止X轴垂直向下 1 1 1 1 1 1 MPU6050静止Y轴垂直向下 -1 -1 -1 -1 -1 -1 GYRO_Y 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0 0 0 0 GYRO_Z 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ACCEL_X 1 1 1 1 1 1 -100 -100 -100 -100 -100 -100 -2 -2 -2 -1 -2 -1 ACCEL_Y 0 0 0 0 0 0 0 0 0 0 0 0 -99 -100 -100 -100 -99 -99 ACCEL_Z 100 100 100 99 99 100 3 4 4 4 3 4 -3 -3 -4 -4 -4 -4 分析表格可知,当静止时:X、Y、Z三个轴上的角速度因为静止几乎为0°/sec。而加速度因为重力的原因,竖直向上的轴为100(放大了100倍,方便观察),即1g,竖直向下的坐标轴为-1g,而水平的坐标轴加速度几乎为0g。
第 22 页
西华大学毕业设计说明书 表6.6 运动时MPU6050原始数据
状态 绕Y轴 顺时针 转动 绕Y轴 逆时针 转动 绕X轴 顺时针 转动 绕X轴 逆时针 转动 绕Z轴 顺时针 转动 绕Z轴 逆时针 转动 GYRO_X -1 -1 -1 -1 -1 -1 -1 -1 9 8 6 5 -15 -14 -13 -13 -1 -1 -1 -1 -4 -4 4 -2 GYRO_Y 35 28 26 21 -21 -20 -20 -20 0 0 0 0 0 0 0 0 1 1 1 1 3 2 -3 0 GYRO_Z 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 17 16 13 14 -32 -29 -27 -27 ACCEL_X -37 -42 -47 -50 22 25 29 33 0 0 0 0 1 0 1 1 0 0 0 1 0 0 0 0 ACCEL_Y 0 0 0 0 0 0 0 0 22 24 26 27 -15 -18 -20 -22 1 0 -1 2 1 0 0 0 ACCEL_Z 90 88 85 83 96 95 94 92 96 95 94 95 96 97 96 95 98 97 99 98 99 98 100 96 分析表格可知,当传感器绕Y轴转动时:X、Z轴上的角速度因为静止几乎为0°/sec。Y轴上,瞬时针转动角速度为正,逆时针转动角速度为负。而加速度因为转动的原因,Y轴上的加速度绝对值在减小,而X轴上的加速度绝对值在增大,而水平上的Y坐标轴加速度几乎为0g。当传感器绕X轴转动时情况和绕Y轴转动类似。
当传感器绕Z轴转动时, X、Y轴上的角速度因为静止几乎为0°/sec。Z轴上,瞬时针转动角速度为正,逆时针转动角速度为负。因为X,Y轴都在水平方向上转动,Z轴一直竖直向上,所以X,Y,Z轴上的加速度都几乎不变,并且X,Y轴上的加速度几乎为0。 6.2.2 姿态解算
在3.飞行器原理里分析到,四旋翼飞行器的飞行方向与速度都是由飞行器的倾角决定的,并且飞行器倾斜的角度越大,飞行速度也就越快。所以我们期望得到的是姿态数据,也就是欧拉角:航向角(yaw)、横滚角(roll)和俯仰角(pitch)。有了这三个角,我们就可以得到当前四轴的姿态,这才是我们想要的结果。而不是X,Y,Z三个轴的加速
第 23 页