From 5e71049e07feffe30c06f5d54f43db4789e90836 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Fri, 12 Jun 2026 11:14:12 +0800 Subject: [PATCH] =?UTF-8?q?=E7=A7=BB=E9=99=A4=E4=BA=86=E6=97=A0=E7=94=A8?= =?UTF-8?q?=E7=9A=84=E8=84=9A=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- QGC测试.py | 82 -------------- c_test.py | 17 --- drone_controller/qgc_router.py | 199 --------------------------------- 3 files changed, 298 deletions(-) delete mode 100644 QGC测试.py delete mode 100644 c_test.py delete mode 100644 drone_controller/qgc_router.py diff --git a/QGC测试.py b/QGC测试.py deleted file mode 100644 index 7e3c32a..0000000 --- a/QGC测试.py +++ /dev/null @@ -1,82 +0,0 @@ -import time -import math -from drone_controller.qgc_router import QGCMissionReceiver - -def gps_to_ned(lon, lat, alt, home_lon, home_lat): - """ - 仅基于经纬度计算东向(E)和北向(N)，忽略高度。 - home_lon, home_lat: 原点经度、纬度 - lon, lat: 目标点经度、纬度 - 返回 (E, N) 单位：米 - """ - from pyproj import Transformer - # 1. 创建 WGS84 经纬度 -> ECEF 的转换器（高度固定为0） - transformer = Transformer.from_crs( - {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, - {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, - always_xy=True - ) - - # 2. 目标点和 home 点的 ECEF 坐标（alt=0） - x_t, y_t, z_t = transformer.transform(lon, lat, 0) - x_h, y_h, z_h = transformer.transform(home_lon, home_lat, 0) - - # 3. 差值 - dx, dy, dz = x_t - x_h, y_t - y_h, z_t - z_h - - # 4. 旋转到 ENU（只取 E,N） - phi = math.radians(home_lat) - lam = math.radians(home_lon) - sin_phi = math.sin(phi) - cos_phi = math.cos(phi) - sin_lam = math.sin(lam) - cos_lam = math.cos(lam) - - E = -sin_lam * dx + cos_lam * dy - N = -sin_phi * cos_lam * dx - sin_phi * sin_lam * dy + cos_phi * dz - U = -alt - - return E, N, U - -def main(): - HOME_LAT = 34 - HOME_LONG = 113 - - # 创建一个用于存放本地局部坐标的列表 - points = [] - - qgc_receiver = QGCMissionReceiver(connection_str="udpin:0.0.0.0:14550", home_lat=HOME_LAT, home_lon=HOME_LONG) - qgc_receiver.start() - - print("\n[MainLoop] 核心控制线程（线程2）启动，等待地面站下发任务...") - - try: - while True: - current_mission = qgc_receiver.get_waypoints() - - if not current_mission: - # 列表为空，原地挂起 - print('[MainLoop] 当前无航点任务...', end='\r') - time.sleep(1.0) - continue - - print(f"\n[MianLoop] 检测到新航线任务! 包含{len(current_mission)} 个航点, 准备进行香橙派自动巡点飞行控制。。。") - - for wp in current_mission: - y, x, z = gps_to_ned(*wp,home_lon=HOME_LONG, home_lat=HOME_LAT) - points.append({'x': x, 'y': y, 'z': z, 'yaw':0}) - print(f"--> 状态机驱动：开始前往目标航点 #{wp['seq']} -> 本地坐标 NED: X={x:.2f}米, Y={y:.2f}米, Z={z:.2f}米") - - time.sleep(3.0) - print(f"--> 航点 #{wp['seq']} 任务处理完毕！") - - print("[MainLoop] 航线列表中所有航点全部执行闭环,任务完毕,开始清空队列...") - print(points) - qgc_receiver.clear_waypoints() - points.clear() - - except KeyboardInterrupt: - qgc_receiver.stop() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/c_test.py b/c_test.py deleted file mode 100644 index fd55888..0000000 --- a/c_test.py +++ /dev/null @@ -1,17 +0,0 @@ -import time -from pymavlink import mavutil - -# 根据你的实际 UDP 配置修改 -# 如果飞控是作为 UDP 服务端（监听），则 Python 应作为客户端连接： -# connection_string = "udpout:飞控IP:14550" -# 如果飞控是作为 UDP 客户端（主动连接 Python），则 Python 应作为服务端监听： -connection_string = "udpin:127.0.0.1:14550" - -master = mavutil.mavlink_connection(connection_string) - -print("等待飞控心跳包...") -try: - master.wait_heartbeat(timeout=5) - print(f"收到心跳，系统 ID: {master.target_system}, 组件 ID: {master.target_component}") -except: - print("超时：未收到飞控心跳包，请检查 IP 和端口配置以及飞控端是否在发送心跳") \ No newline at end of file diff --git a/drone_controller/qgc_router.py b/drone_controller/qgc_router.py deleted file mode 100644 index 7e5a83e..0000000 --- a/drone_controller/qgc_router.py +++ /dev/null @@ -1,199 +0,0 @@ -import threading -import time -from pymavlink import mavutil - - -class QGCMissionReceiver: - def __init__(self, connection_str='udpin:0.0.0.0:14550', sys_id=1, comp_id=1, home_lat=34.0, home_lon=113.0): - """ - QGC 航线拦截接收器 (模拟飞控节点) - :param connection_str: pymavlink连接字符串。udpin:0.0.0.0:14550 表示作为本地服务器等待QGC主动连接 - :param sys_id: 伪装的系统ID (通常飞控为1) - :param comp_id: 伪装的组件ID (通常飞控为1) - :param home_lat: 模拟的起始点纬度（用于在没有真实GPS的室内环境让QGC地图定点） - :param home_lon: 模拟的起始点经度 - """ - self.connection_str = connection_str - self.sys_id = sys_id - self.comp_id = comp_id - self.home_lat = home_lat - self.home_lon = home_lon - - # 初始化 MAVLink 链路 - self.master = mavutil.mavlink_connection( - self.connection_str, - source_system=self.sys_id, - source_component=self.comp_id - ) - - # 线程安全与航点存储 - self.lock = threading.Lock() - self.waypoints_list = [] # 正式供外部控制脚本读取的航点列表 - self._temp_waypoints = [] # 协议传输过程中的临时缓存缓存 - - # 协议状态控制变量 - self.is_running = False - self.expected_count = 0 - self.current_seq = 0 - - def start(self): - """启动后台模拟线程""" - self.is_running = True - - # 线程 1-A: 维持心跳与状态遥测广播 (5Hz) - self.telemetry_thread = threading.Thread(target=self._telemetry_loop, daemon=True) - self.telemetry_thread.start() - - # 线程 1-B: 专门阻塞监听并应答 QGC 的航线握手协议 - self.protocol_thread = threading.Thread(target=self._protocol_loop, daemon=True) - self.protocol_thread.start() - - print(f"[QGCMissionProxy] 虚拟飞控线程已启动，正在监听 UDP 端口: {self.connection_str}") - - def stop(self): - """停止接收器""" - self.is_running = False - if self.master: - self.master.close() - print("[QGCMissionProxy] 虚拟飞控线程已关闭") - - def _telemetry_loop(self): - """高频向QGC广播心跳和定位，激活QGC的航线规划和上传按钮""" - boot_time = time.time() - while self.is_running: - try: - time_ms = int((time.time() - boot_time) * 1000) - - # 1. 发送 HEARTBEAT (伪装成运行中的 ArduPilot 多旋翼，且处于 GUIDED 模式解锁状态) - self.master.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_QUADROTOR, # 四轴/多旋翼 - mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOT, # 固件类型 - mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED | mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, - 0, # custom_mode - mavutil.mavlink.MAV_STATE_ACTIVE # 活动状态 - ) - - # 2. 发送 GLOBAL_POSITION_INT (不发这个QGC地图无法显示飞机，也无法规划航线) - self.master.mav.global_position_int_send( - time_ms, - int(self.home_lat * 1e7), # 纬度 (degrees * 1E7) - int(self.home_lon * 1e7), # 经度 (degrees * 1E7) - 10000, # 绝对高度 (毫米, 10米) - 0, # 相对高度 (毫米, 0米) - 0, 0, 0, # vx, vy, vz 速度 (cm/s) - 9000 # hdg 偏航角 (cdeg, 90度朝东) - ) - - # 3. 发送系统状态 (维持QGC右上角图标正常显示，可选) - self.master.mav.sys_status_send(0, 0, 0, 500, 11100, 100, 80, 0, 0, 0, 0, 0, 0) - - except Exception as e: - print(f"[QGCMissionProxy] 遥测发送异常: {e}") - time.sleep(0.2) # 5Hz 频率广播 - - def _protocol_loop(self): - """标准 MAVLink 航线协议状态机逻辑""" - while self.is_running: - try: - # 阻塞读取消息 - msg = self.master.recv_match(blocking=True, timeout=0.5) - if not msg: - continue - - msg_type = msg.get_type() - src_sys = msg.get_srcSystem() - src_comp = msg.get_srcComponent() - - # ---- 阶段 A: QGC点击了“发送航线”，通知飞机即将上传的总数 ---- - if msg_type == 'MISSION_COUNT': - self.expected_count = msg.count - self.current_seq = 0 - self._temp_waypoints = [] - print(f"\n[QGCMissionProxy] 拦截到QGC航线上传请求！预计航点总数: {self.expected_count}") - - if self.expected_count > 0: - # 核心回复：主动向地面站索要第 0 个航点数据 - self.master.mav.mission_request_int_send( - src_sys, src_comp, self.current_seq, msg.mission_type - ) - else: - # 总数为0则直接发送成功确认 - self.master.mav.mission_ack_send( - src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, msg.mission_type - ) - - # ---- 阶段 B: 接收具体的航点细节 (新版QGC通常使用 MISSION_ITEM_INT 整型坐标协议) ---- - elif msg_type == 'MISSION_ITEM_INT': - if msg.seq == self.current_seq: - # 解析QGC发来的单条数据 - wp = { - 'seq': msg.seq, - 'frame': msg.frame, - 'command': msg.command, - 'lat': msg.x / 1e7, # 还原浮点经纬度 - 'lon': msg.y / 1e7, - 'alt': msg.z, # 高度 - 'param1': msg.param1, # 悬停时间等控制参数 - 'param2': msg.param2, - 'param3': msg.param3, - 'param4': msg.param4 - } - self._temp_waypoints.append(wp) - print( - f"[QGCMissionProxy] 成功接收并缓存航点 [{msg.seq + 1}/{self.expected_count}]: Lat={wp['lat']:.6f}, Lon={wp['lon']:.6f}, Alt={wp['alt']:.2f}m") - - self.current_seq += 1 - if self.current_seq < self.expected_count: - # 继续索要下一个航点 - self.master.mav.mission_request_int_send( - src_sys, src_comp, self.current_seq, msg.mission_type - ) - else: - # ---- 阶段 C: 全部接收完毕，原子化存入正式列表，并向QGC回复“握手成功” ---- - with self.lock: - self.waypoints_list = list(self._temp_waypoints) - print(f"🎉 [QGCMissionProxy] 航线完美拦截并填充！列表当前长度: {len(self.waypoints_list)}") - - self.master.mav.mission_ack_send( - src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, msg.mission_type - ) - - # ---- 兼容机制: 兼容老版本QGC使用的浮点数协议形式 ---- - elif msg_type == 'MISSION_ITEM': - if msg.seq == self.current_seq: - wp = { - 'seq': msg.seq, 'frame': msg.frame, 'command': msg.command, - 'lat': msg.x, 'lon': msg.y, 'alt': msg.z, - 'param1': msg.param1, 'param2': msg.param2, 'param3': msg.param3, 'param4': msg.param4 - } - self._temp_waypoints.append(wp) - self.current_seq += 1 - if self.current_seq < self.expected_count: - self.master.mav.mission_request_send(src_sys, src_comp, self.current_seq, msg.mission_type) - else: - with self.lock: - self.waypoints_list = list(self._temp_waypoints) - self.master.mav.mission_ack_send(src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, - msg.mission_type) - - # ---- 处理地面站点击“清除航线”的请求 ---- - elif msg_type == 'MISSION_CLEAR_ALL': - print("[QGCMissionProxy] 拦截到QGC清除航线指令") - with self.lock: - self.waypoints_list = [] - self.master.mav.mission_ack_send(src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, - msg.mission_type) - - except Exception as e: - print(f"[QGCMissionProxy] 协议状态机运行异常: {e}") - time.sleep(0.1) - - def get_waypoints(self): - """提供给控制主线程调用的线程安全方法：获取当前最新的航点列表""" - with self.lock: - return list(self.waypoints_list) - - def clear_waypoints(self): - """从香橙派端手动清空列表""" - with self.lock: - self.waypoints_list = [] \ No newline at end of file