飞控学习（八）基于您的需求，我推荐SITL + Gazebo组合作为主要仿真环境。以下是详细分析和配置建议：仿真方案对

基于您的需求，我推荐SITL + Gazebo组合作为主要仿真环境。以下是详细分析和配置建议：

仿真方案对比与推荐

方案对比表

仿真类型	适用场景	开发效率	真实性	推荐度
SITL + Gazebo	算法开发、控制逻辑验证	⭐⭐⭐⭐⭐	⭐⭐⭐⭐	🥇 首选
HITL	硬件接口测试、系统集成	⭐⭐	⭐⭐⭐⭐⭐	🥈 后期使用
RflySim	视觉算法、集群仿真	⭐⭐⭐	⭐⭐⭐	🥉 特定场景
JMavSim	基础逻辑验证	⭐⭐⭐⭐	⭐⭐	备选

推荐方案：SITL + Gazebo 详细配置

环境搭建

# 1. 克隆PX4代码
git clone https://github.com/PX4/PX4-Autopilot.git
cd PX4-Autopilot
git checkout master

# 2. 安装依赖（Ubuntu）
./Tools/setup/ubuntu.sh

# 3. 编译SITL
make px4_sitl gazebo

针对您需求的仿真配置

# 运行基础多旋翼仿真（适合航向控制开发）
make px4_sitl gazebo_iris

# 或者使用有GPS的多旋翼模型
make px4_sitl gazebo_iris_opt_flow

# 对于更真实的动力学，使用标准机型
make px4_sitl gazebo_typhoon_h480

实现航向控制的具体仿真测试方案

1. 创建自定义测试场景

# 在PX4-Autopilot/Tools/sitl_gazebo/worlds/创建测试场景
# heading_test.world
<?xml version="1.0"?>
<sdf version="1.6">
  <world name="heading_test">
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    
    <!-- 添加视觉参考物便于观察航向 -->
    <model name="heading_marker">
      <pose>50 0 0 0 0 0</pose>
      <link name="link">
        <visual name="visual">
          <geometry><box><size>2 20 1</size></box></geometry>
        </visual>
      </link>
    </model>
  </world>
</sdf>

2. 航向控制测试脚本

#!/usr/bin/env python3
"""
航向控制仿真测试脚本
测试固定偏航和手动控制偏航功能
"""

import asyncio
import time
from mavsdk import System
from mavsdk.offboard import OffboardError, PositionNedYaw, VelocityNedYaw

class HeadingControlTester:
    def __init__(self):
        self.drone = System()
        self.is_heading_override = False
        self.manual_yaw_setpoint = 0.0
        
    async def run_test(self):
        """运行完整的航向控制测试"""
        await self.drone.connect(system_address="udp://:14540")
        
        print("等待无人机连接...")
        async for state in self.drone.core.connection_state():
            if state.is_connected:
                print("无人机已连接!")
                break
        
        # 起飞到10米高度
        await self.takeoff(10)
        
        # 测试1: 固定偏航飞行
        await self.test_fixed_heading()
        
        # 测试2: 手动控制偏航
        await self.test_manual_heading_override()
        
    async def test_fixed_heading(self):
        """测试固定偏航航线飞行"""
        print("=== 测试1: 固定偏航飞行 ===")
        
        # 设置航线点（北向飞行，但机头指向东方）
        waypoints = [
            (0, 0, -10, 90),    # 东向偏航90度
            (50, 0, -10, 90),   # 保持东向
            (50, 50, -10, 90),  # 保持东向
        ]
        
        for wp in waypoints:
            await self.fly_to_waypoint(*wp)
            await asyncio.sleep(2)
    
    async def test_manual_heading_override(self):
        """测试手动偏航控制"""
        print("=== 测试2: 手动偏航控制 ===")
        
        # 开始航线飞行（北向）
        await self.start_auto_mission()
        
        # 模拟手动偏航输入
        print("开始手动偏航控制...")
        for yaw_angle in [45, 90, 135, 180, -45, -90]:
            await self.manual_heading_control(yaw_angle)
            await asyncio.sleep(3)

PX4代码修改：实现航向控制功能

1. 修改FlightTask AutoLine

// 在PX4-Autopilot/src/modules/flight_mode_manager/tasks/AutoLine/FlightTaskAutoLine.hpp
class FlightTaskAutoLine : public FlightTaskAuto
{
public:
    bool update() override;
    
    // 添加航向控制方法
    void setHeadingMode(HeadingMode mode) { _heading_mode = mode; }
    void setManualYaw(float yaw) { _manual_yaw = yaw; }
    
private:
    enum class HeadingMode {
        AUTO,           // 自动计算航向
        FIXED,          // 固定偏航
        MANUAL          // 手动控制
    };
    
