# aircraft

**Repository Path**: bef/aircraft

## Basic Information

- **Project Name**: aircraft
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-08-06
- **Last Updated**: 2026-09-04

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# SIMDRONE — 四旋翼无人机仿真系统 (替代 PX4 + Gazebo 联调)

**纯 C/C++ 编写，国产可用**。用 Gazebo（物理引擎）+ MAVLink 桥接（替代 PX4 SITL）+ MAVROS 组成完整仿真链，无需从国外下载 PX4 源码。

## 系统架构

```
你的 ROS2 程序 → MAVROS ──UDP:14560──→ mavlink_bridge (伪装PX4)
                                              ↕ MAVLink HIL
                              ┌─────────────────┴─────────────────┐
                              │  gazebo_mavlink_plugin             │
                              │  (Gazebo插件, 读取IMU/施加电机力)   │
                              │  UDP:14556 ← HIL_SENSOR/HIL_GPS   │
                              │  UDP:14557 → HIL_ACTUATOR_CONTROLS│
                              └─────────────────┬─────────────────┘
                                                ↓
                                           Gazebo 仿真
                                     (iris_quadrotor 四旋翼)
```

## 前置条件

```bash
# 安装 Gazebo 11 + 开发头文件 + meson (国内 apt 源, 无需翻墙)
sudo apt install gazebo-11 libgazebo11-dev meson ninja-build

# 安装 ROS2 和 MAVROS + MAVLink 标准库 (按 ROS2 官方教程)
sudo apt install ros-humble-mavros ros-humble-mavlink ros-humble-libmavconn

# ⚠ 安装 GeographicLib EGM96 数据集 (MAVROS 启动必需!)
# MAVROS 的 UAS 节点启动时必须加载 egm96-5.pgm 用于高度换算,
# 缺失会报: GeographicLib exception: File not readable ...
# 安装到用户目录 (免 sudo):
mkdir -p ~/.geographiclib
geographiclib-get-geoids -p ~/.geographiclib egm96-5

# 持久化环境变量 (写进 ~/.bashrc, 一劳永逸):
echo 'export GEOGRAPHICLIB_DATA=~/.geographiclib' >> ~/.bashrc
source ~/.bashrc
```

## 快速开始

### 1. 编译 (meson)

```bash
meson setup build
meson compile -C build
```

生成 (在 `build/` 目录):
- `mavlink_bridge` — MAVLink 桥接程序（替代 PX4 SITL）
- `libgazebo_mavlink_plugin.so` — Gazebo 插件（模型↔UDP 桥接）
- `test_gazebo_link` — 端到端测试程序

> 说明: 使用 meson 而非 CMake/Makefile。meson 通过 pkg-config 查找
> gazebo/sdformat 依赖，不受 sdformat 缺少 CMake 配置文件的影响。

### 2. 一键启动

```bash
# 方法一: 一键脚本
chmod +x run.sh
./run.sh

# 方法二: 分步启动
# 终端1: Gazebo (插件在 build/ 下)
GAZEBO_PLUGIN_PATH=$(pwd)/build:$GAZEBO_PLUGIN_PATH \
GAZEBO_MODEL_PATH=$(pwd)/models:$GAZEBO_MODEL_PATH \
gazebo --verbose worlds/iris.world

# 终端2: MAVLink 桥接
./build/mavlink_bridge

# 终端3: MAVROS
```

### 3. 启动 MAVROS

```bash
# 若未写入 ~/.bashrc, 需先设置数据路径:
export GEOGRAPHICLIB_DATA=~/.geographiclib

# ⚠ 注意: 使用 14561 作为 MAVROS 本地绑定端口!
# 不能用 udp://:14560@... (会与 bridge 双绑同一端口, 导致 VER 请求超时)
ros2 run mavros mavros_node --ros-args \
  -p fcu_url:=udp://:14561@127.0.0.1:14560
```

> 报错排查: 若启动时报 `GeographicLib exception: File not readable`,
> 说明 EGM96 数据集未安装或 `GEOGRAPHICLIB_DATA` 未设置,
> 参见上文"前置条件"的安装步骤。
>
> 若报 `VER: broadcast request timeout`:
> - 检查 MAVROS URL 是否为 `udp://:14561@127.0.0.1:14560` (不能双绑 14560)
> - 确认 bridge 已启动且显示 `[MAVROS] 已连接!`
> - bridge 使用与 MAVROS 同版本的标准 mavlink 库 (2026.6.6),
>   字段布局/CRC/v1-v2 帧解析均由库处理

### 4. 你的 ROS2 程序

正常通过 MAVROS 发送指令（`setpoint_raw/attitude`、`setpoint_raw/local` 等）。

## ✈️ 完整飞行流程 (已实测验证)

以下操作均已在本仿真环境实测通过。三个终端按顺序启动后：

```bash
# 终端1: Gazebo
GAZEBO_PLUGIN_PATH=$(pwd)/build:$GAZEBO_PLUGIN_PATH \
GAZEBO_MODEL_PATH=$(pwd)/models:$GAZEBO_MODEL_PATH \
gazebo --verbose worlds/iris.world

# 终端2: MAVLink 桥接
./build/mavlink_bridge

# 终端3: MAVROS
export GEOGRAPHICLIB_DATA=~/.geographiclib
ros2 run mavros mavros_node --ros-args \
  -p fcu_url:=udp://:14561@127.0.0.1:14560
```

### ① 解锁 (Arm)

```bash
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}"
# 返回 success: True
# 确认: ros2 topic echo /mavros/state → armed: true, MODE(0x80)
```

### ② 起飞 (Takeoff)

```bash
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL \
  "{min_pitch: 0.0, yaw: 0.0, latitude: 0.0, longitude: 0.0, altitude: 2.0}"
# 返回 success: True, 无人机升空至 ~2m
```

### ③ Offboard 定点飞行

```bash
# 切换到 OFFBOARD 模式
ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{custom_mode: 'OFFBOARD'}"
# 返回 mode_sent: True

# 持续发布位置设定点 (无人机飞向目标)
ros2 topic pub -r 20 /mavros/setpoint_position/local geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0, z: 2.0}}}"
# 观察: ros2 topic echo /mavros/local_position/pose
```

### ④ 降落 (Land)

```bash
ros2 service call /mavros/cmd/land mavros_msgs/srv/CommandTOL \
  "{min_pitch: 0.0, yaw: 0.0, latitude: 0.0, longitude: 0.0, altitude: 0.0}"
# 返回 success: True, 无人机降回地面, 电机自动停转
```

## 📋 PX4 源码移植对照表 (逐模块)

本项目的全部控制/估计/仿真算法均逐行对照 PX4 源码移植, 不自创算法.
参考源码 (已用 git 切换至**稳定版本**): `/home/ts/other/PX4-Autopilot` (**v1.16.2** 固件) + `/home/ts/other/PX4-SITL_gazebo-classic` (**70683dc**, 与 v1.16.2 子模块匹配的 SITL 模型).

### 1. 控制核心 `mavlink_bridge.c` (伪装 PX4, 纯 C)

| 函数 | PX4 源码文件 | 说明 |
|------|-------------|------|
| `px4_position_control` | `src/modules/mc_pos_control/PositionControl/PositionControl.cpp` | 位置环 `vel_sp=(pos_sp-pos)*P+vel_ff` → 速度环 `acc=vel_err*P+I-vel_dot*D+acc_ff` → `_accelerationControl` (z_sf=-g+acc_z, body_z 归一化, limitTilt, 推力 `thr=acc_z*(hover/g)-hover` → bodyzToAttitude) |
| `px4_attitude_rate_sp` | `src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp` | 角度环: qd_red 减缩期望姿态 (忽略 yaw 优先 roll/pitch) → qd_dyaw 提取并缩放 (MC_YAW_WEIGHT) → `qe=q⁻¹*qd`, `eq=2*qe.canonical().imag()` → `rate_sp=eq.*P`, 限幅 MC_*RATE_MAX (220/220/200°/s) |
| `px4_rate_update` | `src/lib/rate_control/rate_control.cpp` | 内环: `torque=P*err+I_int-D*vel_dot+FF*rate_sp`, 积分带 i_factor (400°/s 减半) + 饱和 anti-windup, 落地不积分 |
| `px4_att_init/update` | `src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp` | 姿态估计: gyro 积分 + accel 修正 (ATT_W_ACC=0.2) |
| `px4_takeoff_update/ramp` | `src/lib/takeoff/takeoff.cpp` | Takeoff 状态机 (disarmed→spoolup→ready→rampup→flight) + updateRamp 爬升速度缓升 |
| `geninv4/alloc_normalize/alloc_init/px4_allocate` | `src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp` | 伪逆控制分配 (SymmetricSchur→Cholesky→geninv), 力矩+推力归一化, 电机命令; 有效性矩阵按 `ActuatorEffectivenessRotors.cpp` (`moment=ct*pos×axis-ct*km*axis`) 用**模型实际电机位置** (iris.sdf.jinja 0.13/0.22, FLU→FRD) 计算 |
| `quat_*` 四元数库 | `src/lib/mathlib/math/Quaternion.hpp` | 四元数运算 (euler→quat, 乘法, 共轭, 归一化) |
| 姿态数据 | `src/modules/simulation/gz_bridge/GZBridge.cpp` | HIL_SENSOR IMU, FLU→FRD 坐标转换 `q_FLU_to_FRD=(0,1,0,0)` |
| 位置/速度 | `src/modules/ekf2` (观测源) | `vehicle_local_position` 高度=pressure_alt; 垂速 vz = **PX4 EKF2 互补滤波** (IMU 加速度 250Hz 积分 + HIL_GPS vd 50Hz 慢修正 防漂移); 水平位置 = HIL_GPS 经纬度→本地 NED (`process_hil_gps`), 不自创估计 |

