本文初次寫于2023.03.03，pixhawk飛控應該是刷寫了ArduPilot 4.1以上的版本。

機載計算機通過WIFI和vrpn_ros_client獲取動捕系統(tǒng)（vicon或者nokov）的無人機定位數(shù)據(jù)（x，y，z+四元數(shù)），然后通過 /mavros/vision_pose/pose 的ROS話題發(fā)送給APM飛控。APM飛控收到這些動捕數(shù)據(jù)后，并不是直接使用，而是也利用EKF融合了imu，最終飛控實際融合結(jié)果可以在 /mavros/local_position/pose 話題中讀到，但是讀之前要先啟動APM的數(shù)據(jù)流：

rosservice call /mavros/set_stream_rate ..... 
# message_rate 設置100， on_off 設置為true

APM中需要改的參數(shù)：

AHRS_EKF_TYPE=3
EK3_ENABLE=1
EK2_ENABLE=0
COMPASS_USE=0
COMPASS_USE2=0
COMPASS_USE3=0
VISO_TYPE=1 # mavlink
EK3_SRC1_POSXY=6
EK3_SRC1_POSZ=6
EK3_SRC1_YAW=6 # 使用動捕四元數(shù)的偏航角
EK3_SRC1_VELXY=0
EK3_SRC1_VELZ=0
EK3_SRC2_YAW=6 # 可能非必須
EK3_SRC3_YAW=6 # 可能非必須

參考：

Nokov Indoor Optical Tracking System - Blog - ArduPilot Discourse

?Intel RealSense T265 — Copter documentation (ardupilot.org)

注意：

動捕系統(tǒng)的坐標系要z軸朝上，并且遵循xyz右手定則。

如果不使用動捕的偏航角yaw，而是使用磁羅盤，那么要保證動捕的xy軸和真實世界的東北對應。因為mavros默認發(fā)送的xyz是東北天位置。

如果使用動捕的偏航角yaw，那么也要檢查飛控融合后的偏航角是否正確。

/mavros/local_position/pose 中的四元數(shù)可能和? /mavros/vision_pose/pose 中的四元數(shù)并不相同，比如可能轉(zhuǎn)了180°。要調(diào)整動捕球的安裝方式或者剛體建立方式，保證機頭朝向動捕坐標系x軸正向時，mission planner上顯示偏航角是90°，方向是正東；機頭朝向動捕坐標系y軸正向時，mission planner上顯示偏航角是0°或者360°，方向是正北。

?