# px4c

**Repository Path**: bef/px4c

## Basic Information

- **Project Name**: px4c
- **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-17
- **Last Updated**: 2026-09-20

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# px4c — 纯 C 语言的 SITL 飞控（gz sim 仿真）

px4c 是一个用**纯 C 语言**实现的精简 SITL（Software-In-The-Loop）飞控，配合
**gz sim**（Gazebo / gz-sim）仿真器，通过 **MAVLink 标准 C 库**完成**解锁、起飞、
指点飞行、任务飞行、降落**的完整流程，使用 **Meson** 构建。仿真侧用一个轻量
**gz-transport → MAVLink HIL 桥**（`sim/gz_bridge_px4c.cpp`）把 gz sim 的传感器
话题转成 HIL 消息喂给飞控，飞控回发的执行器指令再转回 gz sim 的
`command/motor_speed` 话题驱动 x500 四旋翼模型。

## 源码位置（移植来源，下次继续时先看这里）

| 路径 | 作用 |
|------|------|
| `~/other/PX4-Autopilot/` | PX4 官方源码：`src/modules/mc_pos_control`、`mc_att_control`、`commander`、`navigator` 等模块的**算法移植来源**；`Tools/simulation/gz/models/` 提供 **x500 模型**（`GZ_SIM_RESOURCE_PATH` 指向此处）；`ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500` 提供 x500 的 **CA 分配参数**（混控标定依据） |
| `~/other/PX4-SITL_gazebo-classic/` | Gazebo Classic 仿真插件源码（历史参考；现仿真改用 gz sim + 自写桥，不再依赖该插件） |
| `/opt/ros/humble/include/mavlink/v2.0/` | **标准 MAVLink C 库**（ROS humble 自带，v2 dialect），px4c 的 MAVLink 编解码全部用它，不再自研 |

- 移植原则：飞控核心算法忠实移植为纯 C；MAVLink 协议层使用**标准库**
  （`mavlink_parse_char` / `mavlink_msg_*_pack` / `mavlink_msg_*_decode`）；
  仿真侧保留 C++（gz-sim 插件/桥均为 C++，技术上限，非取舍问题）。

## 移植进度

已完成的移植（对应 PX4 模块）：

| PX4 模块                | px4c 对应实现        | 说明                          |
|-------------------------|----------------------|-------------------------------|
| simulator_mavlink       | `src/fc.c`           | UDP 14560 + MAVLink v1/v2 编解码 |
| commander（状态机）      | `src/fc.c`           | **补全移植**：预飞检查（含估计器 data_good）、DO_SET_MODE 完整解析（OFFBOARD/AUTO/TAKEOFF/LAND）、RTL/SET_HOME、数据链故障保护（COM_DL_LOSS_T=10s）、落地限制解锁 |
| navigator（任务飞行）    | `src/fc.c`           | **移植**：MISSION_COUNT/ITEM_INT 协议、AUTO.MISSION 逐航点飞行（最多 16 航点）+ **巡航速度前馈（MPC_XY_CRUISE=5）**、到达判定 xy/z 分离（dist_xy≤acc_rad && dist_z≤alt_rad） |
| mc_pos_control          | `src/control.c`      | **完整移植** `PositionControl.cpp`：位置/速度/加速度级 + `ControlMath` + **速度前馈（navigator 巡航）** + **TakeoffHandling**（状态机 + ramp：MPC_TKO_SPEED=1.5/MPC_TKO_RAMP_T=3s） + **landing 平滑（MPC_LAND_SPEED=0.25，须低于自动解锁判据 |vz|<0.5，见 2026-09-20 修复记录）** |
| mc_att_control          | `src/control.c`      | **完整移植** `AttitudeControl.cpp`：reduced desired attitude、yaw weight、yawspeed 前馈、rate limit（Brescianini 2013）+ `matrix::Quaternion` 工具 |
| mc_rate_control         | `src/control.c`      | **完整移植** `RateControl.cpp`：PID + FF 前馈、D 项（角加速度）、i_factor 大误差降权、积分限幅、饱和条件积分；**输出物理力矩（Nm）** |
| control_allocator       | `src/control.c`      | **移植** `ActuatorEffectivenessRotors` 几何 + `ControlAllocationSequentialDesaturation` 饱和处理（desaturateActuators 沿推力方向平移保力矩）：力矩 Nm → 电机差（悬停点线性化标定） |
| ekf2（姿态部分）        | `src/ekf_att.c`      | **移植 EKF2 姿态估计核心**：6 维误差状态（四元数+陀螺偏置）、IMU 预测 + 磁力计 **yaw 专用修正**（ATT_W_MAG）+ 比力加速度计更新（ATT_W_ACC）+ 低自旋陀螺偏置估计（ATT_W_GYRO_BIAS）+ innovation gate + m_earth 动态标定 |
| mavlink C library       | **标准库**（ROS humble `/opt/ros/humble/include/mavlink/v2.0/`） | 使用官方 MAVLink v2 C 库（`mavlink_parse_char` / `pack` / `decode`），不自行实现协议 |
| 传感器/估计器            | `src/ekf_att.c` + `src/estimator.c` + `src/px4_ekf_wrap.cpp` | 姿态由 **EKF** 估计（HIL 真值角速度/加速度 + HIL_SENSOR 磁力计 yaw 锁定）；**位置/速度由 PX4 EKF2（完整，`px4_ekf_wrap.cpp` 包装）融合 HIL_GPS(116) GPS 数据**——`gpos/gvel/gpchk` 全部通过（GPS 融合正常） |

