企业官网建设流程全解析

简介：面向无人机视觉定位与飞控开发场景，这套实现方案以树莓派为桥梁，将T265视觉传感器输出的位置数据，经MAVLink协议实时传送至ACfly飞控，使飞控获得稳定的基于视觉的定位能力。项目专为毕业设计、课程设计及嵌入式竞赛备赛打磨，源码、工程与说明文档均经过严格测试，可直接编译烧录并复现，适合作为学习模板或功能扩展的起点。资源包内共有1422个文件，以C语言及C++源码、头文件为主，同时包含编译中间文件、链接脚本、下载配置文件、说明文档与固件输出，整体压缩包约32.58MB，目录划分清晰，能有效缩短检索和移植时间。目前已有四百余人学习浏览，对正在搭建视觉定位无人机、研究飞控通信协议或完成相关课程项目的开发者而言，这份完整工程可提供从底层驱动到上层应用的直接参照，也能帮助理解视觉位姿数据如何融入飞控闭环。

1. 树莓派给 ACfly 送 T265 定位：这条链路真正的瓶颈不在“数据能不能出”

树莓派挂一颗 Intel RealSense T265，跑视觉惯性里程计（VIO），再把位姿通过 MAVLink 送到 ACfly 飞控，这是许多室内无人机从“手动飞”走向“定点”最常选的一条路。T265 本身给的是厘米级相对位置和四元数，ACfly 需要的是它能识别的位置来源，树莓派在这里承担的不只是转发，还要完成坐标帧转换、消息封包和串口时序管理。对刚接触 MAVLink 的人来说，最典型的挫折是：T265 出数正常，飞控也通电，但 ACfly 的姿态环始终不认这个外部位置。下面把链路从源头拆开验证，先用最小程序把 T265 的姿态数据读干净，再选消息、做坐标帧转换、走树莓派 UART 送进飞控，最后用不上桨的静叶实验收口。

2. 树莓派读 T265 位姿：SDK 安装、USB 供电和最小数据通路

在碰 MAVLink 之前，先把树莓派端的 T265 数据通路读到“可信”的状态。不要一上来就怀疑飞控，很多所谓“外部定位没生效”，追下去其实是树莓派上 T265 的tracker_confidence长期为 0，数据根本不能当位置源使用。

2.1 树莓派上装 librealsense 并确认设备

安装路径优先考虑 64 位 Raspberry Pi OS。pyrealsense2 的 Linux 预编译包以 aarch64 形态发布，32 位用户态下需要自己编译，时间成本高，而且经常在树莓派 4B 的旧内核上失败。常见做法是直接用包管理器和 pip 安装：

sudo apt update sudo apt install -y python3-pip pip3 install pyrealsense2 pymavlink pyserial # 添加 udev 规则，让普通用户访问 Realsense USB 设备 sudo sh -c 'echo "SUBSYSTEM==\"usb\", ATTR{idVendor}==\"8086\", MODE=\"0666\"" > /etc/udev/rules.d/99-realsense.rules' sudo udevadm control --reload-rules

插上 T265 后执行设备枚举：

python3 -c "import pyrealsense2 as rs; c=rs.context(); print([d.get_info(rs.camera_info.name) for d in c.query_devices()])"

输出里出现Intel RealSense T265，说明设备识别成功。若列表为空，先看dmesg | tail -20有没有 USB 断连，再查供电：树莓派 4B 的单个 USB 口输出不足时，T265 会表现为枚举成功后掉线，尤其是 WiFi 和外接键盘同时占用 USB 时。树莓派 5 换个 USB 控制器后问题相对少，但老批次 T265 固件连树莓派 5 偶尔有兼容性告警，这种情况先用另一台电脑确认摄像头本身是好的。

2.2 最小读取程序：只拿位姿流，不要碰鱼眼

T265 有左右两个鱼眼相机和一个 IMU，对外提供的是已经融合好的位姿流。最小读取时不启用 fisheye 流，只订阅 pose，USB 带宽和 CPU 占用都小很多：

import pyrealsense2 as rs cfg = rs.config() cfg.enable_stream(rs.stream.pose) pipe = rs.pipeline() pipe.start(cfg) for i in range(30): frames = pipe.wait_for_frames() f = frames.get_pose_frame() if not f: continue p = f.get_pose_data() print(f"frame={i:3d} " f"pos=({p.translation.x:7.3f},{p.translation.y:7.3f},{p.translation.z:7.3f}) " f"vel=({p.velocity.x:5.2f},{p.velocity.y:5.2f},{p.velocity.z:5.2f}) " f"conf={p.tracker_confidence}") pipe.stop()

