system: [SYSTEM]

user: 4. 软件设计 4.1. 超声波数据采集与发送 本文使用Arduino进行超声波数据采集和发送。选用的超声波模块为HC-SR04，可实现对2 cm~450 cm距离内平面相对比较平整的物体进行检测。该模块有四个引脚，分别为Trig、Echo、VCC、GND。首先定义Trig、Echo端连接的引脚；进行串口初始化，设置串口通信波特率；配置Trig引脚为输出，Echo引脚为输入，并在loop()循环中对trig引脚持续输出10 us高电平，获得echo引脚高电平持续的时间；计算超声波传感器测量的距离；调用滤波器，对测量的数据进行滤波处理；串口输出滤波后的数据，具体实现流程如图4所示。 4.2. Pixhawk串口接收 1. 自定义主题 Pixhawk飞控系统中封装了一个跨平台无锁publish/subscribe模式的对象请求代理(uORB)，用于完成进程间通信和数据交换。uORB将进程间通信的数据命名为“主题”，“主题”中定义了通信的数据结构，通过“主题”的发布和订阅完成进程间的数据交换，如图5所示。Pixhawk飞控系统中所有通用接口标准主题全部定义在ardupilot/modules/PX4Firmware/msg目录下的.msg文件中，编译飞控程序过程中， 图4. 测距流程图 图5. 主题发布/订阅 通过调用工具Pyhton，检索该目录下的.msg文件，并以msg.h.template文件中定义的模板，将所有.msg文件转换生成.h文件，并保存在目录PX4Firmware/src/modules/uORB/topics下，该msg.h实现对主题的数据结构定义，通过调用ORB_DECLARE()完成了主题的声明。 本文使用的是自定义主题，则需要在ardupilot/modules/PX4Firmware/msg目录下新建一个名为serial_sonar.msg文件，在该文件中定义Pixhawk串口接收到所有超声波数据。 2. 定义主题 主题的发布需完成三个独立又相关联的动作:调用ORB_DEFINE()定义主题；调用orb_advertise()公告主题；调用orb_publish()发布主题。 Pixhawk中所有标准主题的定义均封装在 ardupilot/modules/PX4Firmware/src/modules/uORB/objects_common.cpp文件中，则本文需要在该文件中完成serial_sonar主题定义。 3. 创建新的任务进程 Pixhawk飞控系统使用具有多任务特性的NuttX实时操作系统，在飞控上电之后，NuttX完成底层任务、设备等初始化工作。Pixhawk飞控系统将所有的任务以模块化的形式进行封装，均在ardupilot/modules/PX4Firmware/src/module目录下。本文需要在该目录下创建一个新的任务模块文件夹，并在该文件夹中定义任务实现文件和.mk文件。任务实现文件中通过调用px4_task_spawn_cmd()函数创建新的任务进程。其中，在入口函数中完成了串口配置、波特率设置、主题公告和发布等工作，具体实现流程如图6所示。而在.mk文件中需要定义该模块编译指令MODULE_COMMAND和依赖文件，以便在编译飞控程序时进行调用和执行。 图6. Pixhawk串口接收程序流程图 4. 加入Pixhawk编译系统 Pixhawk借助make工具实现对飞控程序的编译。Makefile文件中定义了程序编译规则、文件包含和调用关系等。在px4_targets.mk文件中，有以下这样几行程序，规定了飞行固件编译需要哪些底层驱动及任务模块。 本文以“X”型四旋翼飞行器为飞行平台，其固件编译指令为make px4-v2，在解析编译指令的过程中确定了飞控板类型为px4fmu-v2，则上述程序最终只执行config_px4fmu-v2_APM.mk文件中包含的内容。则本文需要在该mk文件中，加入自定义的串口接收任务模块。 4.3. 上层避障实现 上层避障指的是如何在悬停模式(Loiter)下实现飞行器避障飞行，需要解决的问题有：1. 如何将接收到的超声波数据传输至ArduPilot应用中；2. 如何在Loiter飞行模式下编写具体的避障控制逻辑。 ArduPilot应用程序接收超声波数据具体实现流程如图7所示。 1. Pixhawk飞控系统依赖的库可以分为三类：核心库，如AP_AHRS捷联惯导库、AP_Motors电机控制信号解算库、AC_AttitudeControl姿态位置控制库等；传感器库，如AP_InertialSensor陀螺仪和加速度计数据读取库、AP_Baro气压计接口库等；其他库，如AP_Mission用于EEPROM数据存储库、AP_Camera摄像机控制库等，这些库文件集中在ardupilot/libraries目录下。而在ardupilot/ArduCopter目录下有make.inc这样一个文件，该文件中定义了飞控系统所依赖的所有库文件，同时在sketch_sources.mk文件中有如下程序，完成make.inc文件中库列表解析，并将所有库的源文件包含进编译系统中。 图7. 流程图 本文首先需要在ardupilot/libraries目录下创建新的AP_SerialSonar超声波传感器接口库，定义AP_SerialSonar类和get_sonar_data()接收超声波数据函数，其实现流程如图8所示。首先，在AP_SerialSonar类初始化时，调用orb_subscribe()订阅主题，通过px4_poll()监控px4_pollfd_struct_t结构体指定的文件描述符，当出现有数据可读(POLLIN)合法事件时，调用orb_copy()获取订阅的主题中的数据并将该数据保存到指定的buffer中。 其次，将AP_SerialSonar库加入到飞控编译系统中，即在make.inc文件中添加LIBRARIES+ = AP_SerialSonar。 2. Pixhawk中所有类声明全部封装在ardupilot/ArduCopter/Copter.h，而飞行控制栈初始化工作在init_ardupilot()中完成，为保证系统完整性和统一性，本文需要在以上两个地方完成AP_SerialSonar类声明和初始化。 3. 在飞行控制栈的列表任务调度器中，添加新任务用于更新接收的超声波数据。本文添加的新任务为SCHED_TASK(update_serial_sonar，10，100)，其中任务名为update_serial_sonar，调用频率为10 Hz，期望运行最长时间为100 ms。 4. 添加完新任务之后，需要在ArduCopter.cpp中定义任务的具体实现。调用AP_SerialSonar类中用于接收超声波数据的函数get_sonar_data()，并赋值给预先定义好的全局变量，具体实现流程如图9所示。 本文旨在使飞行器具有快速高效的避障功能，具体实现为飞行器采取向上逃逸模式进行避障，即飞行器检测到障碍物时，飞行器尽快处于悬停状态，然后垂直向上飞行，具体实现流程如图10所示。 Loiter飞行模式下首先将roll、pitch通道输入转换成期望的加速度，raw通道输入转换成偏航角速率，throttle通道输入转化成飞行器的爬升速率，然后调用水平位置控制器、高度控制器和姿态控制器完成对飞行器姿态解算和定点控制。本文飞行避障实现流程首先是在获取滚转和俯仰通道输入之前，对飞行器周围环境进行检测，当检测到障碍物时，进一步对飞行器飞行速度进行判断，如果飞行速度不为零，则将滚转和俯仰通道输入强制置零，使飞行器尽快处于悬停状态；如果飞行器本身就处于悬停状态，则增加油门通道输入值，提高飞行器飞行高度，从而实现飞行避障。 图8. get_sonar_data()流程图 图9. 流程图 开发的避障系统相继在Stablize、AltHold、Loiter飞行摸下完成了避障测试，通过Mission Planner观察当飞行器检测到障碍物时，电机输出的情况，图11~图13为避障测试效果图。 图10. 避障流程图 图11. Stablize模式下避障效果图 图12. AltHold模式下避障效果图 图13. Loiter模式下避障效果图