> 历史记录（2026-08-17，自研协议层时期）：早期版本曾自研 MAVLink 编解码，修复过
> 以下"能跑但协议不兼容"的隐性 bug；**现协议层已改用标准 MAVLink C 库**，这些
> 问题由官方库保证正确（CRC、v2 截断、线布局均由库处理）：
>
> 1. **CRC 表错误**：标准**反射式 CRC-16/X.25**（poly `0x8408`）；
> 2. **v2 CRC 顺序错误**：`len, incompat, compat, seq, sysid, compid, msgid(3B), payload, crc_extra`；
> 3. **v2 载荷截断**：gazebo 插件发送 v2 帧时**裁掉尾部零字节**
>    （HIL_STATE_QUATERNION 实际只有 47 字节）——标准库 decode 对缺失字段按 0 返回；
> 4. **线布局按类型大小排序**（MAVLink 生成代码的特性，非 XML 顺序）：
>    - HEARTBEAT = `custom_mode(u32)` 在最前，再是 5 个 u8；
>    - HIL_ACTUATOR_CONTROLS = 新版 81 字节：`time_usec@0, flags@8, controls[16]@16, mode@80`；
>    - HIL_STATE_QUATERNION = 新版 64 字节：`vx/vy/vz` 为 `int16@48/50/52`；
>    - SET_POSITION_TARGET_LOCAL_NED = `time_boot_ms, x..yaw_rate(11×f32), type_mask(u16), coordinate_frame(u8)`；
> 5. **COMMAND_LONG 解码顺序**：线上为 `param1..7, command, target_system, target_component, confirmation`；
> 6. **位置换算漏了度→弧度**：`x = dlat(度) × EARTH_R` 被放大约 57 倍，导致飞机误判位置而失控，
>    已改为 `dlat × π/180 × EARTH_R`。

> 排查技巧（下次遇到"仿真不动/乱飞"先查这些）：用 `build/test_flight --verbose` 或
> 在 FC 启动时加 `--verbose` 直接观察收发帧（`RX msgid=... len=...`）；需要独立抓包
> 时可用 `tcpdump -i lo udp port 14560 -X`；核对插件侧
> `/opt/ros/humble/include/mavlink/v2.0/common/mavlink_msg_*.h` 里
> `_mav_put_*` 的偏移（与标准库一致）。插件 UDP 模式本地端口为 0（随机），
> 但发送目标是 `mavlink_udp_port=14560`。

> 修复记录（2026-08-26，GPS 融合未启动）：px4c 曾用 gazebo 模拟时间戳
> （107 IMU 的 `hs.time_usec`、116 GPS 的 `gps.time_usec`）作为 EKF2 时间戳，
> 且 116/107 时间戳抖动不同步（首条 116 晚于 107 约 104ms）→ EKF2
> `setGpsData` 内部减 `gps_delay_ms(110ms)+dt/2` 后 GPS 时间戳仍 ≥ `imu_delayed`
> → `pop_first_older_than` 失败 → GPS 数据被丢弃 → `gpos/gvel/gpchk` 全 0
> （GPS 融合从未启动）。按 PX4 源码语义（`mavlink_receiver.cpp`：
> `sensor_gps.timestamp = hrt_absolute_time()` 接收时刻，`hil_gps.time_usec`
> 只作 UTC 记录），将 IMU/mag/GPS 全部改用**接收时刻 `now_us()`**（IMU/GPS
> 同步）——修复后 `gpos=1 gvel=1 gpchk=1`，ros2/mavros 解锁、起飞、
> 指点飞行全部验证通过。

> 修复记录（2026-08-31，x500 混控标定——起飞悬停 yaw 振荡→翻滚失控→EKF 位置
> 爆炸）：世界实际加载 `model://x500`（`GZ_SIM_RESOURCE_PATH` 下），但混控
> 系数仍是 **iris** 标定，量纲错配：roll `τ/2.494`（应为 11.05，过冲 4.4×）、
> pitch `τ/1.542`（应为 6.84，过冲 4.4×）、yaw `mix_scale=0.05`（应为 0.38，
> 弱控 7.6×——压不住漂移，之前怎么调 mix_scale 都不行的根因）。按 x500 悬停
> 点线性化重标定（dT/dout=13.16 N/unit；roll 臂 0.84 m、pitch 臂 0.52 m、
> yaw 4·km=0.20，km 取官方 4001_gz_x500 CA 参数，与 PX4 分配矩阵等效被控
> 对象一致）。**注意**：x500_base 模型里转子真实坐标是 ±0.174（正方形）、
> 物理 `momentConstant=0.016`，但 gz-sim 电机物理的 yaw 力矩
> `= −turningDirection·thrust·mc`（正比推力、非转速），标定必须与 PX4 CA
> 参数对齐而非模型原始常数，否则 px4c 沿用 PX4 速率增益会过冲振荡。
> 修复后 `test_flight.py` 4/4 PASS、60s 定点悬停 yaw 53°±3° 稳定、
> roll 最大 13°（仅离地瞬态）、悬停 thr 0.767（标定 0.769）、EKF 水平偏差
> ≤1.35 m。另查证"thr 字段==pos.x（236.820）"为日志里 `GPS data too fast
> 236xxxxxx` 时间戳数字的巧合匹配——对 52 万行旧日志全量核验，printf 参数
> 无错位，thr 从未越界（|thr|≤1.0）。

