e_flight_mission 使用说明

1. 概述

e_flight_mission 是一个纯粹的无人机任务控制包，运行在 ROS 1 Noetic 环境下。它包含三个独立节点，共同完成以下任务流程：

地面站 → [gcs_bridge_node] → 启动信号
                            →  [mission_controller_node]  → OFFBOARD + 解锁
                                                            → 飞至 0.8m 高度悬停
                                                            → 巡航至投掷点
                                                            → 触发投掷 [→ stm32_bridge_node → STM32]
                                                            → 收到 STM32 ACK
                                                            → 巡航至降落点
                                                            → AUTO.LAND 降落

2. 系统架构

┌─────────────────────────────────────────────────────────────────────┐
│                         ROS 网络                                     │
│                                                                     │
│  /e_flight/gcs_start                                                │
│  ◄────────────── gcs_bridge_node ◄──── 串口 /dev/ttyUSB1 ◄── GCS   │
│                                                                     │
│  /e_flight/stm32_ack                                                │
│  ◄──────────── stm32_bridge_node ◄──── 串口 /dev/ttyUSB0 ◄── STM32 │
│                                                                     │
│                     mission_controller_node                          │
│  ┌────────────────────────────────────────────────────────────┐     │
│  │ WAIT_GCS → TAKEOFF → CRUISE_TO_DROP → HOVER_AND_DROP      │     │
│  │         → CRUISE_TO_LAND → AUTO_LAND                        │     │
│  └────────────────────────────────────────────────────────────┘     │
│      │         │            │              │                        │
│      ▼         ▼            ▼              ▼                        │
│  /mavros/    /mavros/    /e_flight/    /mavros/                     │
│  setpoint    set_mode    drop_trigger  cmd/arming                   │
│  /position   /AUTO.LAND                                             │
│                                                                     │
│  ┌────────────────────────────────────────────────────────────┐     │
│  │                   MAVROS (PX4/ArduPilot)                    │     │
│  └────────────────────────────────────────────────────────────┘     │
└─────────────────────────────────────────────────────────────────────┘

3. 节点说明

3.1 `mission_controller_node`（主控状态机）

文件位置： src/mission_controller_node.cpp

类： MissionController

核心功能： 六状态飞行状态机，由 20Hz ROS Timer 驱动。

状态流转图

         ┌──────────────┐
         │   WAIT_GCS   │  ←──── 默认状态，等待 gcs_start=true
         └──────┬───────┘
                │  gcs_start == true
                ▼
         ┌──────────────┐
         │   TAKEOFF    │  ←──── OFFBOARD + ARM，发布 (0,0,0.8)
         └──────┬───────┘
                │  到达 0.8m (±0.1m) && 悬停 3s
                ▼
         ┌──────────────┐
         │ CRUISE_TO    │  ←──── 发布 (1.6, 0, 0.8)
         │   _DROP      │
         └──────┬───────┘
                │  平面距离 ≤ 0.1m
                ▼
         ┌──────────────┐
         │ HOVER_AND    │  ←──── 发布 drop_trigger=true
         │   _DROP      │         等待 3s && stm32_ack==true
         └──────┬───────┘
                │  收到 stm32_ack && 超时
                ▼
         ┌──────────────┐
         │ CRUISE_TO    │  ←──── 发布 (1.6, -1.6, 0.8)
         │   _LAND      │
         └──────┬───────┘
                │  平面距离 ≤ 0.1m
                ▼
         ┌──────────────┐
         │  AUTO_LAND   │  ←──── 调用 set_mode("AUTO.LAND") → 结束
         └──────────────┘

关键参数（硬编码）

参数	值	说明
控制频率	20 Hz	ros::Timer 驱动
起飞高度	0.8 m	起飞点高度
投掷点坐标	(1.6, 0, 0.8)	平面 X≈1.6m
降落点坐标	(1.6, -1.6, 0.8)	相对起飞点 Y≈-1.6m
高度/航点误差	±0.1 m	到达判定阈值
悬停时间	3 s	TAKEOFF 和 HOVER 阶段

3.2 `stm32_bridge_node`（STM32 桥接）

文件位置： src/stm32_bridge_node.cpp

类： Stm32Bridge

核心功能： 桥接 ROS 与 STM32 串口通信。

数据流

ROS  ├─ 订阅: /e_flight/drop_trigger        (std_msgs::Bool)
     │    data=true → 向 STM32 发送 0xAA 0x01 0x55
     │
     ├─ 发布: /e_flight/stm32_ack           (std_msgs::Bool)
     │    收到 STM32 返回 0xAA 0x02 0x55 → data=true
     │
     └─ 串口: /dev/ttyUSB0 @ 115200 8N1

线程模型

┌─ ROS 主线程 ─────────────────────┐
│  ros::spin() 处理 drop_trigger   │
│  回调 → serialWrite(cmd)         │
└──────────────────────────────────┘

