当前位置：首页 > news >正文

记录一次ardupilot_sitl调试longitude的输入数据流

news 2026/6/2 19:37:17

在抓取到gz sim的数据流后 ,就可以不需要这个工具了完全可以用python来写一个服务替代gz的模拟数据

/////////////////////////////////////////////////////

#!/usr/bin/env python3
"""
Gazebo JSON 仿真服务器（应答模式）
- 绑定 UDP 9002 端口
- 每收到一个来自 SITL 的数据包，就发送一帧 JSON 数据作为响应
- 时间戳每次增加固定值（如 0.001 秒），运动积分使用实际接收间隔
"""

import socket
import json
import time
import sys
import random
import signal

BASE_PAYLOAD = {
"timestamp": 0.0,
"imu": {
"gyro": [-1.3821477057480989e-15, -5.405425456198043e-16, 1.6857277913934e-18],
"accel_body": [-1.372462947196204e-8, 2.101031229379505e-10, -9.8]
},
"position": [2.573308782284005e-10, -3.938280660138266e-12, -0.19499942739960947],
"quaternion": [1.0, -1.0098482109629705e-11, -6.598405615460205e-10, 1.110223024564242e-15],
"velocity": [1.5902986085520575e-10, -2.4317334849982724e-12, -5.75753623985299e-06],
"no_time_sync": True,
"no_lockstep": False
}

def build_packet(payload):
return (json.dumps(payload, separators=(',', ':')) + '\n').encode('utf-8')

def update_payload(payload, dt_physical, dt_timestamp):
"""物理积分用 dt_physical，时间戳增量用 dt_timestamp"""
payload["timestamp"] += dt_timestamp
payload["position"][0] += payload["velocity"][0] * dt_physical
payload["position"][1] += payload["velocity"][1] * dt_physical
payload["position"][2] += payload["velocity"][2] * dt_physical
# 添加噪声
payload["imu"]["gyro"][0] += random.uniform(-1e-5, 1e-5)
payload["imu"]["gyro"][1] += random.uniform(-1e-5, 1e-5)
payload["imu"]["gyro"][2] += random.uniform(-1e-5, 1e-5)
payload["imu"]["accel_body"][0] += random.uniform(-1e-3, 1e-3)
payload["imu"]["accel_body"][1] += random.uniform(-1e-3, 1e-3)
payload["imu"]["accel_body"][2] = -9.8 + random.uniform(-1e-3, 1e-3)
return payload

class JSONSimServer:
def __init__(self, bind_port=9002, timestamp_delta=0.001):
self.bind_port = bind_port
self.timestamp_delta = timestamp_delta
self.sock = None
self.client_addr = None
self.running = False
self.payload = {
"timestamp": BASE_PAYLOAD["timestamp"],
"imu": {
"gyro": BASE_PAYLOAD["imu"]["gyro"][:],
"accel_body": BASE_PAYLOAD["imu"]["accel_body"][:]
},
"position": BASE_PAYLOAD["position"][:],
"quaternion": BASE_PAYLOAD["quaternion"][:],
"velocity": BASE_PAYLOAD["velocity"][:],
"no_time_sync": BASE_PAYLOAD["no_time_sync"],
"no_lockstep": BASE_PAYLOAD["no_lockstep"]
}

def start(self):
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.sock.bind(('0.0.0.0', self.bind_port))
print(f"[服务器] 绑定 UDP {self.bind_port}，等待 SITL 数据包...")

self.running = True
last_recv_time = None
packet_count = 0

try:
while self.running:
data, addr = self.sock.recvfrom(4096)
if self.client_addr is None:
self.client_addr = addr
print(f"[服务器] 收到来自 {addr} 的连接，进入应答模式")
elif addr != self.client_addr:
# 忽略其他地址
continue

now = time.perf_counter()
if last_recv_time is None:
dt_physical = 0.01 # 默认步长
else:
dt_physical = now - last_recv_time
last_recv_time = now