这里get_pose_data()返回的 translation 单位是米，velocity 是米/秒，rotation 是 wxyz 顺序的四元数。先跑这个程序确认数据在动：把 T265 拿在手里向前平移，三个坐标轴里应只有一个明显变化，另外两个只有小抖动。

如果打印出的帧间隔不稳，不要马上怀疑代码。先确认同一 USB 控制器上没有挂带宽敏感设备，比如另一路 USB 摄像头或移动硬盘。带宽不足时，pipe.wait_for_frames()会抛RuntimeError: global timeout，这是树莓派上读 T265 最常见的错误之一。

2.3 发送给 MAVLink 前的三项检查

在进入坐标转换之前，我习惯对原始流做三件事：静止记一段、快速甩动一次、再静止。静止时位置漂移应小于厘米级，如果几十秒漂了几十厘米，说明环境纹理太差或者初始化时 IMU 没稳住；快速甩动后 1 秒内应回到静止，回不到说明运动跟踪中途丢失。把这三步的结果对齐到一张口味的检查表：

还有一个容易踩的坑是启动时机。T265 刚上电的头几十帧 IMU bias 还在收敛，直接把这前几秒数据送进飞控，会把状态估计带偏。常见做法是程序打开设备后等 2 秒，或者在起飞前让树莓派和 ACfly 先一起上电预热，完成初始化后再开始发送。

提示：如果 T265 在静止状态下 confidence 长期是 0，优先查光照和纹理，而不是查通信。白墙和纯色地板会让 VIO 直接退化。

3. 坐标帧对齐与 MAVLink 消息选型：T265 位姿怎么进 ACfly

T265 输出的“位置”不能原样打包发出去。必须先搞清它把哪个轴定义为前方，再把姿态转到飞控常用的 NED 习惯，最后选择 ACfly 认识的 MAVLink 消息。这一步做错，日志里通常不报错，表现是定点悬停时飞控持续往一个方向漂。

3.1 选 ATT_POS_MOCAP 还是 VISION_POSITION_ESTIMATE

MAVLink 里和外部位置相关的消息有好几条，常用的是ATT_POS_MOCAP、VISION_POSITION_ESTIMATE和GLOBAL_VISION_POSITION_ESTIMATE。我的取舍习惯如下：

这条链路不需要经纬度，所以GLOBAL_VISION_POSITION_ESTIMATE不选。ATT_POS_MOCAP用四元数发送，树莓派端不用算欧拉角，少一道转换误差；VISION_POSITION_ESTIMATE则让飞控在已有姿态环上再融合一次欧拉角，适合 ACfly 代码里明确解析该消息 ID 的版本。

实际调查方式很直接：在 ACfly 的 MAVLink 解析函数里搜MAVLINK_MSG_ID_ATT_POS_MOCAP和MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE，哪条消息进入状态估计，就用哪条。不要自己发明 MAVLink 自定义消息，外部设备发给飞控的数据尽量走 common 消息集，后续接地面站联调、录日志都方便。

提示：如果你在 ACfly 源码里找不到任何外部位置输入，说明该固件版本还没启用视觉融合，这时候先升级固件或打开对应编译宏，再回头调树莓派，顺序不要反。

3.2 T265 坐标系和飞控 NED 的偏差处理

T265 的坐标约定在不同 SDK 版本里表现不完全一致，多数实现把它当作 ENU（x 向东，y 向北，z 向上）来用，而 ACfly 这类飞控的位置环通常使用 NED（x 向北，y 向东，z 向下）。树莓派侧需要做一次坐标变换，常见映射是交换 xy 并翻转 z：

def enu_to_ned_pose(p): # p: realsense PoseData return ( p.translation.y, # NED x = ENU y p.translation.x, # NED y = ENU x -p.translation.z, # NED z 向下 [p.rotation.w, p.rotation.y, p.rotation.x, -p.rotation.z], )

