fcu_bridge_001 与 fcu_server_001 核心代码说明

1. fcu_bridge_001 核心代码

1.1 main()

作用：初始化参数、建立通信、循环收发数据。

关键点：

• channel 决定使用串口通信还是 TCP 通信。
• offboard 决定当前节点的运行角色：机载电脑或地面站。
• 初始化订阅/发布器后，进入循环：
  • 读取串口或 socket 字节流
  • 写入 mav_buf_receive
  • 调用 parse_data()
  • 调用 flush_data()

说明：main() 负责组织消息接收、解析和发送流程。

1.2 RingBuffer / flush_data() / mavlink_send_msg()

作用：完成底层字节流收发。

• RingBuffer：缓存收发字节。
• mavlink_send_msg()：将 mavlink_message_t 拆分为消息头、载荷（payload）和校验和（checksum），并写入发送缓冲区。
• flush_data()：将发送缓冲区中的字节写入串口或 TCP 连接。

说明：上层负责构造 MAVLink 消息，底层统一完成数据传输。

1.3 mav_send_heartbeat()

作用：周期性发送 HEARTBEAT。

关键点：

• offboard=true 时，类型设为 MAV_TYPE_ONBOARD_CONTROLLER
• 否则设为 MAV_TYPE_GCS

说明：向对端报告当前节点的在线状态和节点类型。

1.4 mav_send_arm() / mav_send_disarm() / mav_send_takeoff() / mav_send_land()

作用：将 ROS 内部命令转换为飞控能识别的 MAVLink 控制命令。

• arm：解锁
• disarm：锁定
• takeoff：起飞
• land：降落

说明：将本地 ROS 命令映射为飞控控制命令。

1.5 mav_send_target(...)

作用：发送目标点或轨迹控制量，消息类型是 SET_POSITION_TARGET_LOCAL_NED。

输入内容包括：

• 位置 x y z
• 速度 vx vy vz
• 加速度 ax ay az
• 航向 yaw
• 航向角速度 yaw_rate

关键点：

• set_goal=true 时，将消息作为高层目标点，使用 MAV_FRAME_MISSION。
• 否则，将消息作为控制量发送。
• 发布路径时对 y 坐标取反，以适配 ROS 与飞控之间的坐标系差异。

说明：该函数是目标控制消息的主要发送入口。

1.6 parse_data()

作用：逐字节解析收到的 MAVLink 消息，再按 msgid 分发处理。

主要处理分支如下：

• HEARTBEAT

  • 判断飞控是否解锁
  • 发布内部命令码 2001/2002/1101
  • 说明：同步飞控锁定状态

• BATTERY_STATUS

  • 转换为 sensor_msgs::BatteryState
  • 说明：将飞控电池状态发布到 ROS

• SCALED_IMU

  • 转换为 sensor_msgs::Imu
  • y/z 轴取反
  • 说明：完成 IMU 坐标转换

• GLOBAL_POSITION_INT

  • 转换为 sensor_msgs::NavSatFix
  • 说明：发布 GNSS 定位信息

• GLOBAL_VISION_POSITION_ESTIMATE

  • 转换为 nav_msgs::Odometry
  • 将位置单位由厘米转换为米
  • y 取反
  • 同时发布 TF 和轨迹
  • 说明：将飞控的位置和姿态数据转换为 ROS 里程计数据

• SET_POSITION_TARGET_LOCAL_NED

  • 当 coordinate_frame == MAV_FRAME_MISSION 时，发布至 goal_001
  • 说明：将该坐标帧下的 MAVLink 目标作为高层目标点处理

1.7 odomHandler() / motionHandler()

作用：向飞控发送 ROS 位姿数据。

共同流程：

• 读取 ROS 位姿
• 四元数转欧拉角
• y/z 取反
• 发送 ATTITUDE 与 LOCAL_POSITION_NED

说明：向飞控提供外部位置和姿态参考。

1.8 missionHandler()

作用：将 mission_001 的 11 维数组转换为目标控制消息。

数组格式：

[yaw, yaw_rate, px, py, pz, vx, vy, vz, ax, ay, az]

说明：该函数是上层任务模块向飞控发送控制数据的接口。

1.9 gnssHandler() / originHandler()

作用：发送 SET_GPS_GLOBAL_ORIGIN。

说明：设置飞控使用的全局原点，避免重复设置或使用错误原点。

2. fcu_server_001 核心代码

2.1 main()

作用：创建 TCP 服务端并等待 fcu_bridge_001 连接。

核心流程：

• socket()
• bind(PORT)
• listen()
• accept()
• 循环读取 socket 数据并调用 parse_data() / flush_data()

说明：该节点通过 TCP 转发 MAVLink 消息。

2.2 flush_data() / mavlink_send_msg()

作用：将待发送的 MAVLink 消息重新编码，并写入 TCP 连接。

说明：服务端与桥接端均采用消息层和传输层分离的结构。

2.3 mav_send_heartbeat()

作用：周期性发送心跳。

关键点：

• 当 armed=true 时，在 base_mode 中设置 MAV_MODE_FLAG_SAFETY_ARMED 标志。

说明：将本地解锁状态同步给对端。

2.4 parse_data()

作用：解析远端发送的 MAVLink 消息，并发布至本地 ROS 话题。

核心分支：

• HEARTBEAT

  • 更新 time_heartbeat
  • 标记链路在线

• SET_POSITION_TARGET_LOCAL_NED

  • 当坐标帧为 MAV_FRAME_MISSION 时，发布至 goal_001
  • 否则，转换为 mission_001 的 11 维数组
  • 说明：将远端目标消息转换为本地任务或目标点

• SET_GPS_GLOBAL_ORIGIN

  • 转换为 sensor_msgs::NavSatFix
  • 说明：同步全局原点

• COMMAND_LONG

  • 处理跟随、起飞、降落等命令
  • 说明：将标准 MAVLink 命令映射为工程内部命令码

• SET_MODE

  • AUTO_ARMED -> command=1
  • AUTO_DISARMED -> command=2
  • 说明：将远端锁定或解锁请求转发至本地 ROS

2.5 cmdHandler()

作用：将本地 command 重新编码为 MAVLink 消息。

关键命令：

• 2001：未解锁
• 2002：已解锁
• 2003：发送 SET_GPS_GLOBAL_ORIGIN

说明：本地 ROS 通过命令话题触发服务端发送数据，无需直接操作 TCP 连接。

2.6 gnssHandler() / battHandler() / odom_global001_handler()

作用：将本地 ROS 状态数据封装为 MAVLink 消息。

• gnssHandler()：NavSatFix -> GLOBAL_POSITION_INT
• battHandler()：BatteryState -> BATTERY_STATUS
• odom_global001_handler()：Odometry -> GLOBAL_VISION_POSITION_ESTIMATE

odom_global001_handler() 包含以下转换操作：

• 将位置单位由米转换为厘米
• y 取反
• 四元数转欧拉角
• pitch/yaw 取反

说明：将本地里程计数据转换为飞控可识别的视觉定位消息。

2.7 execute_mission_1hz()

作用：维持连接并检测链路中断。

• 超过 5 秒未收到心跳时，断开连接。
• 每秒主动发送一次心跳。

说明：通过周期性心跳维护连接，并检测链路中断。

返回教程目录 · 上一章：键盘控制的使用 · 下一章：UWB 套件使用 fcu_core_v2