灭火飞行器（B题）
设计报告
0
目录
摘要：............................................................................................................................................... 2 关键词： ........................................................................................................................................... 2 一、系统方案 ................................................................................................................................... 3
1.1 控制系统的选择 ............................................................................................................... 3 1.2 飞行姿态控制的论证与选择 ........................................................................................... 3 1.3 电机的选择 ....................................................................................................................... 3 1.4 高度测量模块的论证与选择 ........................................................................................... 3 1.5 边界判断模块的选择 ....................................................................................................... 3 1.6 角速度与加速度测量模块选择 ....................................................................................... 4 二、设计与论证 ............................................................................................................................... 4
2.1 控制方法设计 ................................................................................................................... 4
2.1.1降落及飞行轨迹控制 ............................................................................................ 4 2.1.2 飞行高度控制 ....................................................................................................... 5 2.1.3 飞行姿态控制 ....................................................................................................... 5 2.2 参数计算 ........................................................................................................................... 6 三、理论分析与计算 ....................................................................................................................... 6
3.1 Pid控制算法分析 ............................................................................................................ 6 四、电路与程序设计 ....................................................................................................................... 9
4.1 系统组成 ........................................................................................................................... 9 4.2 系统框图 ........................................................................................................................... 9 4.3 系统各模块电路图 ......................................................................................................... 10 4.4 程序流程图 ..................................................................................................................... 13 五、测试方案与测试条件 ............................................................................................................. 13
5.1 测试方案 ......................................................................................................................... 13 5.2 测试条件 ......................................................................................................................... 14 六、结论 ......................................................................................................................................... 16
6.1 pid控制如下图： .......................................................................................................... 16 附录一：元器件明细表 ......................................................................................................... 18 附录二：仪器设备清单 ......................................................................................................... 18 附录三：程序 ......................................................................................................................... 18
1
摘要：
本系统由数据采集、数据信号处理、飞行姿态和航向控制等部分组成。系统选用STM32F4单片机作为主控芯片，对从STM32F4芯片读取到的一系列数据进行PID算法处理并向飞行器的电调给出相应指令，从而达到对飞行器飞行姿态的控制。
采用STM32F4芯片采集四旋翼飞行器的三轴角速度和三轴角加速度数据,用激光定高来判断“火源位置”，以保证飞行器可以直接找到“火源”。通过使用激光判断边界区域，确保可以在相应范围内飞行；nrf51422无线通信模块用来实现遥控器和飞行器之间的通信。
关键词：
STM32F4单片机 激光传感器 超声波测距定高 PID算法 光流定点模块 nrf51422无线模块
2
一、系统方案
本系统主要由控制模块、激光定高模块、电机调速模块、循迹模块、无线通信模块组成，下面分别论证这几个模块的选择。
1.1 控制系统的选择
方案一：STM32F1单片机作为主控芯片来控制飞行器的飞行姿态与方向，带有摄像头采集，并把摄像头采集的数据发送给STM32F4进行处理，运用 pid算法进行姿态调整。
方案二：采用激光定高模块，把数据发送到STM32F4单片机进行数据处理。 由于单片机质量比较重，飞机飞行过程中佩戴摄像头及单片机时，飞行姿态不稳定，较难实现比赛的要求，故采用方案二。
1.2 飞行姿态控制的论证与选择
方案一：单片机将从定高模块中读取出来的飞行原始数据进行PID算法运算，得到当前的飞行器欧拉角，单片机得到这个欧拉角后根据欧拉角的角度及方向输出相应的指令给电调，从而达到控制飞行器平稳飞行的目的
方案二：单片机将定高模块中读取出来的飞行原始数据进行PID算法运算，得到当前飞行器的四元数，单片机再将数据融合，并对电调发出相应指令，从而达到控制飞行器的飞行姿态的目的。
从算法的复杂程度及我们对算法的熟悉程度，我们选择方案二。
1.3 电机的选择
方案一：电机选择空心杯电机，因为是由厂家自带，比较容易使用。 方案二：电机选择无刷电机。
由于无刷电机质量大，飞机较小，使用无刷电机很难带动，故选择方案一。
1.4 高度测量模块的论证与选择
方案一：采用HC-SR04超声波传感器测量飞行器当前的飞行高度。 方案二：采用激光定高模块测量高度。
考虑到对元件的熟悉程度、元件的价格和程序的编写，选择方案二。
1.5 边界判断模块的选择
内置的激光模块，在发射激光的同时，信号返还的值不同，由于边界贴上了黑色胶袋，返回值会产生较大变化，即可以达到边界判断的目的。
3
1.6 角速度与加速度测量模块选择
欧拉角算法通过求解欧拉角微分方程直接计算航向角、俯仰角和横滚角。欧拉角微分方程关系简单明了,概念直观,容易理解，解算过程中无需作正交化处理,但方程中包含有三角运算,这给实时计算带来一定困难。而且当俯仰角接近90°时方程出现退化现象,这相当于平台惯导中惯性平台的锁定，所以这种方法只适用于水平姿态变化不大的情况,而不适用于全姿态运载体的姿态确定。
方向余弦法对姿态矩阵微分方程作求解,避免了欧拉角法中方程的退化问题，可全姿态工作。但姿态矩阵微分方程实质上是包含九个未知量的线性微分方程组，与四元数法相比,计算量大,实时计算困难，所以工程.上并不实用。
四元数法只需求解四个未知量的线性微分方程组,计算量比方向余弦法小,且算法简单，易于操作,是较实用的工程方法。但四元数法实质上是旋转矢量法中的单子样算法,对有限转动引起的不可交换误差的补偿程度不够,所以只适用于低动态运载体(如运输机等)的姿态解算。而对高动态运载体,姿态解算中的算法漂移会十分严重。
旋转矢量法可采用多子样算法实现对不可交换误差做有效补偿,算法关系简单，易于操作,并且通过对系数的优化处理使算法漂移在相同子样算法中达到最小，因此特别适用于角机动频繁激烈或存在严重角振动的运载体的姿态更新。
四元数法和旋转矢量法都通过计算姿态四元数实现姿态更新，但前者直接求解姿态四元数微分方程,而后者通过求解姿态变化四元数再求解姿态四元数,两者的算法思路并不相同。
利用加速度计所获得的角度信息 θg 与陀螺仪积分后的角度θ 进行比较，将比较的误差信号经过比例Tg 放大之后与陀螺仪输出的角速度信号叠加之后再进行积分。对于加速度计给定的角度θg ，经过比例、积分环节之后产生的角度θ必然最终等于θg 。由于加速度计获得的角度信息不会存在积累误差，所以最终将输出角度θ中的积累误差消除了。加速度计所产生的角度信息θg 中会叠加很强的有四轴运动加速度噪声信号。为了避免该信号对于角度θ 的影响，因此比例系数 Tg 应该非常小。这样，加速度的噪声信号经过比例、积分后，在输出角度信息中就会非常小了。由于存在积分环节，所以无论比例Tg多么小，最终输出角度θ必然与加速度计测量的角度θg相等，只是这个调节过程会随着Tg 的减小而延长。
二、设计与论证
2.1 控制方法设计
2.1.1降落及飞行轨迹控制
由于题中有指示线，所以我们采用漫反射激光定高来识别地面的指示线，红外模块将识别指示线后的信号以数据的方式传给单片机，单片机对信号做出反应，控制电调，从而控制飞行器飞行轨迹。
4
程序流程图如下：
2.1.2 飞行高度控制
飞行高度的采集采用激光定高模块来实现。
2.1.3 飞行姿态控制
通过MPU6050模块来测量当前飞行器的三轴加速度和三轴角加速度，利用瑞萨单片机的IIC协议从MPU6050中读取出数据，解读飞行器的飞行姿态，并经过PID算法程序来对数据进行处理，得到当前欧拉角的值，并将处理后的信号传给电调，控制电机的转速，从而达到控制飞行器的飞行姿态的目的。
5
程序流程图如图：
2.2 参数计算
本系统最主要的参数计算是对MPU-6050等传感器采集的原始飞行数据进行处理。
单片机从MPU-6050芯片获取的数据是飞行器的三轴角速度和三轴角加速度，MCU对数据进行PID算法处理可以得到飞行器当前的飞行姿态，PID是比例,积分,微分的缩写。比例调节作用：是按比例反应系统的偏差,系统一旦出现了偏差,比例调节立即产生调节作用用以减少偏差。比例作用大,可以加快调节,减少误差,但是过大的比例,使系统的稳定性下降,甚至造成系统的不稳定。积分调节作用：是使系统消除稳态误差,提高无差度。因为有误差,积分调节就进行,直至无差,积分调节停止,积分调节输出一常值。积分作用的强弱取决与积分时间常数Ti,Ti越小,积分作用就越强。反之Ti大则积分作用弱,加入积分调节可使系统稳定性下降,动态响应变慢。积分作用常与另两种调节规律结合,组成PI调节器或PID调节器。
三、理论分析与计算
3.1 Pid控制算法分析
由于四旋翼飞行器由四路电机带动两对反向螺旋桨来产生推力，所以如何保证电机在平稳悬浮或上升状态时转速的一致性及不同动作时各个电机转速的比例关系是飞行器按照期望姿态飞行的关键。所以这里我们采用到pid控制理论把
6
飞机的当前姿态调整到期望姿态。
比例 r(t) _ e(t) 积分 微分 u(t) 被控对象 C(t)
Pid控制是通过姿态采集模块发送回来的数据与期望姿态进行比对，如果存在误差，就对误差进行比例、积分、微分的调整，再将调整后的值加到当前电机上，从而达到调整的目的。比例调节的反应速度较快，而且调节作用明显，飞机出现俯仰和翻滚时能快速调节回来，但是稳定性较差，往往会调节过火；积分调节可以消除长期误差，排除外界因素的干扰，但是同样会降低系统整体的稳定性，使飞机发生震荡；微分调节可以预测被控设备的将来状态，及时的进行调整，而且对比例调节有抑制作用，加强单比例调节的稳定性，排除调节过度的问题。所以通过pid控制可以完全考虑到整个系统的过去、现在、将来，以使系统达到稳定。
Pid控制油门原理框图如下：
90º 0º 量化 -90Pid调3 转化 飞行器油门 -3
7
3.2飞行姿态控制单元
飞行器模拟图如下图，姿态控制是通过陀螺仪模块进行数据的采集，根据它采集回来的俯仰角（pitch），翻滚角（roll），四旋翼采用十字型连接，这样的话能明确分离俯仰姿态和翻滚姿态，进行分别控制。这时如果飞机处于俯仰状态就调机头和机尾的电机，那边高就减小那边电机的转速，相应的那边低则加大那边电机的转速。如果飞机处于翻滚状态，则调左右电机。
Yaw 2 Roll 3 Pitch 1 头 头右
4 尾 尾 左
3.3 光流模块原理
可以看到，光流模块原理图还是比较简单的，一颗光流模组PMW3901,一颗激光测距传感器VL53L0X, 2颗LDO,一组2.0间距排母，一个ADC采集点，以及电阻、电容、二三极管和MOS管。
PMW3901是PixArt公司最新的高精度低功耗光学追踪模组，可直接获取xy方向运动信息，测量范围 8cm 以上，工作电流<9mA，工作电VDD(1.8~2.1VDC)和VDDIO(1.8~3.6VDC), 使用4线SPI接口通信。原理图可以看出，2颗LDO，一颗1.8V的LDO提供给VDD，一颗3.0V的LDO提供给VDDIO。SPI 接口使用硬件SPI2，片选信号PA8， 另外LED1为红外LED, 默认没有焊接。 VL53L0X是ST公司的第二代激光测距传感器，该芯片集成了激光发射器和SPAD红外接收器，采用FlightSenseTM技术，通过接收器接收到的光子时间来计算距离，实现更快、更远、更精确的测距功能，该传感器工作电压AVDD由
8
3.0V的LDO经过一个10R电阻得到，大概是2.8V，也恰好是VL53L0X工作最佳电压，该传感器测量距离3cm ~200cm，使用IIC通信接口，最大通信速率400K，我们使用模拟IIC (SDA/PB4,SCL/PB5) 和飞控端通信。
此模块同ATK-LED-RING灯环模块，ATK-WIFI-MODULE 摄像头模块一样，有一个ADC采集脚，根据不同的ADC电压值来识别不同的模块ID,然后飞控根据模块ID来控制模块，光流模块的ADC值为2.06V,ADC的值由电阻R6和R7分压得到。模块的电源通过MOS1控制通断，控制信号为E_ SCL/PB0。 另外可以看到电感L1和L2,通过L1和L2来选择VDDIO,如果使用L1,那么VDDIO电源来自于飞控VCC3.0, 如果使用L2，那么VDDIO电源来自光流模块3.0V LDO IC2，默认焊接L2，这样可以得到更稳定的电源。
四、电路与程序设计
4.1 系统组成
本设计采用STM32F411CE作为灭火飞行器系统的主控芯片，搭载光流定点模块、激光定高模块、NRF无线通信模块、电机控制模块等组成。
光流定点模块精确控制巡航高度为1.5m，激光定高模块用来判断边界和“火源”，NRF无线通信模块用来实现飞行器和单片机之间的无线通信，电机驱动飞机飞行。
4.2 系统框图
9
4.3 系统各模块电路图 （1） nrf通信模块电路图 （2） 电源和USB电路图
（3） MiniFly 遥控器 MCU STM32F103C8T6 电路图 （4） 光流模块电路图
10
图（1）nrf通信模块电路图
图（2）电源和USB电路图
11
图（3） MiniFly 遥控器 MCU STM32F103C8T6 电路图
图（4）光流模块电路图
12
4.4 程序流程图
本系统程序的编写采用KEIL软件进行程序的编写，用USB ISP程序下载器将编写好的程序烧写入STM32F4单片机，软件界面如下图
五、测试方案与测试条件
5.1 测试方案
基于四旋翼飞行器设计一个灭火飞行器（简称飞行器）。飞行器活动区域示意图如图1所示。在图1中，左下方的圆形区域是飞行器起飞及降落点；右侧正方形区域是灭火防区，防区中有4个用红色LED模拟的火源（火源用单只0.5W红色发光二极管来实现，建议LED电流不超过25mA）。飞行器起飞后从A处进入防区，并以指定巡航高度在防区巡逻；发现防区有火源，用激光笔发射激光束的方式模拟灭火操作；所有火源全部熄灭后，飞行器从B处飞离防区返航，返航途中需穿越一个矩形框。从起飞到降落的整个操作过程不得超过5分钟，时间越短越好。具体测试场地图如下：
13
5.2 测试条件
（1） 飞行器从起飞地点垂直起飞升高到150cm±10cm的巡航高度。
（2）在起飞点的巡航高度上悬停15秒，然后以巡航高度从A处进入防区巡航飞行。飞行器发现防区内的火源后，飞往火源上方用上激光笔照射火源作为灭火；激光笔光斑在以火源为圆心、直径20cm圆形区域保持2秒及以上即视为灭火成功。 （3）飞行器从B处飞离防区。（4）返航途中飞行器需要穿过一个宽高为100x70cm的矩形框。回到降落点上空，垂直下降，准确平稳地降落在降落点； （5）整个飞行过程计时得分。
14
5.3测试结果表格
灭火飞行器测试数据 实验序飞控标志flag 号 ① 1 前进 ② ③ ① 2 向左 ② ③ ① 3 后退 ② ③ ① 4 向右 ② ③ 延时(s) 5 7 10 5 6.15 10 6 6 6 8 8 8 位移(cm) 倾角1(前后) 倾角2（左右） 位移x(cm) 10 10 4 2.9 2.9 2.9 -4 -4 -3.9 -7 -7 -7 -0.3 -0.3 -0.25 -6 -6 -3 -2.5 -1.9 -1.9 -2.4 -2.7 -2.4 -10 -10 -10 -155 -156 -155 -35 -2 -18 -26 -23 -21 位移y(cm) 150 260 190 0 0 0 -170 -300 -240 0 0 0
15
六、结论
6.1 pid控制如下图：
6.2 四轴定高飞行不稳定的解决方案：
定高是我们组讨论比较多的一个话题，当然主要讨论的就是定高效果的问题，比如定高飞不起来、响应缓慢、飞起来上窜下窜等，反正就是有很多问题，当然这里面不排除硬件(电机，桨叶)的问题以及软件(PID)问题。所以针对这个定高问题，我们又花了大量时间和精力来优化定高功能，我们改动了不少代码，甚至代码框架也改动了，定高使用单环PID控制，四轴自身重量(增删扩展模块)对定高影响很大，油门响应也很慢。
最后我们修正了以上的bug，使用串级(位置环+速度环)PID的方式控制定高，增加稳定性的同时提高了响应速度，定高稳定性也不会受到自重影响，然后设置默认- -键起飞高度为150cm。
16
6.3 偏飞厉害的解决方案：
（1）电机坏了，电机坏了也分好几种情况，比如电机蓝色底座脱离或者脱落，这种情况导致电机输出动力减弱，甚至有烧坏MOS管的可能；还有像电机输出轴弯曲，装上桨叶转动起来就会因为不平衡产生很大的震动，震动的直接结果就是导致传感器读数据精度变差甚至错误；还有可能是电机长时间工作，轴承或者电刷磨损厉害，导致动力减弱，因为有刷电机是有寿命的，按正常使用来说，这个电机还是有几个月寿命的。对于这种电机坏了的情况，我们只需要换上新的电机即可。
（2）偏飞也有可能是桨叶不平衡所致，因为空心杯转速高，装上不平衡桨叶高速转动，就会产生很大震动，过大的震动导致传感器度数精度降低甚至错误。怎么看桨叶是否平衡呢?最直接的就是看桨叶是否磨损，变形以及残缺等。如果这种情况，直接换上新的桨叶即可（注意桨叶是有正反的）。
（3）偏飞也有可能是信号干扰导致，信号干扰导致四轴接受不到控制信号，从而出现偏飞的情况，如果遇到干扰严重的情况，可以重置遥控器和四轴。
17
附录
附录一：元器件明细表
（1）STM32F411C单片机
（2）MPU9250（3 轴加速计+3 轴陀螺仪+3 轴磁力计） （3）BMP280（气压传感器） （4）激光模块（用于演示灭火） （5）光流定点模块 （6）NRF51422无线模块 （7）3.7V/250mAh 锂电池
（8）716 空心杯电机 + 46mm 桨叶
附录二：仪器设备清单
（1）线性稳压电源 （2） 0.5wled灯泡
附录三：程序
1、多任务管理程序
#include \"usart.h\" #include \"delay.h\" #include \"led.h\" #include \"key.h\" #include \"oled.h\" #include \"24l01.h\" #include \"adc.h\"
#include \"hw_config.h\" #include \"config_param.h\" #include \"beep.h\"
#include \"radiolink.h\" #include \"usblink.h\"
#include \"remoter_ctrl.h\" #include \"atkp.h\" #include \"display.h\" #include \"keyTask.h\" /* FreeRtos includes */ #include \"FreeRTOS.h\" #include \"task.h\"
* ALIENTEK MiniFly_Remotor * main.c
static TaskHandle_t startTaskHandle;
18
static void startTask(void *param); u16 flag = 0; int main(void) {
NVIC_SetVectorTable(FIRMWARE_START_ADDR,0);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);/*中断配置初始化*/ delay_init(); /*delay初始化*/ configParamInit(); /*配置参数初始化*/ ledInit(); /*led初始化*/ oledInit(); /*oled初始化*/
beepInit(); /*蜂鸣器初始化*/ keyInit(); /*按键初始化*/ joystickInit(); /*摇杆初始化*/
usb_vcp_init(); /*usb虚拟串口初始化*/ radiolinkInit(); /*无线通信初始化*/ usblinkInit(); /*usb通信初始化*/ displayInit(); /*显示初始化*/
xTaskCreate(startTask, \"START_TASK\&startTaskHandle);/*创建起始任务*/
vTaskStartScheduler();/*开启任务调度*/ while(1){};/* 任务调度后不会执行到这 */ }
/*创建任务*/
void startTask(void *param) {
taskENTER_CRITICAL(); /*进入临界区*/
xTaskCreate(radiolinkTask, \"RADIOLINK\&radiolinkTaskHandle);/*创建无线连接任务*/
xTaskCreate(usblinkTxTask, \"USBLINK_TX\创建usb发送任务*/
xTaskCreate(usblinkRxTask, \"USBLINK_RX\创建usb接收任务*/
xTaskCreate(commanderTask, \"COMMANDER\创建飞控指令发送任务*/
xTaskCreate(keyTask, \"BUTTON_SCAN\ 创建按键扫描任务*/
19
/*/*/*/*
xTaskCreate(displayTask, \"DISPLAY\ /*创建显示任务*/
xTaskCreate(configParamTask, \"CONFIG_TASK\创建参数配置任务*/
xTaskCreate(radiolinkDataProcessTask, \"DATA_PROCESS\100, NULL, 6, NULL); /*创建无线通信数据处理任务*/
xTaskCreate(usblinkDataProcessTask, \"DATA_PROCESS\NULL); /*创建USB通信数据处理任务*/
vTaskDelete(startTaskHandle); /*删除开始任务*/
taskEXIT_CRITICAL(); /*退出临界区*/ }
2.指令发送程序
static void flight(void) {
static u8 flipBeepTimeOut;
oled_fill(0,24,127,63,0);
/*遥控器解锁、加锁*/
if(getJoystick1Dir(1)==BACK_LEFT && getJoystick2Dir(1)==BACK_RIGHT) {
RCLock = false; }
if(getJoystick1Dir(1)==BACK_RIGHT && getJoystick2Dir(1)==BACK_LEFT) {
RCLock = true; }
/*显示控制模式、锁头、飞行模式*/ GUI_Circle(20,43,16,1); GUI_Circle(20,43,17,1);
if(configParam.flight.ctrl == ALTHOLD_MODE) oled_showChar(14,30,'H',12,24,1);
20
else if(configParam.flight.ctrl == MANUAL_MODE) oled_showChar(14,30,'M',12,24,1); else
oled_showChar(14,30,'T',12,24,1);
oled_showPicture(49,43-15,bmp_lock[RCLock],30,30);
oled_showPicture(94,43-15,bmp_flightMode[configParam.flight.mode],30,30);
GUI_Circle(94+15,43,16,1); GUI_Circle(94+15,43,17,1);
/*按键处理*/
u8 keyState = getKeyState();
if(keyState == KEY_J1_SHORT_PRESS)//一键起飞、降落 {
if( RCLock==false && isMFCanFly==true &&
(configParam.flight.ctrl == ALTHOLD_MODE || configParam.flight.ctrl == THREEHOLD_MODE) ) {
sendRmotorCmd(CMD_FLIGHT_LAND, NULL); }
delay_ms(3000);
flag = 1; //向前纠正 delay_ms(1000);
flag = 2; //向左纠正 delay_ms(1000); flag = 0;//悬停15S delay_ms(5000); delay_ms(5000); delay_ms(5000);
flag = 1; //开始进入，前进 delay_ms(6000); delay_ms(1000); flag = 0;//第1次 delay_ms(2000);
flag = 2; //向左 delay_ms(6150); delay_ms(2000); flag = 0;
delay_ms(2000);
flag = 3; //向后 delay_ms(6000);
21
flag = 0;
delay_ms(2000);
flag = 4; //向右 delay_ms(6000); delay_ms(1500); flag = 0;
delay_ms(2000);
keyState=KEY_J1_SHORT_PRESS;
if(keyState == KEY_J1_SHORT_PRESS)//一键起飞、降落 {
if( RCLock==false && isMFCanFly==true &&
(configParam.flight.ctrl == ALTHOLD_MODE || configParam.flight.ctrl == THREEHOLD_MODE) ) {
sendRmotorCmd(CMD_FLIGHT_LAND, NULL); } } }
else if(keyState == KEY_L_SHORT_PRESS)//紧急停机 {
sendRmotorCmd(CMD_EMER_STOP, NULL); }
else if(keyState == KEY_J2_SHORT_PRESS)//进入微调界面 {
if( RCLock==false && isMFCanFly==true) {
trimFlag = true;
runBeepAcktion(TRIM_BEEP); setShow_ui(TRIM_UI); } }
else if(keyState == KEY_R_LONG_PRESS)//调试界面 {
//if(RCLock == false) {
setShow_ui(DEBUG_UI); } }
else if(keyState == KEY_R_SHORT_PRESS)//一键翻滚 {
22
if( RCLock==false && isMFCanFly==true &&
MFLowPower==false && configParam.flight.flip==FLIP_ENABLE && configParam.flight.ctrl != THREEHOLD_MODE) /*光流定点模式，关闭空翻*/ {
flipFlag = !flipFlag; flipBeepTimeOut = 0; if(flipFlag == true)
runBeepAcktion(FLIP_BEEP); else
stopBeepAcktion(FLIP_BEEP); } }
else if(keyState == KEY_J2_LONG_PRESS) {
if(RCLock == true) {
setShow_ui(MENU_UI); } }
/*事件处理*/
if(flipFlag == true) {
enum dir_e flipDir = getJoystick2Dir(0);
if(flipDir != CENTER) {
flipFlag = false;
stopBeepAcktion(FLIP_BEEP);
sendRmotorCmd(CMD_FLIP, flipDir); }
else if(++flipBeepTimeOut>30) {
flipFlag = false;
stopBeepAcktion(FLIP_BEEP); flipBeepTimeOut = 0; } } }
23
3、姿态控制程序 /*发送飞控数据*/
if(getRCLock()==false && radioinkConnectStatus()==true) {
remoterData_t send;
switch(configParam.flight.mode) {
case HEAD_LESS:
send.flightMode = 1; break; case X_MODE:
send.flightMode = 0; break; }
switch(configParam.flight.ctrl) {
case ALTHOLD_MODE://定高模式 send.ctrlMode = 1; break;
case MANUAL_MODE: //手动模式 send.ctrlMode = 0; break;
case THREEHOLD_MODE: send.ctrlMode = 3; break; }
if(flydata.thrust<=MIN_THRUST && send.ctrlMode==0) {
send.thrust = 0; } else {
send.thrust = flydata.thrust; }
if(getTrimFlag() == true) {
send.pitch = 0; send.roll = 0; }
24
else {
if(!flag) //原地停止 定 定 定 定
}
{
send.pitch = flydata.pitch; send.roll = flydata.roll; }
else if(flag == 1) {
send.pitch = 10; send.roll = -0.25; }
else if(flag == 2) {
send.pitch = 2.9;
send.roll = -6; }
else if(flag == 3) {
send.pitch = -4;
send.roll = -1.9; }
else if(flag == 4) {
send.pitch =1.8;
send.roll = 5; }
//send.pitch = flydata.pitch ; //send.roll = flydata.roll; }
send.yaw = flydata.yaw;
send.trimPitch = configParam.trim.pitch; send.trimRoll = configParam.trim.roll; /*发送飞控数据*/
sendRmotorData((u8*)&send, sizeof(send));
25 前进 //比较稳向左 //比较稳
向后 //比较稳
//向右 //比较稳
//
//
//
因篇幅问题不能全部显示，请点此查看更多更全内容
Copyright © 2019- awee.cn 版权所有 湘ICP备2023022495号-5
违法及侵权请联系：TEL:199 1889 7713 E-MAIL:2724546146@qq.com
本站由北京市万商天勤律师事务所王兴未律师提供法律服务