四元数按同一个轴映射处理，但这里有个容易误解的点：ENU 和 NED 之间的轴映射带一次镜像，不是简单绕某个轴转 90 度，所以不能只改四元数的符号。如果你在实机上发现机头指向对不上，优先检查启动姿态而不是改映射矩阵——T265 的世界坐标是跟随开机瞬间摄像头朝向的，起飞前强制让摄像头水平、机头朝一个已知方向，会少掉很多对轴问题。

如果摄像头安装在机架上有固定安装角，不要试图在树莓派上做频繁的欧拉角补偿。把安装偏角写成常量数组，在enu_to_ned_pose()返回前乘一次即可，动态调整只会引入额外抖动。

3.3 pymavlink 消息打包：四元数顺序、协方差与时间戳

用 pymavlink 发送时调用很简单，但参数要按约定填：

from pymavlink import mavutil mav = mavutil.mavlink_connection('udp:127.0.0.1:14550') def send_att_pos_mocap(ts, pos_ned, q_ned, conf): cov = [0.0] * 21 if conf >= 1: pos_cov = 1e-6 if conf >= 3 else 1e-4 cov[0] = cov[1] = cov[2] = pos_cov mav.mav.att_pos_mocap_send( time_usec=int(ts * 1_000_000), q=[q_ned[0], q_ned[1], q_ned[2], q_ned[3]], x=pos_ned[0], y=pos_ned[1], z=pos_ned[2], covariance=cov, )

四元数顺序一定是 wxyz，pymavlink 不会自动重排，写错之后飞控收到的姿态会在三维空间里翻转。协方差数组长度 21，对应 6x6 矩阵的上三角，这里只给位置前三个元素赋值，姿态协方差保持 0，表示姿态完全可信。置信度为 0 时不要发送，外部调用方应把该帧丢弃，因为状态估计端最怕的是“置信度极低但位置还在缓慢变化”的数据持续灌进来。

时间戳这一项也容易忽略。time_usec使用树莓派的单调时钟即可，不需要同步 GPS 时钟；ACfly 更关心频率和连续性，而不是绝对时间对齐。发送频率跟随 T265 输出保持在 30 Hz，不要人为加 sleep 降到 10 Hz，位置环更新率越低，定点刚度越差。

4. 树莓派 UART 接 ACfly：串口配置、接线和发送循环

数据打包完成后，链路从“本地逻辑”变成“物理连线”。树莓派 4B 或 5 的 GPIO14/15 默认不是给你做串口用的，先把它从系统控制台和蓝牙手里抢回来，再接 ACfly，否则树莓派输出的 MAVLink 和终端调试输出混在一起，ACfly 只会当成垃圾数据丢掉。

4.1 先打开 /dev/serial0：关登录串口，防止 GPIO 被蓝牙占用

树莓派开串口的步骤在不同系统版本上略有差别，原理相同：

sudo raspi-config # Interface Options -> Serial Port # "login shell over serial" 选 No # "serial port hardware" 选 Yes

也可以直接编辑/boot/firmware/config.txt（旧版本是/boot/config.txt）：

enable_uart=1 dtoverlay=disable-bt

重启后用ls -l /dev/serial0确认存在。dtoverlay=disable-bt把 GPIO14/15 从蓝牙分配的 PL011 里释放出来，避免波特率乱掉。如果不想关蓝牙，可以用dtoverlay=miniuart-bt，但 mini UART 的时钟与核心频率联动，树莓派变频时容易出现字节错位，我一般不推荐。

验证串口可用性的最快方式是把串口当普通文件写一次：

python3 -c "import serial; s=serial.Serial('/dev/serial0', 115200, timeout=0.2); print(s.write(b'test')); s.close()"

能写入不代表接线正确，但至少排除了权限和设备节点问题。遇到No such file or directory，优先检查config.txt是否被系统更新覆盖，以及当前用户是否在dialout组内。

4.2 电平匹配与针脚连接

树莓派 GPIO 是 3.3V TTL，ACfly 板载串口通常也以 3.3V 为参考电压。连接时遵循交叉规则：树莓派 TX 接飞控 RX，树莓派 RX 接飞控 TX，地线共地。常见的树莓派 4B 引脚对应：

如果不确定 ACfly 的串口是 3.3V 还是 5V 电平，先查原理图再接线。直接把 5V 输出接到树莓派 GPIO 上，轻则读不到数据，重则烧坏 SOC。两端 GND 必须连在一起，不共地时波形整体偏移，会出现“发着发着就错一个字节”的典型故障。