┌─ 独立接收线程 ───────────────────┐
│  while(ros::ok()):               │
│    byte = serialRead()            │
│    状态机匹配 0xAA 0x02 0x55     │
│    → publish(stm32_ack=true)     │
└──────────────────────────────────┘

帧协议

方向	帧数据	含义
ROS → STM32	`0xAA 0x01 0x55`	触发投掷
STM32 → ROS	`0xAA 0x02 0x55`	投掷完成 ACK

3.3 `gcs_bridge_node`（地面站桥接）

文件位置： src/gcs_bridge_node.cpp

类： GcsBridge

核心功能： 通过无线数传接收地面站启动信号。

数据流

GCS (数传电台) → 串口 /dev/ttyUSB1
                ↓
            gcs_bridge_node
              ├─ 独立线程读串口
              ├─ 状态机匹配 0xAA 0xFF 0x55
              └─ 发布 /e_flight/gcs_start = true (×3 次, 1Hz)

发布机制

首次收到启动帧后：以 1Hz 频率发布 3 次 gcs_start=true，确保主控状态机可靠接收
后续收到相同帧：通过 started_ 原子标志位屏蔽，不再重复发布
线程持续运行，不影响 ROS 主循环

4. 编译方法

4.1 前置依赖

# 基础 ROS 依赖（通常已安装）
sudo apt install ros-noetic-roscpp ros-noetic-mavros-msgs \
                 ros-noetic-geometry-msgs ros-noetic-std-msgs

4.2 编译

# 进入工作空间（假设已经 catkin_init_workspace）
cd ~/e_flight_ws

# 单独编译本包
catkin_make --only-pkg-with-deps e_flight_mission

# 或编译整个工作空间
catkin_make

4.3 Source 环境

source ~/e_flight_ws/devel/setup.bash
# 建议添加到 ~/.bashrc
# echo "source ~/e_flight_ws/devel/setup.bash" >> ~/.bashrc

5. 运行方法

5.1 按顺序启动

建议使用 roslaunch 或逐节点启动：

# 终端 1：启动 MAVROS（与飞控通信）
roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0:921600

# 终端 2：启动 STM32 桥接
rosrun e_flight_mission stm32_bridge_node

# 终端 3：启动地面站桥接
rosrun e_flight_mission gcs_bridge_node

# 终端 4：启动任务控制器（状态机）
rosrun e_flight_mission mission_controller_node

5.2 使用 roslaunch（推荐）

创建 launch/mission.launch：

<?xml version="1.0"?>
<launch>
    <!-- MAVROS -->
    <include file="$(find mavros)/launch/px4.launch">
        <arg name="fcu_url" value="/dev/ttyACM0:921600"/>
    </include>

    <!-- 任务控制器 -->
    <node name="mission_controller" pkg="e_flight_mission"
          type="mission_controller_node" output="screen"/>

    <!-- STM32 桥接 -->
    <node name="stm32_bridge" pkg="e_flight_mission"
          type="stm32_bridge_node" output="screen">
        <param name="port" value="/dev/ttyUSB0"/>
        <param name="baudrate" value="115200"/>
    </node>

    <!-- GCS 桥接 -->
    <node name="gcs_bridge" pkg="e_flight_mission"
          type="gcs_bridge_node" output="screen">
        <param name="port" value="/dev/ttyUSB1"/>
        <param name="baudrate" value="115200"/>
    </node>
</launch>

运行：

roslaunch e_flight_mission mission.launch

5.3 调试模式（无硬件）

在不连接真实飞控的情况下，可单独启动各节点测试 ROS 通信：

# 启动状态机（会报串口错误但不影响逻辑测试）
rosrun e_flight_mission mission_controller_node

# 模拟发送 gcs_start 信号
rostopic pub -1 /e_flight/gcs_start std_msgs/Bool "data: true"

# 模拟发送 stm32_ack 信号
rostopic pub -1 /e_flight/stm32_ack std_msgs/Bool "data: true"

# 查看状态机输出
rostopic echo /mavros/setpoint_position/local

6. 地面站启动帧协议

地面站通过无线数传电台向下发送 3 字节固定帧。

字节索引	值	说明
Byte 0	`0xAA`	帧起始标识
Byte 1	`0xFF`	地面站启动指令
Byte 2	`0x55`	帧结束标识（校验）

匹配状态机：

IDLE  → 收到 0xAA → STATE_1
STATE_1 → 收到 0xFF → STATE_2
          收到 0xAA → 保持 STATE_1
          其他 → IDLE
STATE_2 → 收到 0x55 → MATCH SUCCESS!
          收到 0xAA → STATE_1
          其他 → IDLE

⚠️ 地面站应连续发送该帧 3-5 次（间隔 100ms），以确保数传链路可靠送达。

7. STM32 投掷帧协议