> 修复记录（2026-09-03，gz 桥磁计双重旋转——起飞后 ~8s 机体漂移/翻滚→坠毁
> 400 m 外→EKF 收碰撞尖峰发散 1e18）：gz-sim 磁计系统（Magnetometer.cc 里
> `SetWorldMagneticField`+`SetWorldPose`）已把世界磁场按世界→机体旋转到传感器
> 机体系（FLU）后发布。PX4 官方 `GZBridge::magnetometerCallback` 因此只做
> 固定轴重排 `x=-fy, y=-fx, z=fz`（FLU→FRD），**不做任何姿态旋转**。本桥
> （gz_bridge_px4c.cpp:254-262）此前对已属机体系的磁场再乘 `q_nb^-1`（把机体
> 磁场当世界系）= 双重旋转：机体磁场被恒等地映回 NED 世界磁场，EKF 却按机体
> 磁场使用。起飞时机体水平（机体≈世界、yaw 恒定）误差小，前 ~8s 悬停正常；
> 飞动后 yaw/tilt 变化，注入误差随姿态增大 → EKF 倾角估计被拉偏（实测真值
> roll ~6° 却估 26-28°）→ 控制按错误 roll 反方向倾斜 → 机体翻覆坠毁 → EKF
> 再收碰撞尖峰发散 1e18（1e18 是症状，根因在此双重旋转）。此前 "(-fy,-fx,fz)
> + q_nb^-1 → decl=-92.54" 是"轴重排+双重旋转"的错误组合；正确是仅重排、不
> 旋转（与官方一致）。另查证 `baro data too fast`（单帧 8ms < 8.7ms 门限）
> 为无害红鲱鱼——与发散无因果。修复后 `hold_test.py` 60s 定点悬停最大水平
> 偏移 0.02 m（修复前 400 m 坠毁）、roll −0.8~+0.1° / pitch −0.2~+0.4°、
> alt z=−2.03 m 稳定、全程 `magf=0`（磁融合未失败）、`starting mag 3D
> fusion` 后无 `stopping mag`、全程无 1e18 发散、干净落地 disarm。

> 修复记录（2026-09-07，落地容差过宽——飞机悬停在离地 8cm 即判"已落地"）：
> FC 落地检测条件 `g_state.z > (g_ground_z - 0.15f) && fabsf(g_state.vz) < 1.0f`
> 容差 0.15m 过宽：飞机在离地 8cm 处悬停（速度≈0）即触发 disarm，用户观察到
> "发降后代还在悬停"。收紧为 `g_ground_z - 0.03f` + 速度 0.5m/s，要求飞机
> 真降到离地 3cm 以内、速度低于 0.5m/s 才判定落地解锁。同步收紧测试脚本：
> test_flight.py `z > -0.3` → `z > -0.03`，mavros_cli_test.sh `Z < 0.5` →
> `Z < 0.15`，hold_test.py 改为主动等 z < 0.03 循环。修复后 pymavlink 4/4 PASS
> （touchdown z=0.00 精确触地，修复前 z=-0.082 即离地 8cm）。

> 修复记录（2026-09-10，ROS2 降落失败——FC 从未收到 NAV_LAND）：
> 根因是 FC 用**一个共享的 `mavlink_status_t` 解析器**处理同一个 UDP socket
> 上的两路独立来源（bridge@14562 以 ~250Hz 洪泛 107/115/113 + MAVROS@14561
> 的控制帧）。MAVLink 解析器是有状态字节流状态机，两路帧交错时一个来源留下
> 的半帧让共享状态失步，另一来源的帧被错位解析。实测后果：MAVROS 发了 3×
> msgid=76（COMMAND_LONG）但 FC 组装出 **0× CMD 300（NAV_LAND）**——降落
> 指令被吞，飞机永不进 LANDING、停在 2m 悬停；还解出垃圾帧 `SET_MODE
> custom=100925440`（测试只发过 custom=6）把状态机打回 STANDBY；TAKEOFF
> param7=2.0 被解析成 0.20。修复：按 (源 IP:端口) 分通道、各来源独立解析器
> （fc.c `rx_channel_for`），与 PX4 官方 mavlink_receiver 每 remote 独立解析
> 状态的设计一致。同步修复 start_all.sh 残留清理：按 ps 完整 args 匹配
> （`gz sim|gz_bridge|px4c_fc|mavros_node`）而非 comm——gz sim 子进程 comm
> 是 `ruby`、bridge 的 comm 带尾下划线，按 comm 匹配全漏杀，残留旧 sim 在同
> 一 DDS domain 交替发布同名 topic 污染 bridge 输入。修复后 ROS2 全链路
> 7/7 PASS：ARM → OFFBOARD → TAKEOFF（z=1.98m，param 正确 2.00）→ HOLD
> （最大偏差 0.03m）→ GUIDED 指点 → LANDING → 真值落地 wz=-0.013m
> （/tmp/gtruth.log，离地 1.3cm 内）→ AUTO-DISARM；`custom=100925440`
> 垃圾帧 0 次、`all height sources failing` 0 次。

> 修复记录（2026-09-20，ROS2 GUIDED 指点坐标框 + 0.2m 降落弹跳不解锁）：
> ROS2 全链路收尾时解决两个问题。（1）**指点坐标框**：本机 MAVROS fork 的
> setpoint_raw 插件把 `/mavros/setpoint_raw/local` 的 `PositionTarget` 按
> **ENU**（east/north/up）解释再转 NED，即 FC 的 (N,E,D) = (in.y, in.x, −in.z)
> （coordinate_frame=1）。之前直接发 NED（z 负=下），`z=−2.29` 被取反当"向下
> 2.3m"，飞机被直接压向地面——不是协议/状态机 bug，是坐标框误用。改按 ENU
> 发布（北向发 y 槽、高度发 z=+AGL）后：FC 正确解出 `GUIDED target
> (0.7, 0.0, −1.9)`（北向 0.7m）、北向跟随收敛（FC pos 北分量 −0.10→0.87
> m）、高度无漂移（gtruth 真值 wz 稳 1.97–2.02 m）、收 msgid=84 共 69 帧、
> 无发散。（2）**0.2m 悬停降落弹跳、不自动解锁**：`MPC_LAND_SPEED=0.7`
> 比 fc.c 自动解锁判据 `|vz|<0.5` 还快——接地速度超限，飞机弹跳反复、
> 解锁条件抓不住（2m 降落偶成、0.2m 悬停降落必弹，弹跳期间 wz 在
> −0.39↔+0.29 间振荡）。修复：`control.c` 下降速度限 0.7→0.25 m/s（低于
> 判据门限留足裕量；PX4 用 LandDetector 状态机判地，px4c 简化为速度判据，
> 故接地速度必须低于判据门限）。修复后 0.2m 悬停→land→4s 内
> `landed, auto-DISARMED`，wz 平滑落地（降落窗口跨度 0.000，无弹跳）。

> 修复记录（2026-09-20，定点飞行间歇发散——CPU 饱和 + 控制热路径 I/O）：
> 定点悬停间歇失稳（同一二进制：好 run 漂移 ≤0.3m，坏 run 40m 漂移甚至
> 278m 数值爆炸、EKF zacc 到 1e21、GNSS fusion timeout 反复触发）。
> A/B 对照定案：**09-10 稳定版二进制（4e8991f）在当前环境同样崩**（wz
> 跨度 3.57m、xy 40m、GNSS timeout×45），排除当日代码 diff；EKF2 源码
> 仓库、bridge 二进制（09-07）、gz 包版本均确认未变。环境侧：4 核机器
> 负载 13.6（Firefox/GNOME/QQ 抢核），gz sim GUI 渲染 + `-v 4` 全量
> 日志把实时步挤爆 → HIL 时间戳抖动 → EKF 垂直估计先坏（zacc 0→−220
> m/s²）→ 位置环追噪声、飞机物理飘走（wz 2↔6m 限幅振荡）→ 飞离原点后
> NavSat alt 跳到 314m → EKF 数值爆炸。两个修复：（1）`start_all.sh`
> **默认 headless**（`-s -v 1`，无 3D GUI；`PX4C_GUI=1` 开 GUI 需留足
> CPU）——headless 后同一测试 wz 跨度 3cm、xy ≤2cm、GNSS timeout=0，
> `start_all.sh` 固化版复测同样通过；（2）摘除 control.c 里 09-14 遗留
> 的 250Hz 速率环诊断（每 tick `fprintf` 到 `/tmp/ratedbg.log`，实测
> 写到 1.5GB）——控制热路径 I/O 抖动是第二放大器，摘除后最坏情形从
> 278m 爆炸收敛到 4m 振荡。**结论：px4c 定点控制律本身没问题；定点
> 测试必须在低 CPU 负载环境跑，GUI 渲染 + 高负载是已知干扰源。**

## 背景与范围

- `PX4-Autopilot`（约 2644 个 C/C++ 文件、30MB）体量巨大，且依赖 C++ 的
  类/模板/STL 以及 NuttX 实时系统层，**不存在"机械翻译成纯 C"的可行路径**。
- 因此本仓库将 SITL 飞行真正需要的核心算法**忠实移植为纯 C**，算法结构与
  PX4 的 `mc_pos_control` / `mc_att_control` 级联控制一致（见上表）。
- Gazebo Classic 的插件 API 本身是 C++，插件侧（`PX4-SITL_gazebo-classic`）保留
  原样作为**仿真器**使用，不做纯 C 化（技术上限，非取舍问题）。

