尧图网站建设 
ArduPilot C 模拟器接入指南基于 UDP JSON 接口的 libAP_JSON 库解析与实战【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot导读ArduPilot 的 SITLSoftware 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 和端口物理后端simulator / physics backend在9002 端口上监听入站消息对应InitSockets(fdm_address, fdm_port_in)中的绑定地址与端口minimal.cpp与simpleRover.cpp中均使用ap.InitSockets(127.0.0.1, 9002)收到来自 SITL 的报文后物理后端应向报文的来源 IP 和端口回复这一回程地址由SocketExample::last_recv_address获取并保存在libAP_JSON的fcu_address/fcu_port_out成员中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 可以看到完整的公有接口API作用关键参数/单位InitSockets(fdm_address, fdm_port_in)绑定 UDP 端口等待 SITL 连接地址字符串 端口号默认127.0.0.1:9002ReceiveServoPacket(servo_out[])接收 ArduPilot 下发的 16 路 PWM 舵机指令uint16_t servo_out[16]SendState(timestamp, gyro, accel, pos, attitude, velocity)上报机体完整状态见下文字段说明setAirspeed(airspeed_in)设置空速m/s可选影响空速传感器setWindvane(direction, speed)设置表观风向风速rad、m/s可选0 rad 表示迎风setRangefinder(rangefinder_in, n)设置最多 6 个测距仪读数m可选对应rng_1~rng_6其中SendState的完整签名摘自 libAP_JSON.hvoid 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 18458ReceiveServoPacket收到包后会校验该值不匹配则打印 Incorrect protocol magic 并丢弃防止把陌生 UDP 流量误认为 ArduPilot 数据frame_rate表示 SITL 建议的仿真时间步长即 1/SIM_RATE_HZ。物理后端可以自由忽略该值但通常应设定最大时间步长限制frame_count每输出一帧递增一次客户端会检测重复帧frame_count未变与丢失帧跳变并在 SITL 重启导致计数重置时提示 ArduPilot controller has resetPWM 范围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]}必填字段字段含义单位/坐标系timestamp物理时间绝对时间非时间步长秒imu.gyro角速度roll, pitch, yawrad/s机体坐标系imu.accel_body机体加速度x, y, zm/s²机体坐标系position位置北、东、下m惯性/地球坐标系velocity速度北、东、下m/s惯性/地球坐标系attitude或quaternion姿态欧拉角或四元数二者至少其一rad / 无量纲在 SIM_JSON.h 的keytable中timestamp、imu.gyro、imu.accel_body、velocity均标记为required trueattitude与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.0m/s。此外协议还支持libAP_JSON未封装但 SITL 端 SIM_JSON.h 已解析3D 风场velocity_wind:[3.2,0.0,-0.7]m/sNED 系遥控器输入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。整个工程使用 C11 标准见 CMakeLists.txt 的CMAKE_CXX_STANDARD 11。七、simpleRover一维物理模型集成范例simpleRover.cpp 展示了一个 1-D 小车模型如何与库集成是把真实物理模型接入协议的最佳模板。伺服映射约定模型将伺服通道定义如下注释位于simpleRover::update内throttle油门实际作为速度控制使用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 --mapsim_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_JSONC 模拟器接入 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创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考 ArduPilot C++ 模拟器接入指南：基于 UDP JSON 接口的 libAP_JSON 库解析与实战
ArduPilot C 模拟器接入指南基于 UDP JSON 接口的 libAP_JSON 库解析与实战【免费下载链接】ardupilotArduPlane, ArduCopter, ArduRover, ArduSub source项目地址: https://gitcode.com/GitHub_Trending/ar/ardupilot导读ArduPilot 的 SITLSoftware 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 和端口物理后端simulator / physics backend在9002 端口上监听入站消息对应InitSockets(fdm_address, fdm_port_in)中的绑定地址与端口minimal.cpp与simpleRover.cpp中均使用ap.InitSockets(127.0.0.1, 9002)收到来自 SITL 的报文后物理后端应向报文的来源 IP 和端口回复这一回程地址由SocketExample::last_recv_address获取并保存在libAP_JSON的fcu_address/fcu_port_out成员中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 可以看到完整的公有接口API作用关键参数/单位InitSockets(fdm_address, fdm_port_in)绑定 UDP 端口等待 SITL 连接地址字符串 端口号默认127.0.0.1:9002ReceiveServoPacket(servo_out[])接收 ArduPilot 下发的 16 路 PWM 舵机指令uint16_t servo_out[16]SendState(timestamp, gyro, accel, pos, attitude, velocity)上报机体完整状态见下文字段说明setAirspeed(airspeed_in)设置空速m/s可选影响空速传感器setWindvane(direction, speed)设置表观风向风速rad、m/s可选0 rad 表示迎风setRangefinder(rangefinder_in, n)设置最多 6 个测距仪读数m可选对应rng_1~rng_6其中SendState的完整签名摘自 libAP_JSON.hvoid 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 18458ReceiveServoPacket收到包后会校验该值不匹配则打印 Incorrect protocol magic 并丢弃防止把陌生 UDP 流量误认为 ArduPilot 数据frame_rate表示 SITL 建议的仿真时间步长即 1/SIM_RATE_HZ。物理后端可以自由忽略该值但通常应设定最大时间步长限制frame_count每输出一帧递增一次客户端会检测重复帧frame_count未变与丢失帧跳变并在 SITL 重启导致计数重置时提示 ArduPilot controller has resetPWM 范围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]}必填字段字段含义单位/坐标系timestamp物理时间绝对时间非时间步长秒imu.gyro角速度roll, pitch, yawrad/s机体坐标系imu.accel_body机体加速度x, y, zm/s²机体坐标系position位置北、东、下m惯性/地球坐标系velocity速度北、东、下m/s惯性/地球坐标系attitude或quaternion姿态欧拉角或四元数二者至少其一rad / 无量纲在 SIM_JSON.h 的keytable中timestamp、imu.gyro、imu.accel_body、velocity均标记为required trueattitude与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.0m/s。此外协议还支持libAP_JSON未封装但 SITL 端 SIM_JSON.h 已解析3D 风场velocity_wind:[3.2,0.0,-0.7]m/sNED 系遥控器输入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。整个工程使用 C11 标准见 CMakeLists.txt 的CMAKE_CXX_STANDARD 11。七、simpleRover一维物理模型集成范例simpleRover.cpp 展示了一个 1-D 小车模型如何与库集成是把真实物理模型接入协议的最佳模板。伺服映射约定模型将伺服通道定义如下注释位于simpleRover::update内throttle油门实际作为速度控制使用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 --mapsim_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_JSONC 模拟器接入 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创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考