4.3 树莓派发送 MAVLink 到 ACfly 的完整脚本

把前面几节合起来，就是一个可以直接跑的发送循环：

import time import pyrealsense2 as rs from pymavlink import mavutil UART = '/dev/serial0' BAUD = 115200 mav = mavutil.mavlink_connection(UART, baud=BAUD) mav.mav.srcSystem = 1 mav.mav.srcComponent = 1 cfg = rs.config() cfg.enable_stream(rs.stream.pose) pipe = rs.pipeline() pipe.start(cfg) # 等 IMU bias 收敛，丢弃启动初期不稳定帧 deadline = time.monotonic() + 2.0 while time.monotonic() < deadline: pipe.wait_for_frames() while True: frames = pipe.wait_for_frames() pf = frames.get_pose_frame() if not pf: continue p = pf.get_pose_data() # ENU -> NED pos = (p.translation.y, p.translation.x, -p.translation.z) q = [p.rotation.w, p.rotation.y, p.rotation.x, -p.rotation.z] if p.tracker_confidence >= 1: cov = [0.0] * 21 pos_cov = 1e-6 if p.tracker_confidence >= 3 else 1e-4 cov[0] = cov[1] = cov[2] = pos_cov mav.mav.att_pos_mocap_send( time_usec=int(time.monotonic() * 1_000_000), q=q, x=pos[0], y=pos[1], z=pos[2], covariance=cov, ) else: time.sleep(0.01)

这段脚本有几个值得注意的点。mav.mav.att_pos_mocap_send()会自动完成 MAVLink 封包、CRC 和字节流序列化，不需要手工拼协议头；srcSystem=1是为了后面接地面站时，避免和地面站的系统号冲突。主循环以pipe.wait_for_frames()作为同步源，发送频率等于 T265 输出频率，不会被 CPU 占用拉低。

如果 ACfly 固件解析的是VISION_POSITION_ESTIMATE，用vision_position_estimate_send()代替，参数换成 roll、pitch、yaw。欧拉角转换建议放在树莓派端用四元数转换，不要依赖 ACfly 内部推算，外部视觉的欧拉角更直接对应 T265 的观测。串口写失败时不要简单break退出，常见做法是记录错误次数、等待 0.1 秒后重试，避免 USB 瞬时抖动把整条任务链路打死。

5. 静叶实验验证链路：不急着给油，先把 T265 定位喂到 ACfly 里

接入真实螺旋桨之前，先用“静叶实验”验证外部定位是否真正生效：电机上锁，T265 和树莓派按正常方向安装，ACfly 只供电不装桨，先把整条链路验证完。

5.1 串口抓包确认物理链路

先看字节是否到达 ACfly。如果手头有 USB-TTL 或者飞控支持串口透传地面站，直接把树莓派输出的/dev/serial0接到地面站，用 mavproxy 看：

mavproxy.py --master=/dev/serial0 --baud=115200 --out=udp:127.0.0.1:14550

或者用最直接的一条命令看串口上有没有数据在流：

timeout 3 cat /dev/serial0 | xxd | head

能看到连续的FE 09开头的 MAVLink 帧，说明波特率、接线和树莓派串口都通了。如果看到FE开头但校验一直错，优先查共地、波特率和串口是否有系统日志混入。帧率稳定在 25-30 Hz 时，说明发送循环本身没有丢帧。

5.2 地面站回传和机头旋转检查

在 ACfly 连接的地面站日志里，观察位置来源的实时曲线。把 T265 固定在桌面上，先静止 10 秒，日志里位置应保持在零点附近；然后用手握住摄像头左右旋转 90 度，姿态应跟随变化且不出现回弹；再向前推 30 厘米，NED x 轴应有正方向变化，y 轴几乎不动。这三项都符合，说明坐标帧和消息类型都对了。

最后再谈一个调参视角：当 ACfly 能识别出视觉位置但定点依然漂移，通常问题不在树莓派，而是飞控对“外部位置源”的信任程度不足。确保 ACfly 状态估计里不是总偏向加速度计积分，而是给视觉位置一个正常的信任系数；同时把磁力计航向辅助降权，以 T265 的 yaw 作为主要航向源，这样室内无 GPS 时，飞控才真正把 T265 定位当作位置观测来用。

本文还有配套的精品资源，点击获取