## 目录结构

```
px4c/
├── meson.build              # Meson 构建配置（px4c_fc + test_flight，include 标准 MAVLink 库）
├── src/
│   ├── fc.c                 # 主循环：UDP、状态机、命令处理、遥测、故障保护、任务飞行
│   ├── control.c / .h       # 位置/姿态/速率级联控制（PX4 完整移植）+ TakeoffHandling/landing + ControlAllocator（几何分配 + 饱和处理；混控按 x500 物理标定）
│   ├── estimator.c / .h     # 姿态估计器（attitude_estimator_q 互补滤波移植，保留作对比）
│   ├── ekf_att.c / .h       # EKF2 姿态估计核心（6 维误差状态：IMU 预测 + 磁力计 yaw 专用修正 + 比力加速度计更新）
│   ├── px4_ekf_wrap.cpp / .h # PX4 EKF2 完整（extern "C" 包装，位置/速度 GPS 融合）
│   └── test_flight.c        # 纯 C 飞行测试客户端（标准 MAVLink 库）
├── sim/
│   ├── gz_bridge_px4c(.cpp) # gz-transport → MAVLink HIL 桥（C++）：订阅 x500 的 imu/mag/navsat/pose 话题 → 组 HIL 消息发 UDP 14560；收 105（执行器）→ 发 command/motor_speed
│   └── worlds/gz_px4c.sdf   # gz sim 世界（地面 + model://x500 + IMU/NavSat/Magnetometer/AirPressure/Sensors 系统）
├── scripts/
│   ├── start_all.sh         # 健壮启动脚本（pidfile 精确 kill，无僵尸进程）
│   ├── run_sitl.sh          # 一键启动（Gazebo Classic 时代脚本，现实际用下方"运行"步骤的 gz sim 三进程方式）
│   ├── render_iris.sh       # 渲染 iris 模型（历史；现用 x500）
│   ├── test_flight.py       # pymavlink 飞行测试：arm→takeoff→指点(5,0)→land，逐级校验
│   ├── takeoff_test.py      # pymavlink 起飞测试（baseline-relative AGL 校验）
│   ├── mavros_test.py       # ROS2/MAVROS 飞行测试：arm→takeoff→hold→guided→land
│   ├── mavros_cli_test.sh   # ROS2/MAVCLI 飞行测试（完整链路）
│   ├── hold_test.py         # 定点悬停 60s 专项（验证长悬停 yaw 稳定）
│   ├── guided_move_test.py  # GUIDED 指点主动位置环决定性测试（ENU 坐标框）
│   ├── tko_02_retest.py     # 0.2m 起飞复测（复刻失败路径：2m 起飞→LAND→重解锁→0.2m）
│   └── hold_crash_repro.py  # 长悬停发散复现（0.8m 悬停监控姿态发散/坠地）
└── README.md
```

## 环境依赖

- Ubuntu 22.04，`gcc`、`meson`、`ninja`
- **gz sim**（Gazebo，`gz` 命令 + gz-sim 系统插件库：physics/user-commands/
  scene-broadcaster/imu/navsat/magnetometer/air-pressure/sensors/
  multicopter-motor-model），ROS humble 环境可用
- **x500 模型**：`~/other/PX4-Autopilot/Tools/simulation/gz/models`（启动时需
  `export GZ_SIM_RESOURCE_PATH=<该目录>`）
- **标准 MAVLink C 库**：`/opt/ros/humble/include/mavlink/v2.0/`（ROS humble 自带，
  或在 PX4-Autopilot 源码 `mavlink/include/mavlink/v2.0/`）
- 桥与测试：`sim/gz_bridge_px4c.cpp`（C++，g++ 编译）；飞行测试为
  `scripts/test_flight.py` / `scripts/hold_test.py`（Python + pymavlink），
  另有纯 C 测试客户端 `build/test_flight`（标准 MAVLink 库）

## 构建

```bash
# 1) 构建 C 飞控与 C 测试客户端（Meson）
meson setup build
meson compile -C build        # 产物: build/px4c_fc, build/test_flight

# 2) 编译 gz→MAVLink HIL 桥（C++，一次性）
g++ -O2 -o sim/gz_bridge_px4c sim/gz_bridge_px4c.cpp -std=c++17 \
    -I/usr/include/gz/transport13 -I/usr/include/gz/msgs10 -I/usr/include/gz/utils2 \
    -lgz-transport13 -lgz-msgs10 -lprotobuf
```

## 运行与测试

### 一键启动（推荐）

```bash
bash scripts/start_all.sh   # 自动清理僵尸进程、启动 gz sim → bridge → FC → MAVROS
```

### 手动启动（三进程）

启动三进程（均后台、日志入 `build/sitl_logs/`）：

```bash
export GZ_SIM_RESOURCE_PATH=~/other/PX4-Autopilot/Tools/simulation/gz/models
export GZ_SIM_SYSTEM_PLUGIN_PATH=/tmp:/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins
gz sim sim/worlds/gz_px4c.sdf -r -v 1 &          # 仿真世界（x500，等待 20s 传感器初始化）
./sim/gz_bridge_px4c &                            # gz 话题 ⇄ MAVLink HIL（UDP 14560）
./build/px4c_fc --port 14560 --verbose &          # 飞控
```

> **关键：gz sim 启动后必须等待 20+ 秒**，确保传感器话题（imu/mag/navsat）出现发布者
> 再启动 bridge，否则 bridge 的 sensor flags 全 0、FC 收不到 HIL 数据。
> 验证：`gz topic -l | grep imu` 应有输出；bridge 日志 `bridge flags: imu=1 mag=1 nav=1 pose=1`。

### MAVROS 启动

```bash
source /opt/ros/humble/setup.bash
ros2 run mavros mavros_node --ros-args \
    --params-file /tmp/mavros_udp.yaml &
# 等待 15s 连接，验证：
ros2 topic echo /mavros/state --once | grep connected   # 应显示 connected: true
```

`/tmp/mavros_udp.yaml` 内容（UDP 模式）：
```yaml
/**:
  ros__parameters:
    fcu_url: "udp://:14561@127.0.0.1:14560"
    gcs_url: "udp://@127.0.0.1:14550"
    tgt_system: 1
```

### pymavlink 测试

```bash
python3 scripts/test_flight.py   # arm→takeoff 2m→指点(5,0)→land，逐级 PASS/FAIL
python3 scripts/hold_test.py     # 定点悬停 60s 专项（验证长悬停 yaw/姿态/EKF 稳定性）
python3 scripts/takeoff_test.py 0.2 60   # 0.2m 起飞（baseline-relative AGL 校验）
```

### ROS2/MAVROS 测试

```bash
bash scripts/mavros_cli_test.sh  # 完整链路：arm→set_mode offboard→takeoff→hold→guided→land
```

- **ARM 解锁**：COMMAND_LONG(400)，校验 ACK + HEARTBEAT armed 位；
- **SET_MODE OFFBOARD**：msg 11，PX4 标准模式切换；
- **TAKEOFF 起飞**：COMMAND_LONG(22)，校验 LOCAL_POSITION_NED z；
- **GUIDED 指点飞行**：SET_POSITION_TARGET_LOCAL_NED 飞到 (5, 0)；
- **LAND 降落**：COMMAND_LONG(21)，校验落地并自动解锁；
- 结束：`pkill -f "gz sim"; pkill -f gz_bridge_px4c; pkill -f px4c_fc`。
- **注意**：单实例运行——同时只能有一个 px4c_fc 进程绑定 UDP 14560，否则两进程抢包导致收 0 RX。用 `start_all.sh` 确保无僵尸进程。

## 网络与协议

| 端口 | 用途 |
|------|------|
| 14560 | 飞控 UDP 监听（HIL 传感器 + GCS 命令/遥测，SO_REUSEADDR\|SO_REUSEPORT 复用） |
| 14561 | MAVROS → FC（SET_MODE/msg 11、COMMAND_LONG、SET_POSITION_TARGET 等） |
| 14562 | gz-transport HIL 桥 → FC（HIL_SENSOR/107 + HIL_STATE/115 + HIL_GPS/113） |

- 飞控绑定 `0.0.0.0:14560`（`SO_REUSEADDR|SO_REUSEPORT`），仿真插件向
  `127.0.0.1:14560` 发送 HIL 状态，飞控向插件源地址回发执行器指令；
- 解锁后 `HIL_ACTUATOR_CONTROLS.mode` 置 `MAV_MODE_FLAG_SAFETY_ARMED`（插件据此
  使能电机），降落/解锁后清除；
- **MAVLink v2**（v1 兼容）：标准库 `mavlink_parse_char` + `mavlink_msg_*_pack` /
  `decode`，自动支持 CRC 校验、v2 载荷截断；
- **MAVROS 兼容**：
  - `MAVLINK_MSG_ID_SET_MODE`(msg 11)：FC 直接处理模式切换（PX4 标准）；
  - `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES`(520)：FC 回复 `AUTOPILOT_VERSION`(msg 148)
    + `MAV_RESULT_ACCEPTED`，不回 `COMMAND_ACK`（避免 MAVROS cmd 插件 Promise 崩溃）；
  - 默认 CMD 返回 `MAV_RESULT_ACCEPTED`（PX4 语义）；
  - **指点坐标框**：`/mavros/setpoint_raw/local` 的 `PositionTarget` 被本机
    MAVROS 插件按 **ENU** 解释（coordinate_frame=1），FC (N,E,D) =
    (in.y, in.x, −in.z)——发北向用 `y` 槽、发高度用 `z=+AGL`；直接发 NED
    （z 负=下）会被取反，把飞机压向地面；
- 遥测消息集（TX）：HEARTBEAT、ATTITUDE、LOCAL_POSITION_NED、SYS_STATUS、
  GPS_RAW_INT、GLOBAL_POSITION_INT、VFR_HUD、BATTERY_STATUS、HOME_POSITION、
  HIL_ACTUATOR_CONTROLS、COMMAND_ACK、STATUSTEXT、AUTOPILOT_VERSION、
  MISSION_REQUEST/ACK/CURRENT/COUNT（共 18 种）；
  接收（RX）：HIL_STATE_QUATERNION、HIL_SENSOR、HIL_GPS、HEARTBEAT、COMMAND_LONG、
  SET_MODE、SET_POSITION_TARGET_LOCAL_NED、MISSION_COUNT、MISSION_ITEM_INT、
  MISSION_SET_CURRENT（共 10 种）。