    HeadingMode _heading_mode{HeadingMode::AUTO};
    float _manual_yaw{0.0f};
    float _fixed_yaw{0.0f};
    
    float calculateDesiredYaw();
};

// FlightTaskAutoLine.cpp
float FlightTaskAutoLine::calculateDesiredYaw()
{
    switch (_heading_mode) {
        case HeadingMode::AUTO:
            // 默认行为：朝向下一个航点
            return _get_heading_to_next_waypoint();
            
        case HeadingMode::FIXED:
            // 固定偏航模式
            return _fixed_yaw;
            
        case HeadingMode::MANUAL:
            // 手动控制模式
            return _manual_yaw;
    }
    
    return 0.0f;
}

bool FlightTaskAutoLine::update()
{
    // 调用父类方法进行基础航线跟踪
    if (!FlightTaskAuto::update()) {
        return false;
    }
    
    // 重写航向控制
    _setpoints.yaw = calculateDesiredYaw();
    _setpoints.yaw_valid = true;
    
    return true;
}

2. 创建测试用MAVLink命令

# heading_control_test.py
from pymavlink import mavutil
import time

def test_heading_controls():
    # 连接SITL
    master = mavutil.mavlink_connection('udp:127.0.0.1:14550')
    
    # 等待连接
    master.wait_heartbeat()
    print("连接到SITL")
    
    # 切换到Offboard模式
    master.set_mode_offboard()
    
    # 测试固定偏航
    print("测试固定偏航45度")
    send_yaw_command(master, 45, fixed=True)
    time.sleep(5)
    
    # 测试手动偏航控制
    print("测试手动偏航控制")
    for yaw in [90, 135, 180, -90, -45, 0]:
        send_yaw_command(master, yaw, fixed=False)
        time.sleep(2)

def send_yaw_command(master, yaw_deg, fixed=True):
    """发送航向控制命令"""
    yaw_rad = math.radians(yaw_deg)
    
    if fixed:
        # 固定偏航模式
        master.mav.command_long_send(
            master.target_system, master.target_component,
            mavutil.mavlink.MAV_CMD_DO_SET_ROI, 0,
            0, 0, 0, 0, 0, 0, yaw_rad
        )
    else:
        # 手动偏航模式
        master.mav.attitude_target_send(
            int(time.time() * 1000),  # 时间戳
            0,  # 目标mask
            0, 0, 0,  # 姿态（不使用）
            0, 0, 0,  # 角速度（不使用）
            yaw_rad  # 偏航角
        )

仿真调试工具配置

1. 实时参数监控

# 在仿真运行时监控参数
uorb top                 # 查看主题数据流
param set MPC_YAW_MODE 4 # 尝试不同的偏航模式
param show MPC_*         # 查看所有MPC参数

2. 数据记录与分析

# 启用详细日志记录
param set SDLOG_MODE 1
param set SDLOG_PROFILE 77  # 记录控制相关数据

# 仿真后分析日志
ulog_info.py log_001.ulg     # 查看日志信息
python3 -m pyulog plot log_001.ulg vehicle_attitude_setpoint.msg

分阶段测试计划

第1周：基础功能验证

# 测试1：基础飞行能力
make px4_sitl gazebo_iris
# 验证：起飞、悬停、降落是否正常

# 测试2：航线飞行
commander takeoff
mission start
# 验证：能否按航线飞行

第2周：固定偏航实现

// 测试固定偏航45度飞行
setHeadingMode(HeadingMode::FIXED);
setFixedYaw(math::radians(45.0f));
// 验证：飞机是否保持45度航向飞行

第3周：手动控制集成

// 测试手动偏航覆盖
setHeadingMode(HeadingMode::MANUAL);
// 通过MAVLink发送手动偏航指令
// 验证：能否实时控制航向

第4周：性能优化测试

# 测试航向控制性能
test_cases = [
    {'yaw_change': 90, 'max_time': 5.0},    # 90度转向应在5秒内完成
    {'yaw_change': 180, 'max_overshoot': 10}, # 超调小于10度
]

为什么选择SITL+Gazebo？

快速迭代：代码修改→编译→测试只需几分钟
调试友好：可以添加printf、可视化传感器数据
成本为零：不需要硬件设备
功能完整：支持完整的PX4栈和Gazebo物理引擎
生态成熟：有大量现成的模型和世界文件

下一步行动建议

立即开始：按照上面的环境搭建步骤配置SITL+Gazebo
验证基础功能：确保能正常运行基础仿真
逐步实现：先实现固定偏航，再添加手动控制
频繁测试：每个小修改都进行仿真验证

这个方案能让您在几天内就开始实际开发，而不是花费几周时间搭建复杂的仿真环境。需要我详细说明某个具体步骤吗？