**控制参数** (与 PX4 **v1.16.2 稳定版**默认一致): `MC_ROLL_P=6.5, MC_PITCH_P=6.5, MC_YAW_P=2.8, MC_YAW_WEIGHT=0.4`; `MC_ROLLRATE_P=0.08, MC_PITCHRATE_P=0.08, MC_YAWRATE_P=0.3`; `MC_ROLLRATE_I=0.25, MC_YAWRATE_I=0.15`; `MC_RATE_D=0.001`; `MPC_XY_P=0.95, MPC_XY_VEL_P=1.8, MPC_Z_P=1.0, MPC_Z_VEL_P=4.0(ACC), MPC_Z_VEL_I=2.0(ACC), MPC_Z_VEL_D=0.0`; `MPC_THR_HOVER=0.72(插件转速²力模型实际悬停 actuator), MPC_THR_MIN=0.12`; `MPC_ACC_UP_MAX=4.0, DOWN_MAX=3.0`; `MPC_TILTMAX_AIR=45°, LND=12°`; 起飞爬升速度 `MPC_TKO_SPEED=1.5`.

### 2. 仿真插件 `gazebo_mavlink_plugin.cpp` + `gazebo_link.c`

| 模块 | PX4-SITL 源码 | 说明 |
|------|--------------|------|
| 电机力模型 | `src/gazebo_motor_model.cpp` | 推力 `force=(cmd*MAX_ROT_VEL)²*MOTOR_CONST` (转速²关系), 反扭矩 `-turn_dir*force*moment_constant` (CCW=+1, CW=-1, 0.06), 旋翼阻力 `-|vel|*0.000175*vel`, 桨叶挥舞滚转力矩 `-|vel|*turn_dir*1e-06*vel_perp`, 前向速度空速缩放 (25 m/s 推力归零) |
| 电机动力学 | `src/gazebo_motor_model.cpp` | 一阶滤波 timeConstantUp=0.0125s, Down=0.025s |
| HIL 消息 | `src/gazebo_mavlink_interface.cpp` | HIL_SENSOR (250Hz, 含 pressure_alt) / HIL_GPS (50Hz, 含 vn/ve/vd) / HIL_ACTUATOR_CONTROLS 打包解析 |
| 施加方式 | `src/gazebo_motor_model.cpp` | 逐 rotor link `AddRelativeForce(0,0,force)` (力矩由几何自然产生), 反扭矩/滚转力矩施加 base_link |

### 3. 模型 `models/iris_quadrotor/iris_quadrotor.sdf`

对照 `PX4-SITL_gazebo-classic/models/iris/iris.sdf.jinja` 渲染结果:
- 机身 base_link: 1.5 kg, Ixx=Iyy=0.029125, Izz=0.055225, 碰撞盒 0.47×0.47×0.11
- imu_link (0.015 kg) + imu_joint (revolute)
- 4× rotor (0.005 kg) 用 **revolute 关节** (可旋转), rotor_0/1 CCW, rotor_2/3 CW
- X 构型电机布局: rotor_0(0.13,-0.22) / rotor_1(-0.13,0.20) / rotor_2(0.13,0.22) / rotor_3(-0.13,-0.20), 高度 0.023
- world 初始位姿 `0 0 0.83 0 0 90°` — **yaw=90° 使模型朝北** (body x = NED x), 对齐 body 与 NED 坐标系 (Gazebo 默认模型朝东, 不旋转则水平位置控制方向错 90°); 高度 0.83m 悬空启动 (PX4 sitl_run.sh `-z 0.83`, 避免贴地接触弹射)

### 4. 必要桥接壳 (非算法, 伪装 PX4 必需)

| 模块 | 说明 |
|------|------|
| UDP/MAVLink 收发 | 监听 14556 (HIL_SENSOR/GPS), 发送 14557 (HIL_ACTUATOR), 与 MAVROS 通信 14560 |
| MAVLink 消息打包 | HEARTBEAT/ATTITUDE/LOCAL_POSITION_NED/SYS_STATUS/HIL_ACTUATOR_CONTROLS 等 (标准库 2026.6.6) |
| 指令处理 | COMMAND_LONG (arm/takeoff/land), SET_POSITION_TARGET_LOCAL_NED, SET_MODE 等 |

### 5. 关键移植修复记录 (v1.16.2 稳定版统一后)

