沉浸式远航固定翼无人机系统-Part 4-飞控姿态回传六轴平台
本文最后更新于 2025-12-21，文章内容可能已经过时。
前言
作为一名无人机飞手兼模拟器爱好者，我一直有一个大胆的想法：能不能用真实的飞控（ArduPilot/Pixhawk）来驱动一个赛车动感座椅？
通常动感座椅（Motion Simulator）是配合赛车游戏（如《DiRT Rally》、《Assetto Corsa》）使用的，它们通过 UDP 协议输出车辆的姿态数据。而 Mission Planner（MP）是飞控的地面站软件，它能实时显示飞控的姿态。
如果我写一个 Python 脚本，在 Mission Planner 内部运行，读取飞控数据，伪装成《DiRT Rally》的游戏数据包发给动感平台软件（如 FlyPT Mover），是不是就能实现“把飞控当手柄，座椅跟着动”的效果？
答案是肯定的，但过程远比我想象的曲折。这是一段关于坐标系旋转、欧拉角陷阱和脑洞大开的调试记录。
第一阶段：打通数据链路
Mission Planner 内置了一个基于 IronPython 的脚本环境，可以让我们直接访问 cs.roll、cs.pitch 等变量。
第一步很简单：
在 MP 中运行 Python 脚本。
读取
cs.roll,cs.pitch,cs.yaw。按照 Codemasters 的 UDP 协议（Extradata 3 格式）打包数据。
通过 Socket 发送给本地的动感软件（端口 20777）。
最开始的代码虽然粗糙，但座椅确实动了！只不过……动得乱七八糟。
第二阶段：遭遇“坐标系之墙”
现实世界（航空）常用的坐标系是 NED（北-东-下），而游戏开发常用的坐标系各不相同，Codemasters 往往使用 右-上-前 的定义。
直接把 Roll 发给 Roll，Pitch 发给 Pitch 是行不通的。必须构建旋转矩阵，将 NED 向量转换到游戏坐标系：
def ned_to_game_coords(v_ned):
# Game X (Right) = NED Y
# Game Y (Up) = -NED Z
# Game Z (Forward) = NED X
return (v_ned[1], -v_ned[2], v_ned[0])
这一步修正后，座椅的方向似乎对了一半，但因为我的飞控安装方式非常特殊（可能是垂直安装或倾斜安装），数值范围完全不对。
第三阶段：寻找“零点”
我的飞控安装角度导致静止状态下的 Pitch（俯仰）读数竟然是 42 度到 89 度之间变化。这直接导致动感座椅一启动就“躺平”了。
起初我尝试写一个线性映射函数 map_range，强行把 [42, 89] 映射到 [40, -40]。虽然能用，但逻辑太复杂且不够线性。
顿悟时刻： 既然是物理安装角度带来的偏差，直接在原始数据上做减法不就行了？ 经过几次测试，我发现偏移量非常巨大：
减去 90 度？还不够。
减去 135 度？Bingo！
# 简单粗暴但有效
target_pitch = raw_pitch - 135.0
现在，当我把飞控摆正（在我的定义下）时，座椅终于归中平稳了。
第四阶段：空间几何的终极挑战
解决了归中问题，我遇到了最棘手的问题：轴向错乱。
当我把飞控向前推（Pitch）时，座椅反应正确。但是：
飞控自旋（Yaw） 时，座椅竟然在做 横滚（Roll） 动作。
飞控横滚（Roll） 时，座椅毫无反应。
这不仅是坐标系旋转的问题，更是因为飞控被 竖立安装（转了 90 度），导致物理上的“水平面”发生了改变。原本围绕 Z 轴的旋转（Yaw），在新的参考系下变成了围绕 X 轴的旋转（Roll）。
解决方案：轴交换与重映射
我不能仅仅依赖数学上的矩阵旋转，必须在输入端就进行“手术”。
Pitch 保持不变（这是唯一正常的轴）。
Roll 接管 Yaw 的数据：因为飞控竖着转，它的 Yaw 实际上对应模拟器的 Roll。这里有个坑：Yaw 是 0-360 度的，必须在脚本启动时记录
yaw_zero，只发送相对变化量。Yaw 接管 Roll 的数据：飞控原本的 Roll 现在对应模拟器的 Yaw。
# 核心修复逻辑
# A. Pitch: 修正安装偏差
target_pitch = raw_pitch - 135.0
# B. Roll: 使用 FC 的 Yaw (带归零逻辑)
diff_yaw = raw_yaw - yaw_zero
if diff_yaw > 180: diff_yaw -= 360 # 处理过零点问题
target_roll = diff_yaw
# C. Yaw: 使用 FC 的 Roll
target_yaw = raw_roll
同时，为了配合这种特殊的空间变换，我们在输出端的坐标变换矩阵中也做了一个 135 度的 X 轴旋转，确保重力矢量（G值）也能正确跟随座椅倾斜。
最终成果
经过一下午的调试，代码终于定版。
现在，当我拿起手里的飞控板：
前倾 -> 座椅前倾。
左倾 -> 座椅左转（Yaw）。
水平旋转 -> 座椅左倾（Roll）。
这种奇妙的操控感，仿佛通过一根隐形的线，将手中的传感器与身下的机械巨兽连接在了一起。
写在最后
这次开发让我深刻体会到：
数据可视化至关重要：如果不打印出
raw_pitch的值，我可能永远在瞎猜映射区间。相信物理直觉：当数学公式变得过于复杂时，通常是因为如果你没看清物理本质（比如简单的轴交换比复杂的四元数变换更管用）。
ArduPilot 强大的生态：Mission Planner 的脚本功能虽然冷门，但绝对是神级工具。
如果你也有闲置的飞控和动感座椅，不妨试试这个脚本，体验一下“手持飞控开赛车”的硬核乐趣！
本文代码已开源，适用于 Mission Planner 环境。
import sys
import math
import socket
import struct
import time
# 引入 Mission Planner 的类库 (IronPython)
# from System import DateTime
# --- 检查运行环境 ---
try:
_test = cs.roll
except NameError:
print("!!!")
print("错误: 此脚本必须在 Mission Planner 内部运行!")
print("请打开 Mission Planner -> Scripts -> Select Script -> Run")
print("!!!")
sys.exit(1)
# --- 配置区域 ---
TARGET_IP = '127.0.0.1'
TARGET_PORT = 20777
# --- 动态参数 ---
# [修改] 增加最大倾角限制，以适应减去 135 后的数值范围
MAX_TILT_DEG = 180.0
SCALE_FACTOR = 1.0
# 平滑系数
G_SMOOTH_FACTOR = 0.08
ATT_SMOOTH_FACTOR = 0.15
# 死区设置
IMU_DEADZONE = 0.02
ATT_DEADZONE_DEG = 0.2
# --- 常量 ---
DEG_TO_RAD = math.pi / 180.0
RAD_TO_DEG = 180.0 / math.pi
G_CONST = 9.81
# 全局状态字典
state = {
'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0,
'gx': 0.0, 'gy': 0.0, 'gz': 0.0,
'speed': 0.0
}
last_state = state.copy()
# [新增] 用于记录初始 Yaw，实现 Yaw 转 Roll 时的归零
yaw_zero = None
def normalize(v):
norm = math.sqrt(v[0]**2 + v[1]**2 + v[2]**2)
if norm == 0: return (0, 0, 0)
return (v[0]/norm, v[1]/norm, v[2]/norm)
def apply_deadzone(value, threshold):
if abs(value) < threshold: return 0.0
return value - (threshold if value > 0 else -threshold)
def get_rotation_matrix(roll, pitch, yaw):
c_r, s_r = math.cos(roll), math.sin(roll)
c_p, s_p = math.cos(pitch), math.sin(pitch)
c_y, s_y = math.cos(yaw), math.sin(yaw)
# R_nb (Body to NED) - 计算NED坐标系下的基向量
r11 = c_p * c_y
r21 = c_p * s_y
r31 = -s_p
r12 = (s_r * s_p * c_y) - (c_r * s_y)
r22 = (s_r * s_p * s_y) + (c_r * c_y)
r32 = s_r * c_p
r13 = (c_r * s_p * c_y) + (s_r * s_y)
r23 = (c_r * s_p * s_y) - (s_r * c_y)
r33 = c_r * c_p
return (r12, r22, r32), (r13, r23, r33), (r11, r21, r31)
def ned_to_game_coords(v_ned):
# [保持] 坐标系变换：先绕 X 轴旋转 90 度，再旋转 45 度（共 135 度）
# 这部分逻辑保持不变，因为您提到 Pitch 是正确的
# cos(45) = sin(45) ≈ 0.7071
k = 0.70710678
x_90 = v_ned[1]
y_90 = v_ned[0]
z_90 = v_ned[2]
x_new = x_90
y_new = y_90 * k - z_90 * k
z_new = y_90 * k + z_90 * k
return (x_new, y_new, z_new)
def main():
global yaw_zero
print("--- MP Internal DiRT Bridge (Axis Swapped) ---")
print("Target: " + TARGET_IP + ":" + str(TARGET_PORT))
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
packet_count = 0
last_print = time.time()
last_change_time = time.time()
while True:
now = time.time()
# --- 1. 读取数据 ---
raw_roll = cs.roll
raw_pitch = cs.pitch
raw_yaw = cs.yaw
# [新增] 初始化 Yaw 零点
if yaw_zero is None:
yaw_zero = raw_yaw
print("Yaw Zero set to: " + str(yaw_zero))
r_ax = cs.ax
r_ay = cs.ay
r_az = cs.az
r_spd = cs.groundspeed
# --- 2. 转换 ---
curr_gx = float(r_ax) / 1000.0
curr_gy = float(r_ay) / 1000.0
curr_gz = float(r_az) / 1000.0
curr_gz -= 1.0 # 移除重力
# 加速度死区
curr_gx = apply_deadzone(curr_gx, IMU_DEADZONE)
curr_gy = apply_deadzone(curr_gy, IMU_DEADZONE)
curr_gz = apply_deadzone(curr_gz, IMU_DEADZONE)
# --- 3. 轴映射与修正 (Axis Swap) ---
# A. Pitch (保持原样)
# ------------------------------------------------
target_pitch = raw_pitch - 135.0
# B. Roll (使用 FC Yaw)
# ------------------------------------------------
# 计算 Yaw 相对初始位置的偏差，作为 Roll 输入
diff_yaw = raw_yaw - yaw_zero
# 处理 0-360 跨越问题 (例如 359 -> 1)
if diff_yaw > 180: diff_yaw -= 360
if diff_yaw < -180: diff_yaw += 360
target_roll = diff_yaw # FC Yaw 映射到 游戏 Roll
# C. Yaw (使用 FC Roll)
# ------------------------------------------------
target_yaw = raw_roll # FC Roll 映射到 游戏 Yaw
# 死区处理 (使用新的 target 值)
if abs(target_roll) < ATT_DEADZONE_DEG: target_roll = 0.0
# --- 4. 卡顿检测 ---
has_changed = (abs(raw_roll - state['roll']) > 0.001 or
abs(raw_pitch - state['pitch']) > 0.001) # 这里依然比较原始值变化即可
if has_changed:
last_change_time = now
time_since_change = now - last_change_time
if time_since_change > 0.5 or not cs.connected:
decay = 0.9
state['roll'] *= decay
state['pitch'] *= decay
state['gx'] *= decay
state['gy'] *= decay
state['gz'] *= decay
target_roll = state['roll']
target_pitch = state['pitch']
# --- 5. 平滑 ---
state['roll'] = state['roll']*(1-ATT_SMOOTH_FACTOR) + target_roll*ATT_SMOOTH_FACTOR
state['pitch'] = state['pitch']*(1-ATT_SMOOTH_FACTOR) + target_pitch*ATT_SMOOTH_FACTOR
# Yaw 的平滑 (这里 target_yaw 已经是 absolute value 了)
# 注意：因为我们互换了轴，这里的 state['yaw'] 实际上存储的是来自 FC Roll 的数据
diff_yaw_state = target_yaw - state['yaw']
state['yaw'] = state['yaw'] + diff_yaw_state * ATT_SMOOTH_FACTOR
state['gx'] = state['gx']*(1-G_SMOOTH_FACTOR) + curr_gx*G_SMOOTH_FACTOR
state['gy'] = state['gy']*(1-G_SMOOTH_FACTOR) + curr_gy*G_SMOOTH_FACTOR
state['gz'] = state['gz']*(1-G_SMOOTH_FACTOR) + curr_gz*G_SMOOTH_FACTOR
state['speed'] = r_spd
# --- 6. 计算向量 (Codemasters 模式) ---
final_r = max(min(state['roll'] * SCALE_FACTOR, MAX_TILT_DEG), -MAX_TILT_DEG)
final_p = max(min(state['pitch'] * SCALE_FACTOR, MAX_TILT_DEG), -MAX_TILT_DEG)
final_y = state['yaw']
rad_r = final_r * DEG_TO_RAD
rad_p = final_p * DEG_TO_RAD
rad_y = final_y * DEG_TO_RAD
# 获取 NED 基向量
v_right_ned, v_down_ned, v_fwd_ned = get_rotation_matrix(rad_r, rad_p, rad_y)
# 获取 Up 向量 (Up = -Down)
v_up_ned = (-v_down_ned[0], -v_down_ned[1], -v_down_ned[2])
# 转换为游戏坐标
vec_right = normalize(ned_to_game_coords(v_right_ned))
vec_up = normalize(ned_to_game_coords(v_up_ned))
vec_fwd = normalize(ned_to_game_coords(v_fwd_ned))
# --- 7. 发送数据 ---
data = [0.0] * 66
data[0] = float(now % 10000.0)
out_spd = state['speed'] if state['speed'] > 1.0 else 10.0
if time_since_change > 2.0: out_spd = 0.0
data[7] = float(out_spd)
# 填入向量 (Codemasters Extradata 3 标准位置)
data[11] = float(vec_right[0]); data[12] = float(vec_right[1]); data[13] = float(vec_right[2])
data[14] = float(vec_up[0]); data[15] = float(vec_up[1]); data[16] = float(vec_up[2])
data[17] = float(vec_fwd[0]); data[18] = float(vec_fwd[1]); data[19] = float(vec_fwd[2])
# 填入加速度
data[36] = float(state['gx'] * G_CONST) # Surge
data[37] = float(state['gy'] * G_CONST) # Sway
data[38] = float(-state['gz'] * G_CONST) # Heave
try:
packed = struct.pack('<66f', *data)
sock.sendto(packed, (TARGET_IP, TARGET_PORT))
packet_count += 1
except Exception as e:
print("Socket Error: " + str(e))
if now - last_print > 0.5:
# 打印调试信息：显示轴互换后的状态
print("R(from Yaw):%.1f | P:%.1f | Y(from Roll):%.1f" % (state['roll'], state['pitch'], state['yaw']))
last_print = now
Script.Sleep(20)
main()