From 162b177bc15be898e89b82473da5f25e80eec155 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= <yimiao9036@outlook.com>
Date: Fri, 5 Jun 2026 09:39:08 +0800
Subject: [PATCH] =?UTF-8?q?2026.6.5=E5=B0=8F=E6=9B=B4=E6=96=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 drone_controller/qgc_router.py | 173 +++++++++++++++++++++++++++++++++
 run_mission.py                 |  10 +-
 2 files changed, 182 insertions(+), 1 deletion(-)
 create mode 100644 drone_controller/qgc_router.py

diff --git a/drone_controller/qgc_router.py b/drone_controller/qgc_router.py
new file mode 100644
index 0000000..6d33955
--- /dev/null
+++ b/drone_controller/qgc_router.py
@@ -0,0 +1,173 @@
+import time
+import threading
+from pymavlink import mavutil
+
+
+class QGCMissionProxy:
+    def __init__(self, qgc_link):
+        """
+        :param qgc_link: 专门连接地面站数传电台的 mavutil 链接对象
+        """
+        self.qgc = qgc_link
+        self.translated_waypoints = []  # 翻译出来供你本地 mission_manager 使用的航点列表
+        self.is_mission_ready = False
+        self.running = True
+
+        # 锁机制防止数据脏读
+        self.lock = threading.Lock()
+
+        # 启动后台监听线程
+        self.proxy_thread = threading.Thread(target=self._listen_qgc_loop, daemon=True)
+        self.proxy_thread.start()
+
+    def _listen_qgc_loop(self):
+        """高频监听地面站发来的 MAVLink 消息"""
+        print("[QGC Proxy] 拦截网关已启动，等待地面站上传航线...")
+
+        while self.running:
+            try:
+                # 阻塞读取来自 QGC 的消息，超时时间为0.5秒
+                msg = self.qgc.recv_match(blocking=True, timeout=0.5)
+                if not msg:
+                    continue
+
+                msg_type = msg.get_type()
+
+                # 1. 拦截：收到地面站宣告的航点总数
+                if msg_type == 'MISSION_COUNT':
+                    self._handle_mission_count(msg)
+
+                # 2. 拦截：收到地面站发来的开始执行命令
+                elif msg_type == 'COMMAND_LONG':
+                    if msg.command == mavutil.mavlink.MAV_CMD_MISSION_START:
+                        self._handle_mission_start()
+                    elif msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
+                        print(f"[QGC Proxy] 拦截到地面站解锁指令 -> 参数: {msg.param1}")
+
+            except Exception as e:
+                print(f"[QGC Proxy] 接收异常: {e}")
+                time.sleep(0.1)
+
+    def _handle_mission_count(self, msg):
+        """处理航点上传握手协议"""
+        target_system = msg.target_system
+        target_component = msg.target_component
+        wp_count = msg.count
+
+        print(f"[QGC Proxy] 检测到 QGC 正在上传航线，包含航点数量: {wp_count}")
+
+        temp_waypoints = []
+        handshake_success = True
+
+        # 依次向 QGC 请求每一个航点的具体坐标
+        for i in range(wp_count):
+            # 向 QGC 请求第 i 个航点 (使用 MISSION_REQUEST_INT 保证高精度)
+            self.qgc.mav.mission_request_int_send(
+                target_system,
+                target_component,
+                i,
+                mavutil.mavlink.MAV_MISSION_TYPE_MISSION
+            )
+
+            # 等待 QGC 回应具体的航点条目
+            item_msg = self.qgc.recv_match(type='MISSION_ITEM_INT', blocking=True, timeout=2.0)
+
+            if item_msg is None:
+                print(f"[QGC Proxy] 错误: 请求航点 {i} 超时，中止握手。")
+                handshake_success = False
+                break
+
+            # 解析地理/局部坐标
+            # frame 1: MAV_FRAME_GLOBAL_INT (室外经纬度)
+            # frame 12: MAV_FRAME_LOCAL_NED (室内/本地局部坐标)
+            frame = item_msg.frame
+
+            # 坐标转换逻辑（核心翻译部分）
+            if frame == mavutil.mavlink.MAV_FRAME_LOCAL_NED:
+                # 如果同伴在 QGC 规划的是室内本地 NED 坐标，直接除以 1000 转换为米
+                x = item_msg.x / 1000.0
+                y = item_msg.y / 1000.0
+                z = item_msg.z / 1000.0  # 注意：飞控 NED 的 Z 轴向下为正
+            elif frame in [mavutil.mavlink.MAV_FRAME_GLOBAL_INT, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT]:
+                # 如果是室外 GPS 航点
+                # x 乘以 1e-7 得到 纬度，y 乘以 1e-7 得到 经度
+                lat = item_msg.x / 1e7
+                lon = item_msg.y / 1e7
+                alt = item_msg.z  # 高度（米）
+
+                # 【关键生产力转换】：在此处可以调用你在本地计算的投影函数，将经纬度转换为相对起始点的 (x, y) 米
+                x, y = self._gps_to_local_meters(lat, lon)
+                z = -alt  # 转换为你本地代码习惯的 Z 轴定义（若你的代码向上为正，则取负值；若严格遵循 NED 则保持向下为正）
+            else:
+                x, y, z = 0.0, 0.0, 0.0
+
+            # 提取航点附加动作参数（如偏航角、悬停时间）
+            param1_hold_time = item_msg.param1  # 悬停时间（秒）
+            param4_yaw = item_msg.param4  # 期望偏航角（度）
+
+            # 翻译并打包成符合你本地脚本格式的字典对象
+            wp_dict = {
+                "index": i,
+                "command": item_msg.command,  # 如 MAV_CMD_NAV_WAYPOINT
+                "x": x,
+                "y": y,
+                "z": z,
+                "hold_time": param1_hold_time if param1_hold_time > 0 else 3.0,  # 缺省悬停3秒
+                "yaw": param4_yaw
+            }
+            temp_waypoints.append(wp_dict)
+            print(f" -> 成功拦截并翻译航点 [{i}]: X={x:.2f}, Y={y:.2f}, Z={z:.2f}, 偏航={param4_yaw}°")
+
+        if handshake_success:
+            # 向 QGC 发送 MISSION_ACK，宣告上传成功，让地面站界面显示绿色的“保存成功”
+            self.qgc.mav.mission_ack_send(
+                target_system,
+                target_component,
+                mavutil.mavlink.MAV_MISSION_RESULT_ACCEPTED,
+                mavutil.mavlink.MAV_MISSION_TYPE_MISSION
+            )
+            with self.lock:
+                self.translated_waypoints = temp_waypoints
+                self.is_mission_ready = True
+            print("[QGC Proxy] 航线拦截握手完美结束！影子航线已在香橙派内存中就绪。")
+        else:
+            # 告诉 QGC 上传失败
+            self.qgc.mav.mission_ack_send(
+                target_system,
+                target_component,
+                mavutil.mavlink.MAV_MISSION_RESULT_ERROR,
+                mavutil.mavlink.MAV_MISSION_TYPE_MISSION
+            )
+
+    def _gps_to_local_meters(self, lat, lon):
+        """
+        一个简易的平地经纬度转局部米制坐标函数。
+        实际使用中，可以保存起飞点的经纬度作为参考原点（Home点）。
+        """
+        # 伪代码演示，你可以替换为标准的 GeographicLib 或 pyproj 投影库
+        # 这里假设以固定参考点进行转换
+        home_lat, home_lon = 30.000000, 120.000000
+        delta_lat = lat - home_lat
+        delta_lon = lon - home_lon
+
+        # 地球每度纬度约为 111320 米
+        x_meters = delta_lat * 111320.0
+        # 经度长度随纬度余弦收缩
+        y_meters = delta_lon * 111320.0 * 0.866
+        return x_meters, y_meters
+
+    def _handle_mission_start(self):
+        """当地面站点击『开始任务』时触发该本地回调"""
+        print("[QGC Proxy] 地面站发出了【START MISSION】指令！")
+        # 这里用来激活你在香橙派上写的状态机
+        # 例如将全局事件设置为 True：event_start_local_mission.set()
+
+    def get_local_waypoints(self):
+        """提供给你本地外部业务循环读取的接口"""
+        with self.lock:
+            if self.is_mission_ready:
+                return self.translated_waypoints
+            return None
+
+    def close(self):
+        self.running = False
\ No newline at end of file
diff --git a/run_mission.py b/run_mission.py
index 8ed654f..46b387a 100644
--- a/run_mission.py
+++ b/run_mission.py
@@ -1,4 +1,5 @@
 import time
+import json
 from drone_controller.base_control import DroneController
 from drone_controller.mission_manager import MissionManager, FailsafeTriggered
 
@@ -10,10 +11,17 @@ flight_waypoints = [
     {'x': 0.0, 'y': 0.0, 'z': -1.2, 'yaw': 0},  # 航点 3：回到原点上方，高度降到 1.2 米
 ]
 
+def load_config(config_path="./config.json"):
+    with open(config_path, "r", encoding="utf-8") as f:
+        return json.load(f)
+
 
 def main():
+    # 加载配置
+    config = load_config()
+    cfg = config["flight_control"]
     # 2. 初始化底层 SDK 驱动
-    uav = DroneController(connection_string='/dev/ttyUSB0', baud=115200)
+    uav = DroneController(connection_string=cfg.get("connection_string"), baud=cfg.get("baud_rate"))
     if not uav.connect():
         print("物理串口打开失败，请检查数传连接或端口权限。")
         return