APM飞控使用动捕等外部定位

本文初次写于2023.03.03，pixhawk飞控应该是刷写了ArduPilot 4.1以上的版本。

机载计算机通过WIFI和vrpn_ros_client获取动捕系统（vicon或者nokov）的无人机定位数据（x，y，z+四元数），然后通过 /mavros/vision_pose/pose 的ROS话题发送给APM飞控。APM飞控收到这些动捕数据后，并不是直接使用，而是也利用EKF融合了imu，最终飞控实际融合结果可以在 /mavros/local_position/pose 话题中读到，但是读之前要先启动APM的数据流：

rosservice call /mavros/set_stream_rate ..... 
# message_rate 设置100， on_off 设置为true

APM中需要改的参数：

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 # 使用动捕四元数的偏航角
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)

注意：

动捕系统的坐标系要z轴朝上，并且遵循xyz右手定则。

如果不使用动捕的偏航角yaw，而是使用磁罗盘，那么要保证动捕的xy轴和真实世界的东北对应。因为mavros默认发送的xyz是东北天位置。

如果使用动捕的偏航角yaw，那么也要检查飞控融合后的偏航角是否正确。

/mavros/local_position/pose 中的四元数可能和  /mavros/vision_pose/pose 中的四元数并不相同，比如可能转了180°。要调整动捕球的安装方式或者刚体建立方式，保证机头朝向动捕坐标系x轴正向时，mission planner上显示偏航角是90°，方向是正东；机头朝向动捕坐标系y轴正向时，mission planner上显示偏航角是0°或者360°，方向是正北。