7.1 ROS → STM32（投掷触发）

字节索引	值	说明
Byte 0	`0xAA`	帧起始标识
Byte 1	`0x01`	投掷触发指令
Byte 2	`0x55`	帧结束标识

7.2 STM32 → ROS（投掷完成确认）

字节索引	值	说明
Byte 0	`0xAA`	帧起始标识
Byte 1	`0x02`	投掷完成 ACK
Byte 2	`0x55`	帧结束标识

8. 参数配置

8.1 stm32_bridge_node

参数	类型	默认值	说明
`~port`	string	`/dev/ttyUSB0`	STM32 串口设备
`~baudrate`	int	`115200`	波特率

8.2 gcs_bridge_node

参数	类型	默认值	说明
`~port`	string	`/dev/ttyUSB1`	地面站数传串口
`~baudrate`	int	`115200`	波特率

8.3 mission_controller_node

当前为硬编码参数。如需动态配置，可将以下常量改为 ros::param::get() 读取：

参数名（建议）	类型	默认值	说明
`~takeoff_height`	double	0.8	起飞高度 (m)
`~drop_x`	double	1.6	投掷点 X (m)
`~drop_y`	double	0.0	投掷点 Y (m)
`~land_x`	double	1.6	降落点 X (m)
`~land_y`	double	-1.6	降落点 Y (m)
`~hover_time`	double	3.0	各阶段悬停时间 (s)
`~waypoint_tolerance`	double	0.1	航点到达判定阈值 (m)

9. Topic & Service 总表

9.1 订阅话题

话题名	类型	发布者	说明
`/mavros/state`	`mavros_msgs::State`	MAVROS	飞控连接与模式
`/mavros/local_position/pose`	`geometry_msgs::PoseStamped`	MAVROS	当前位置 (ENU)
`/e_flight/gcs_start`	`std_msgs::Bool`	gcs_bridge_node	地面站一键启动
`/e_flight/stm32_ack`	`std_msgs::Bool`	stm32_bridge_node	STM32 投掷完成
`/e_flight/drop_trigger`	`std_msgs::Bool`	mission_controller_node	投掷触发

9.2 发布话题

话题名	类型	发布者	说明
`/mavros/setpoint_position/local`	`geometry_msgs::PoseStamped`	mission_controller_node	目标航点 (20Hz)
`/e_flight/drop_trigger`	`std_msgs::Bool`	mission_controller_node	触发投掷
`/e_flight/stm32_ack`	`std_msgs::Bool`	stm32_bridge_node	STM32 确认
`/e_flight/gcs_start`	`std_msgs::Bool`	gcs_bridge_node	地面站启动信号

9.3 服务调用

服务名	类型	调用者	说明
`/mavros/cmd/arming`	`mavros_msgs::CommandBool`	mission_controller_node	解锁/上锁
`/mavros/set_mode`	`mavros_msgs::SetMode`	mission_controller_node	切换飞行模式

附录 A：常见问题

Q1：节点启动时提示 "无法打开串口"

原因： 权限不足或串口不存在。

解决：

# 添加用户到 dialout 组（可能需要重启）
sudo usermod -a -G dialout $USER

# 确认串口存在
ls -l /dev/ttyUSB*

# 临时修改权限（调试用）
sudo chmod 666 /dev/ttyUSB0 /dev/ttyUSB1

Q2：状态机卡在某个状态

检查步骤：

rostopic echo /mavros/state — 确认 connected: true
rostopic echo /mavros/local_position/pose — 确认位置数据
rostopic echo /e_flight/gcs_start — 模拟发送 true 测试
rostopic echo /e_flight/stm32_ack — 模拟发送 true 测试

Q3：编译时提示找不到头文件

解决：

# 确认 CMakeLists.txt 中 find_package 包含所有依赖
# 确认 package.xml 中 <depend> 标签包含所有依赖
catkin_make --only-pkg-with-deps e_flight_mission

附录 B：快速启动脚本

保存为 ~/e_flight_ws/run_mission.sh：

#!/bin/bash
source ~/e_flight_ws/devel/setup.bash

# 先启动 MAVROS
roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0:921600 &
sleep 5

# 启动桥接节点
rosrun e_flight_mission stm32_bridge_node &
rosrun e_flight_mission gcs_bridge_node &
sleep 2

# 启动任务控制器
rosrun e_flight_mission mission_controller_node

chmod +x ~/e_flight_ws/run_mission.sh

本文档最后更新：2026-05-07 适用 ROS 版本：Noetic

Name		Name	Last commit message	Last commit date
src		src
CMakeLists.txt		CMakeLists.txt
HARDWARE_FREE_TESTING.md		HARDWARE_FREE_TESTING.md
README.md		README.md
TUTORIAL.md		TUTORIAL.md
package.xml		package.xml

Latest commit

History

Folders and files

Repository files navigation