## 控制律（对应 PX4 算法，完整版移植）

```
位置误差 --P(+navigator 巡航前馈)--> 速度指令 --PID(积分+ARW)--> 加速度指令 --> 推力向量(倾斜限幅+垂直优先饱和)
  --> 期望姿态(四元数) --四元数姿态控制(yaw weight)--> 角速率指令(含 yawspeed 前馈)
  --> 角速率 PID(完整版: D+FF+抗饱和积分) --> 物理力矩(Nm) --> ControlAllocator(rotor 几何分配 + desaturate 饱和处理) --> 电机
```

**位置/速度/加速度级 —— PX4 `mc_pos_control/PositionControl.cpp` 完整移植**
（与 PX4 源码逐函数对应，含 `ControlMath.cpp` 工具函数）：

| PX4 函数 | px4c 对应 | 说明 |
|---|---|---|
| `PositionControl::_positionControl()` | `ctrl_update` 内 `_positionControl` 段 | 位置 P → 速度指令；`ControlMath::constrainXY` 水平速度限幅（位置项优先于前馈项）、Z 限幅 |
| `PositionControl::_velocityControl()` | `ctrl_update` 内 `_velocityControl` 段 | **速度 PID**（P/I/D 三项，D 项用测量加速度）；垂直积分限幅 ±g；垂直推力优先 + 水平余量分配；Rundqwist 跟踪**抗饱和 ARW** |
| `PositionControl::_accelerationControl()` | `ctrl_update` 内 `_accelerationControl` 段 | 期望体轴 Z；`ControlMath::limitTilt` 倾斜限幅；悬停推力模型 `thrust_ned_z = acc_z·Th/g − Th`；推力向量 → 期望姿态 |
| `ControlMath::constrainXY` | `constrain_xy()` | 2D 向量限幅（含二次方程通解） |
| `ControlMath::limitTilt` | `limit_tilt()` | 体轴 Z 倾斜限幅 |

**姿态级 —— PX4 `mc_att_control/AttitudeControl.cpp` 完整移植**
（Brescianini/Hehn/D'Andrea 2013《Nonlinear Quadrocopter Attitude Control》，
与 PX4 源码逐行对应）：

| PX4 函数 | px4c 对应 | 说明 |
|---|---|---|
| `AttitudeControl::setProportionalGain()` | `ctrl_init`（`att_p[3]`/`yaw_w`） | yaw 增益除以 `yaw_w` 以补偿 yaw 降权 |
| `AttitudeControl::update()` | `attitude_update()` | ① reduced desired attitude（忽略偏航优先横滚/俯仰，`qd_red = Quat(e_z, e_z_d)·q`）；② 提取 delta yaw 并按 `yaw_w` 缩放重组；③ 姿态误差 `qe = q⁻¹·qd` → `eq = 2·canonical(qe).imag`；④ 角速率指令 = `eq ∘ gain` + yawspeed 前馈（世界 z 轴投影到机体系）；⑤ rate limit 限幅 |
| `matrix::Quaternion` 工具 | `quat_from_vecs/quat_inversed/quat_canonicalize/quat_imag/quat_dcm_z` | 两向量最短旋转（含 180° 角点）、共轭逆、canonical 归一、虚部、`dcm_z()` |

**速率级 —— PX4 `lib/rate_control/RateControl.cpp` 完整移植**：

| PX4 函数 | px4c 对应 | 说明 |
|---|---|---|
| `RateControl::update()` | `ctrl_update` 内 `_rateControl` 段 | `torque = P·err + I − D·ang_accel + FF·rate_sp`（FF 默认 0，D 用角速度差分近似角加速度） |
| `RateControl::updateIntegral()` | 同上 | i_factor 大误差降权（400°/s 归一）、±`rate_int_lim` 限幅、饱和时条件积分（正饱和只减、负饱和只增） |

**估计器 —— EKF2 姿态估计核心（`src/ekf_att.c`，替代互补滤波）**：

