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

在抓取到gz sim的数据流后 ,就可以不需要这个工具了 完全可以用python来写一个服务替代gz的模拟数据/////////////////////////////////////////////////////#!/usr/bin/env python3Gazebo JSON 仿真服务器应答模式- 绑定 UDP 9002 端口- 每收到一个来自 SITL 的数据包就发送一帧 JSON 数据作为响应- 时间戳每次增加固定值如 0.001 秒运动积分使用实际接收间隔import socketimport jsonimport timeimport sysimport randomimport signalBASE_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_timestamppayload[timestamp] dt_timestamppayload[position][0] payload[velocity][0] * dt_physicalpayload[position][1] payload[velocity][1] * dt_physicalpayload[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 payloadclass JSONSimServer:def __init__(self, bind_port9002, timestamp_delta0.001):self.bind_port bind_portself.timestamp_delta timestamp_deltaself.sock Noneself.client_addr Noneself.running Falseself.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 Truelast_recv_time Nonepacket_count 0try:while self.running:data, addr self.sock.recvfrom(4096)if self.client_addr is None:self.client_addr addrprint(f[服务器] 收到来自 {addr} 的连接进入应答模式)elif addr ! self.client_addr:# 忽略其他地址continuenow time.perf_counter()if last_recv_time is None:dt_physical 0.01 # 默认步长else:dt_physical now - last_recv_timelast_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 1if 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 Falseif self.sock:self.sock.close()print([服务器] 已停止)def main():port 9002timestamp_delta 0.001 # 每收到一次时间戳增加 0.001 秒if len(sys.argv) 1:try:timestamp_delta float(sys.argv[1])except ValueError:passif len(sys.argv) 2:try:port int(sys.argv[2])except ValueError:passserver JSONSimServer(bind_portport, timestamp_deltatimestamp_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 --boardsitl# 编译四轴飞控固件./waf copter//////启动sitl/home/charlie/opt/ardu/ardupilot/build/sitl/bin/arducopter --model JSON --speedup 1 --slave 0 --sim-address127.0.0.1 -I0mavproxy.py --retries 5 --out 127.0.0.1:14550 --master tcp:127.0.0.1:5760//////////////////////////////////快速解锁起飞rc 3 1000arm throttlerc 3 1600void 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 i0; i8 i16; 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): frame20900 rate1200 PWM[1000,1000,1000,1000,0,0,0,0]JSON servo out (16ch): frame21000 rate1200 PWM[1006,1006,1006,1006,0,0,0,0]JSON servo out (16ch): frame21100 rate1200 PWM[1036,1036,1036,1036,0,0,0,0]JSON servo out (16ch): frame21200 rate1200 PWM[1066,1066,1066,1066,0,0,0,0]JSON servo out (16ch): frame21300 rate1200 PWM[1096,1096,1096,1096,0,0,0,0]JSON servo out (16ch): frame21400 rate1200 PWM[1126,1126,1126,1126,0,0,0,0]JSON servo out (16ch): frame21500 rate1200 PWM[1150,1150,1150,1150,0,0,0,0]JSON servo out (16ch): frame21600 rate1200 PWM[1439,1439,1439,1439,0,0,0,0]JSON servo out (16ch): frame21700 rate1200 PWM[1577,1573,1573,1577,0,0,0,0]//////////////////////////更新position