企业官网建设流程全解析

ArduPilot C++ 模拟器接入指南：基于 UDP JSON 接口的 libAP_JSON 库解析与实战

【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot

导读

ArduPilot 的 SITL（Software In The Loop）仿真框架支持通过标准 JSON 协议接入外部物理引擎，而libraries/SITL/examples/JSON/C++目录提供了一套用 C++ 编写的轻量级客户端库libAP_JSON，让任何用 C++ 实现的飞行/车辆动力学模型都能以极低成本与 ArduPilot 自动驾驶固件打通数据链路。本文将以该目录下的 readme.md 为主线，结合libAP_JSON、minimal、simpleRover等源码，完整讲解 UDP 连接模型、伺服指令二进制包格式、JSON 状态上报字段、物理模型集成方式与调试方法，帮助读者在 Linux 环境下快速搭建一个属于自己的 ArduPilot SITL 仿真后端。

一、为什么需要 C++ JSON 接口：简化模拟器接入

ArduPilot 的 SITL 后端种类繁多（如基于真实飞机模型的多旋翼、固定翼动力学模型），但对外部模拟器而言，最通用的接入方式是通过--model JSON启动的 JSON 后端。C++ 示例目录提供的最小库libAP_JSON将这套协议的客户端侧完全封装，模拟器作者只需要：

• 调用InitSockets建立 UDP 监听；
• 用ReceiveServoPacket拿到 ArduPilot 下发的舵机 PWM 指令；
• 用SendState回传姿态、加速度、位置、速度等机体状态。

正如 readme.md 所述："This simplifies adding support for ArduPilot to a simulator."（这简化了为模拟器添加 ArduPilot 支持的过程）。该库最初改编自 Pierre Kancir 为 Gazebo 编写的 ArduPilot 插件（见 libAP_JSON.cpp 头注释），因此其 API 设计天然适合接入 Gazebo 等外部物理引擎。

二、UDP 连接模型与自动发现机制

C++ 库与 SITL 之间通过 UDP 链路通信，连接模型的关键设计点是无需在物理后端配置目标 IP 和端口：

1. 物理后端（simulator / physics backend）在9002 端口上监听入站消息，对应InitSockets(fdm_address, fdm_port_in)中的绑定地址与端口，minimal.cpp与simpleRover.cpp中均使用ap.InitSockets("127.0.0.1", 9002)；
2. 收到来自 SITL 的报文后，物理后端应向报文的来源 IP 和端口回复，这一"回程地址"由SocketExample::last_recv_address获取，并保存在libAP_JSON的fcu_address/fcu_port_out成员中；
3. ArduPilot SITL每 10 秒发送一次输出消息（即使没有收到输入数据），物理后端据此实现自动发现（auto-detect）。

这套机制消除了跨进程、跨机器部署时手动指定 SITL 端口的麻烦。在 libAP_JSON.cpp 的ReceiveServoPacket中可以看到，库内部通过sock.recv非阻塞收包、调用last_recv_address记录对端地址，随后SendState使用sendto(s, ..., fcu_address, fcu_port_out)回发数据。若 10 秒内未收到任何输入，SITL 会重发输出帧但不递增帧计数（见 JSON 协议总文档），从而支持物理模型重启后重新连接。

连接状态与超时处理

libAP_JSON维护一个ap_online布尔标志表示 ArduPilot 是否在线：

• 未检测到 ArduPilot 时，ReceiveServoPacket的接收超时仅为 1ms，主循环可以快速跳过，不会阻塞仿真主线程；
• 一旦收到合法数据包，ap_online置为true，接收等待时间提升至 10ms 以容忍网络抖动；
• 在线状态下连续丢失connectionTimeoutMaxCount（默认 10）个包后判定连接断开，ap_online复位并打印 "Broken ArduPilot connection" 提示（源码位于 libAP_JSON.cpp）。

三、libAP_JSON 核心 API 一览

从 libAP_JSON.h 可以看到完整的公有接口：

其中SendState的完整签名（摘自 libAP_JSON.h）：