| PX4 函数 | px4c 对应 | 说明 |
|---|---|---|
| `Ekf::predictState` | `ekf_att_predict()` | IMU 预测：陀螺偏置修正 + 四元数积分（AxisAngle）+ 状态转移 F 传播协方差 P |
| 磁力计修正（yaw 专用） | `ekf_att_update_mag()` | 对齐 `attitude_estimator_q`：mag_earth → `wrap_pi(atan2(mag_earth.y, mag_earth.x) − mag_decl)`，corr += q⁻¹(0,0,−mag_err)×w_mag×gainMult（高自旋 gainMult≤10）——只修正 yaw，不拉偏 roll/pitch |
| 比力加速度计更新 | `ekf_att_update_acc()` | 对齐 `_accel` 语义（HIL_SENSOR 比力）+ k=q⁻¹(0,0,1) 观测模型；EKF 协方差加权更新（H=−[h×]、K=PHᵀ(HPHᵀ+R)⁻¹）+ innovation gate |
| 陀螺偏置估计 | `ekf_att_update_mag()` | 低自旋（<0.175 rad/s）时 bias += corr×w_bias×dt，±ATT_BIAS_MAX（yaw bias 由磁力计收敛） |
| 权重参数 | `w_mag=0.1` / `w_acc=0.2` / `w_gyro_bias=0.1` / `bias_max=0.05` | ATT_W_MAG / ATT_W_ACC / ATT_W_GYRO_BIAS / ATT_BIAS_MAX；m_earth 动态标定（初始姿态 + HIL 磁力计） |

**参数对照（`ctrl_init`，对应 PX4 参数，全部对齐 PX4 默认值）**：

| px4c 字段 | 值 | PX4 参数 |
|---|---|---|
| `pos_p` | (0.95, 0.95, 1.0) | MPC_XY_P / MPC_Z_P |
| `vel_p` / `vel_i` / `vel_d` | (1.8,1.8,4.0) / (0.4,0.4,1.0) / (0.2,0.2,0.0) | MPC_XY/Z_VEL_P_ACC、MPC_XY/Z_VEL_I、MPC_XY/Z_VEL_D_ACC |
| `att_p`（yaw 已 ÷yaw_w） | (6.5, 6.5, 2.8/0.4) | MC_ROLL_P / MC_PITCH_P / MC_YAW_P |
| `yaw_w` | 0.4 | MC_YAW_WEIGHT |
| `rate_limit` | (3.84, 3.84, 3.49) rad/s | MC_ROLLRATE_MAX / MC_PITCHRATE_MAX / MC_YAWRATE_MAX |
| `tilt_lim` / `tilt_lim_lnd` | 45° / 12° | MPC_TILTMAX_AIR / MPC_TILTMAX_LND（起飞/降落时） |
| `thr_min` / `thr_max` / `thr_xy_margin` | 0.12 / 1.0 / 0.3 | MPC_THR_MIN / MPC_THR_MAX / MPC_THR_XY_MARGIN |
| `vel_lim_hor` / `vel_lim_up` / `vel_lim_down` | 12 / 3 / 1.5 m/s | MPC_XY_VEL_MAX / MPC_Z_VEL_MAX_UP / MPC_Z_VEL_MAX_DN |
| `hover_thrust` | 0.769（`--hover` 0.76） | MPC_THR_HOVER（x500 悬停转速标定） |
| `mix_scale` | 0.38 | 无直接对应——yaw 混控量纲 = 1/(4·km·dT/dout)，按 4001_gz_x500 CA 参数标定 |
| `takeoff_speed` / `takeoff_ramp_time` | 1.5 m/s / 3 s | MPC_TKO_SPEED / MPC_TKO_RAMP_T |
| `takeoff_spoolup_time` | 1 s | COM_SPOOLUP_TIME |

- 悬停油门 `hover_thrust ≈ 0.769`（对应 **x500** 模型 `motorConstant=8.54858e-6`、
  `maxRotVelocity=1000 rad/s`、总质量 2.064 kg 的悬停转速：
  `sqrt(g·m/(4·k·ωmax²))`）；可用 `--hover` 参数覆盖；
- 混控几何：rotor_0=FR(+x−y)、rotor_1=BL(−x+y)、rotor_2=FL(+x+y)、rotor_3=BR(−x−y)，
  旋转方向 0/1 逆时针、2/3 顺时针（与 4001_gz_x500 CA_ROTOR0-3 逐电机一致）；
- 混控量纲（**按 x500 悬停点线性化标定**，dT/dout = 2k·ωmax²·out_hover = 13.16 N/unit）：
  roll `τ/11.05`（臂 0.84 m 合成）、pitch `τ/6.84`（臂 0.52 m 合成）、
  yaw `mix_scale=0.38=1/(4·km·dT/dout)`（km=0.05）。

## 限制说明

- 姿态由 `src/ekf_att.c`（EKF2 姿态估计核心）融合 HIL 真值角速度/加速度 +
  HIL_SENSOR 磁力计（yaw 专用修正）得到；**位置/速度由 PX4 EKF2（完整，
  `src/px4_ekf_wrap.cpp` 包装）融合 HIL_GPS(116) GPS 数据**（gazebo GPS 模型，
  带 eph/epv/vel/vn/ve/vd 语义）——`gpos/gvel/gpchk` 全部通过（GPS 融合正常）；
- 磁力计修正（ATT_W_MAG=0.1）已启用（HIL_SENSOR 含磁力计），yaw 专用修正
  经协方差/增益加权锁定航向（悬停 yaw 收敛，稳态偏差为动态 decl 标定精度）；
- 仅使用了测试所需的最小 MAVLink 消息子集（收发共 23 种：TX 16 + RX 7，见"网络与协议"；
  编解码由标准库提供，无需自研）；
- Gazebo 插件仍需 C++（Gazebo API 限制），飞控本体 100% C 代码。