| # | 修复 | 按 PX4 源码依据 | 说明 |
|---|------|----------------|------|
| 1 | vz GPS 修正 BUG | `src/modules/ekf2` (互补滤波) | `bs->vel_z = bs->vz_est` 覆盖了 HIL_GPS vd, 导致 GPS 修正项恒为 0, vz 变纯积分漂移振荡 → 改用独立字段 `gps_vd` 保存 HIL_GPS vd |
| 2 | 垂速限幅 constrain 语义 | `PositionControl::setVelocityLimits` + `math::constrain` | 原实现先钳上限再钳下限导致 rampup 早期 (vel_up 负) 恒钳 2.45; 改 PX4 else-if 链 `(val<min)?min:((val>max)?max:val)`, 允许中间值 |
| 3 | 起飞爬升速度 | `FlightTaskAuto` (`z_vel_constraint = mpc_tko_speed`) | takeoff ramp 用 **MPC_TKO_SPEED=1.5** (非 MPC_Z_VEL_MAX_UP=3.0), 爬升太快会过冲振荡 |
| 4 | landed 检测 | `vehicle_land_detected` + `flying_but_ground_contact` | `alt<0.15 && in_air` — 起飞前贴地不算 landed (状态机正常推进), 曾升空后掉回地面 → landed (停止控制, 防持续拉升振荡) |
| 5 | 水平位置缺失 | `ekf2` (经纬度→本地 NED) | `process_hil_gps` 原未设置 pos_x/pos_y (水平位置恒 0 → 水平控制失效漂移) → 按 WGS84 经纬度差计算本地 NED 坐标 |
| 6 | body 与 NED 坐标系 | PX4 SITL 模型朝向 | world 初始 yaw=90° 使模型朝北 (body x = NED x), 否则水平位置控制方向错 90° (追 y=东 却往南漂移) |
| 7 | mix 矩阵匹配模型物理 | `ActuatorEffectivenessRotors.cpp` | 有效性矩阵用模型实际电机位置 (iris.sdf.jinja 0.13/0.22, FLU→FRD), 而非固件 10015 参数 (0.1515/0.245, 版本数值不同步) |
| 8 | 起飞推力从 0 开始 | `MulticopterPositionControl.cpp` (`minimum_thrust = flying ? thr_min : 0`) | 起飞阶段 (非 flight) 最小推力 = 0, flight 后才用 MPC_THR_MIN |
| 9 | rampup 目标高度 | PX4 takeoff setpoint | rampup 用目标高度 (非保持当前), 靠位置误差 + vel_up 限幅控制爬升, 防过冲 |

## ✅ 测试验证结果 (稳定版 v1.16.2 移植后实测)

| 验证项 | 结果 |
|--------|------|
| 起飞悬停 | ✅ 模型升空悬停、**姿态完全水平 (w≈1, 从未翻倒)**、**零漂移 (x/y≈0)**、高度收敛趋向 2m 目标 |
| OFFBOARD 定点飞行 | ✅ 水平定点正确 (追目标 (1,1,2) 到达 x≈0.95, y≈1.39 目标附近)、OFFBOARD 高空接管成功 (模型从 4.2m 平滑回落目标高度) |

> **已知剩余优化项**: 高度环过冲振荡 (0.05↔5.4m) — 简化 vz 互补滤波 (加速度积分+GPS 修正) vs PX4 EKF2 完整卡尔曼滤波的差距. 模型姿态/水平控制完全稳定 (姿态水平、零漂移、水平定点正确), 完整移植 EKF2 可进一步消除高度振荡.

## 文件结构

```
simdrone/
├── mavlink_bridge.c              # MAVLink 桥接 (伪装 PX4, 标准库)
├── gazebo_link.c / gazebo_link.h # Gazebo 插件纯 C 核心 (标准库)
├── gazebo_mavlink_plugin.cpp     # Gazebo 插件 C++ 壳
├── models/iris_quadrotor/
│   ├── iris_quadrotor.sdf        # 四旋翼 SDF 模型
│   └── model.config              # 模型描述文件 (必需!)
├── worlds/iris.world             # Gazebo 世界文件
├── meson.build                   # 构建配置 (meson)
├── run.sh                        # 一键启动脚本
└── README.md                     # 本文件
```

## 通信细节

| 方向 | 端口 | 消息 | 频率 |
|------|------|------|------|
| Gazebo→Bridge | UDP 14556 | HIL_SENSOR (IMU) | 250 Hz |
| Gazebo→Bridge | UDP 14556 | HIL_GPS | 50 Hz |
| Bridge→Gazebo | UDP 14557 | HIL_ACTUATOR_CONTROLS | 100 Hz |
| Bridge↔MAVROS | UDP 14560 | 标准 MAVLink 遥测/指令 | 可变 |

## 自定义端口

```bash
# 指定端口: 传感器:执行器:MAVROS
./mavlink_bridge -p 14556:14557:14560
```