# 更新仿真状态：物理步长用实际间隔，时间戳增量用固定值
self.payload = update_payload(self.payload, dt_physical, self.timestamp_delta)
packet = build_packet(self.payload)
self.sock.sendto(packet, self.client_addr)

packet_count += 1
if packet_count % 100 == 0:
print(f"[服务器] 已发送 {packet_count} 包, 时间戳={self.payload['timestamp']:.3f}s")
except KeyboardInterrupt:
print("\n[服务器] 用户中断")
except Exception as e:
print(f"[服务器] 错误: {e}")
finally:
self.stop()

def stop(self):
self.running = False
if self.sock:
self.sock.close()
print("[服务器] 已停止")

def main():
port = 9002
timestamp_delta = 0.001 # 每收到一次，时间戳增加 0.001 秒
if len(sys.argv) > 1:
try:
timestamp_delta = float(sys.argv[1])
except ValueError:
pass
if len(sys.argv) > 2:
try:
port = int(sys.argv[2])
except ValueError:
pass
server = JSONSimServer(bind_port=port, timestamp_delta=timestamp_delta)
signal.signal(signal.SIGINT, lambda s, f: server.stop())
signal.signal(signal.SIGTERM, lambda s, f: server.stop())
server.start()

if __name__ == "__main__":
main()
/////////////

根据抓包的数据每10m收到一个包会包里面的timestamp 5.892 每次加 0.001
////////////////
如果需要longitude 变化需要将 "velocity": [1.5902986085520575e-10, -2.4317334849982724e-12, -5.75753623985299e-06],
改成 "velocity": [1.5902986085520575e-10, 5.0, -5.75753623985299e-06],

///////////////////////////

./waf distclean

# 配置为 SITL 仿真平台 + 开启 debug 调试符号
./waf configure --debug --board=sitl
# 编译四轴飞控固件
./waf copter

//////启动sitl

/home/charlie/opt/ardu/ardupilot/build/sitl/bin/arducopter --model JSON --speedup 1 --slave 0 --sim-address=127.0.0.1 -I0

mavproxy.py --retries 5 --out 127.0.0.1:14550 --master tcp:127.0.0.1:5760

//////////////////////////////////快速解锁起飞

rc 3 1000
arm throttle
rc 3 1600

void JSON::output_servos(const struct sitl_input &input)

{
添加pwm给到电机的数值

static uint32_t ccc = 0;

if (ccc++ % 100 == 0) {

printf("JSON servo out (16ch): frame=%u rate=%u PWM=[", pkt.frame_count, pkt.frame_rate);

for (uint8_t i=0; i<8 && i<16; i++) {

printf("%s%u", i?",":"", pkt.pwm[i]);

}

printf("...]\n");

}

/////输出示例

PWM=[1000,1000,1000,1000,0,0,0,0] //对应X型多旋翼的4个电机从右上角逆时针排序
JSON servo out (16ch): frame=20900 rate=1200 PWM=[1000,1000,1000,1000,0,0,0,0]
JSON servo out (16ch): frame=21000 rate=1200 PWM=[1006,1006,1006,1006,0,0,0,0]
JSON servo out (16ch): frame=21100 rate=1200 PWM=[1036,1036,1036,1036,0,0,0,0]
JSON servo out (16ch): frame=21200 rate=1200 PWM=[1066,1066,1066,1066,0,0,0,0]
JSON servo out (16ch): frame=21300 rate=1200 PWM=[1096,1096,1096,1096,0,0,0,0]
JSON servo out (16ch): frame=21400 rate=1200 PWM=[1126,1126,1126,1126,0,0,0,0]
JSON servo out (16ch): frame=21500 rate=1200 PWM=[1150,1150,1150,1150,0,0,0,0]
JSON servo out (16ch): frame=21600 rate=1200 PWM=[1439,1439,1439,1439,0,0,0,0]
JSON servo out (16ch): frame=21700 rate=1200 PWM=[1577,1573,1573,1577,0,0,0,0]

//////////////////////////

更新position

查看全文

http://www.zskr.cn/news/1449001.html