void SendState(double timestamp, double gyro_x, double gyro_y, double gyro_z, // rad/sec double accel_x, double accel_y, double accel_z, // m/s^2 double pos_x, double pos_y, double pos_z, // m in inertial frame double phi, double theta, double psi, // attitude radians double V_x, double V_y, double V_z); // m/s in inertial frame

注意源码注释明确要求IMU 姿态采用 NED 约定：x 向前、y 向右、z 向下。minimal.cpp中给出的静止在地面示例即使用accel = (0, 0, -9.81)——因为支撑面给机体的反作用力在 z 轴向下坐标系中表现为 -9.81 m/s² 的加速度。

四、下行通道：SITL 输出的二进制伺服包

SITL 向物理后端发送的是二进制格式数据包，结构定义同时出现在客户端 libAP_JSON.cpp 的servo_packet与 SITL 服务端 SIM_JSON.h 的servo_packet_16中，二者完全对应：

struct servo_packet { uint16_t magic; // 18458 固定魔数，用于协议版本校验 uint16_t frame_rate; // 期望仿真步长对应的帧率 uint32_t frame_count; // 输出帧计数，用于检测丢帧/重复帧 uint16_t pwm[16]; // 16 路舵机 PWM 值（微秒） };

要点：

• magic = 18458：ReceiveServoPacket收到包后会校验该值，不匹配则打印 "Incorrect protocol magic" 并丢弃，防止把陌生 UDP 流量误认为 ArduPilot 数据；
• frame_rate：表示 SITL 建议的仿真时间步长（即 1/SIM_RATE_HZ）。物理后端可以自由忽略该值，但通常应设定最大时间步长限制；
• frame_count：每输出一帧递增一次，客户端会检测"重复帧"（frame_count未变）与"丢失帧"（跳变），并在 SITL 重启导致计数重置时提示 "ArduPilot controller has reset"；
• PWM 范围：16 路舵机值单位为微秒，典型范围 1000~2000；
• 扩展为 32 通道：设置参数SERVO_32_ENABLE = 1后，SITL 输出包变为pwm[32]且 magic 变为29569（见 SIM_JSON.h 的servo_packet_32）。

客户端还做了**缓冲排空（drain）**处理：当网络积压多包时，ReceiveServoPacket会循环读取直至recv返回 -1，只保留最新的数据包，避免仿真跟随延迟滞后（见 libAP_JSON.cpp）。

五、上行通道：JSON 状态上报字段详解

物理后端回传给 SITL 的是纯文本 JSON，行首和行尾以\n包裹。完整协议说明见 JSON 协议总文档。libAP_JSON::SendState生成的 JSON 结构如下：

{"timestamp":2500,"imu":{"gyro":[0,0,0],"accel_body":[0,0,0]},"position":[0,0,0],"attitude":[0,0,0],"velocity":[0,0,0]}

必填字段

在 SIM_JSON.h 的keytable中，timestamp、imu.gyro、imu.accel_body、velocity均标记为required = true；attitude与quaternion虽然标为可选，但协议规定两者必须至少提供一个，且若同时提供，SITL 优先使用四元数。字段顺序无关紧要。

可选字段（增强传感器仿真）

这些字段由setAirspeed、setWindvane、setRangefinder三个 setter 控制，只有调用过对应 setter（内部标志位置位）才会被SendState序列化进 JSON：

• 测距仪："rng_1":1.0…"rng_6":1.0，对应 6 个测距仪实例，最多 6 个（libAP_JSON内部数组大小为 6，超出会打印 "Too many rangefinder values!"）；
• 表观风向风速："windvane":{"direction":0,"speed":0}，direction 单位为 rad，顺时针相对机头，0 表示正迎风；
• 空速："airspeed":25.0（m/s）。

此外，协议还支持（libAP_JSON未封装，但 SITL 端 SIM_JSON.h 已解析）：

• 3D 风场"velocity_wind":[3.2,0.0,-0.7]（m/s，NED 系）；
• 遥控器输入"rc":{"rc_1":1500,...,"rc_12":1500}（最多 12 通道）；
• 电池"battery":{"voltage":50.39,"current":64.01}；
• 时间同步标志no_time_sync与锁步标志no_lockstep。

地面静止时的正确加速度

minimal.cpp有一段关键注释值得注意：当飞行器停在地面时，IMU 加速度计会感应到地面支撑力对抗重力产生的向上加速度，在 z 轴向下的 FRD 机体坐标系中应表示为 -9.81 m/s²。因此静止示例调用：

ap.SendState(timestamp, 0, 0, 0, // gyro 0, 0, -9.81, // accel（地面支撑反作用） 0, 0, 0, // position 0, 0, 0, // attitude 0, 0, 0); // velocity

六、最小示例 minimal 深入解读

minimal.cpp 展示了每个库方法的用法，主循环结构可概括为：

int main() { libAP_JSON ap; if (ap.InitSockets("127.0.0.1", 9002)) { /* started socket */ } while (true) { double timestamp = (double) micros() / 1000000.0; // 秒 if (ap.ReceiveServoPacket(servo_out)) { /* 可选：打印 PWM */ } if (!ap.ap_online) continue; // 未连上则跳过状态上报 // 设置可选传感器数据 ap.setAirspeed(1); ap.setWindvane(1, 1); ap.setRangefinder(rangefinder_example, 6); // 上报必填状态 ap.SendState(timestamp, 0,0,0, 0,0,-9.81, 0,0,0, 0,0,0, 0,0,0); usleep(1000); // 目标 ~1000 Hz 循环（实际约 800 Hz） } }

其中micros()借助std::chrono::high_resolution_clock实现，时间戳换算为秒。minimal构建后可直接用于测试库本身（readme 明确说明 "can be used to test the library as well"）。整个工程使用 C++11 标准（见 CMakeLists.txt 的CMAKE_CXX_STANDARD 11）。

七、simpleRover：一维物理模型集成范例

simpleRover.cpp 展示了一个 1-D 小车模型如何与库集成，是"把真实物理模型接入协议"的最佳模板。

伺服映射约定

模型将伺服通道定义如下（注释位于simpleRover::update内）：

1. throttle（油门）：实际作为速度控制使用；
2. steering（转向）：实际作为偏航角速度 omega 使用（当前 1-D 版本未启用）。

simpleRover.cpp的通道索引取自servo_out[2]（即 RC 通道 3），通过线性插值_interp1D把 1100~1900 的 PWM 映射到 -1~+1 m/s 的速度：

double max_velocity = 1; // m/s double body_v = _interp1D(servo_out[2], 1100, 1900, -max_velocity, max_velocity);

这正是 readme 中"rover responds to throttle commands on RC channel 3"的由来。

物理状态更新

simpleRover::update实现了最基本的运动学递推：

• 计算时间步长timestep = state.timestamp - old_state.timestamp，并做异常防护：时间倒退报错、时间未推进警告跳过、步长超过 60 秒警告跳过；
• 由速度差分得加速度：accel_x = (V_x - old_V_x) / timestep；
• 由速度积分得位移：pos_x += V_x * timestep；
• 更新成功后把state拷贝到old_state，再调用SendState上报。

该例清晰地演示了"接收舵机 → 更新物理 → 上报状态"的标准循环。状态结构体simpleRoverState（见 simpleRover.h）字段与SendState参数一一对应，方便套用到更复杂的模型。

八、构建与运行完整流程

1. 编译示例

minimal与simpleRover两个可执行文件通过 CMake 构建：

mkdir build && cd build cmake .. make

构建产物为build/minimal与build/simpleRover。readme 同时说明minimal.cpp也可直接单文件编译：g++ minimal.cpp -o minimal.o。

2. 启动物理引擎

./simpleRover

程序启动后绑定127.0.0.1:9002等待 ArduPilot SITL 的报文。

3. 启动 SITL 并指定 JSON 后端

另开一个终端，使用-f JSON指定 JSON 框架：

sim_vehicle.py -v Rover -f JSON --console --map

sim_vehicle.py位于仓库的 Tools/autotest 目录，是 ArduPilot 官方的 SITL 启动脚本。启动后两个进程通过 UDP 自动建立连接（Rover 默认主回路频率为 50Hz，见 JSON 协议总文档 对 SIM_RATE_HZ 的说明）。

4. 在 MAVProxy 控制台中操控小车

在sim_vehicle.py打开的 MAVProxy 控制台（提示符为MANUAL>）中输入：

# 解锁（arm throttle） MANUAL> arm throttle # 全油门前进（期望速度 1 m/s） MANUAL> rc 3 1900 # 全油门后退（期望速度 -1 m/s） MANUAL> rc 3 1100 # 停止 MANUAL> rc 3 1500

由于simpleRover把 RC3 的 PWM 线性映射为 ±1 m/s 的速度，上述指令应能观察到位置沿 x 轴前进/后退/停止。完整命令序列见 readme.md。

九、调试与排错

1. 连接状态输出

libAP_JSON在运行时会打印关键事件：

[libAP_JSON] flight dynamics model at 127.0.0.1:9002 [libAP_JSON] Connected to ArduPilot controller @ 127.0.0.1:xxxxx [libAP_JSON] Broken ArduPilot connection (no packets received)

其中"Connected"出现说明自动发现成功，IP/端口是 SITL 的实际来源地址；若反复出现 "Broken" 提示，需检查网络连通性与防火墙。

2. SITL 端字段校验

首次连接时，SITL 会打印一条消息，报告成功接收了哪些字段（如timestamp、gyro、accel_body、position、attitude、velocity、rng_1等）。若必填字段缺失，SITL 会停止运行；可选字段缺失则继续。该消息是核对物理后端上报内容是否完整的最直接手段（示例输出见 JSON 协议总文档）。

3. 启用调试打印

将 libAP_JSON.cpp 顶部的#define DEBUG_ENABLED 0改为 1，可打印每次收发的字节数、magic、frame_rate、frame_count 以及完整 PWM 数组与发送的 JSON 字符串，便于定位协议层问题。

4. 常见问题对照

• magic 校验失败：确认对端确实是 ArduPilot SITL JSON 后端（-f JSON），而非其他 SITL 后端；
• SITL 报必填字段缺失：检查SendState是否在所有分支都被调用、字段拼写是否与协议一致；
• 小车不动：确认ap_online已为 true（否则主循环会continue跳过上报），并检查servo_out[2]收到的 PWM 是否为 1100~1900 范围。

十、总结

通过libAP_JSON，C++ 模拟器接入 ArduPilot SITL 只需掌握四件事：UDP 9002 监听、二进制伺服包解析（magic 18458/16 通道或 29569/32 通道）、JSON 状态上报（必填的 timestamp/imu/position/attitude/velocity 与可选的 rng/windvane/airspeed 等）以及物理模型与主循环的整合方式。minimal提供了 API 用法的完整参考，simpleRover提供了 1-D 物理模型的集成范式，读者完全可以在此基础上替换为自己的刚体动力学、空气动力学或多体模型，将 ArduPilot 作为自动驾驶控制器运行在任意自研仿真环境中。更完整的协议字段说明可继续阅读 JSON 协议总文档，SITL 服务端的解析实现位于 SIM_JSON.h 与 SIM_JSON.cpp。

【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot