From 5e4f02fce171f7c701875554804d67d68c9f49ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= <yimiao9036@outlook.com>
Date: Wed, 24 Jun 2026 15:19:26 +0800
Subject: [PATCH] =?UTF-8?q?2026.6.24=E4=BF=AE=E6=94=B9=E8=BD=AC=E6=8D=A2?=
 =?UTF-8?q?=E9=93=BE=E8=B7=AF=E4=B8=AD=E9=80=BB=E8=BE=91=E9=94=99=E8=AF=AF?=
 =?UTF-8?q?=EF=BC=8C=E4=BF=9D=E8=AF=81=E5=88=9D=E5=A7=8B=E7=8A=B6=E6=80=81?=
 =?UTF-8?q?=E6=AD=A3=E5=B8=B8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 drone_controller/mission_manager.py | 22 +++++++++++++++++++++-
 run_mission.py                      |  5 +++--
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py
index d43ae4c..d5f989e 100644
--- a/drone_controller/mission_manager.py
+++ b/drone_controller/mission_manager.py
@@ -35,6 +35,7 @@ logger = logging.getLogger(__name__)
 
 class MissionState(Enum):
     """无人机任务状态枚举"""
+    WAITING_WAYPOINTS=-1  # 等待地面站下发航点
     INIT = 0              # 初始化 / 等待连接
     ARMING = 1            # 解锁中
     TAKEOFF = 2           # 自动起飞中
@@ -124,6 +125,17 @@ class MissionManager:
         # ---- 任务参数 ----
         # 航点列表（外部队列已经使用 NED 坐标系，Z 负值向上）
         self.waypoints = list(waypoints) if waypoints else []
+        #  根据是否有航点决定初始状态
+        if self.waypoints:
+            # 有预置航点（如测试用硬编码），直接进入初始化
+            self.state = MissionState.INIT
+            self._waypoints_ready = True
+            logger.info("MissionManager 初始化完成，已有 %d 个航点", len(self.waypoints))
+        else:
+            # 无航点，进入等待状态（等待 QGC 下发）
+            self.state = MissionState.WAITING_WAYPOINTS
+            self._waypoints_ready = False
+            logger.info("MissionManager 初始化完成，等待 QGC 下发航点...")
         self.target_altitude = -abs(target_altitude)   # NED Z（负值向上）
         self.arrival_radius = arrival_radius
         self.hold_duration = hold_duration
@@ -134,7 +146,6 @@ class MissionManager:
         )
 
         # ---- 状态机 ----
-        self.state = MissionState.INIT
         self.current_wp_index = 0
 
         # ---- 当前目标航点（共享给外部主循环 / UAVControlLoop） ----
@@ -203,6 +214,15 @@ class MissionManager:
                 logger.warning("⚠️ 尚未收到 STATE，跳过本轮处理")
                 return
 
+        # ---- 特殊状态，等待航点 ----
+        if self.state == MissionState.WAITING_WAYPOINTS:
+            if self._waypoints_ready:
+                logger.info("✅ 航点已就绪，转入 INIT 状态")
+                self.state = MissionState.INIT
+                self._state_start_time = time.time()
+            else:
+                return
+
         # ---- Failsafe 越权检查 ----
         if self.state not in (MissionState.INIT, MissionState.FINISHED):
             if state is not None:
diff --git a/run_mission.py b/run_mission.py
index efe849d..e3ea62a 100644
--- a/run_mission.py
+++ b/run_mission.py
@@ -507,14 +507,15 @@ def main():
 
     # 允许命令行指定配置文件路径
     config_file = sys.argv[1] if len(sys.argv) > 1 else "./config.json"
-
+    # 也可以传入固定点
     # waypoints = [
     #         {'x': 1.0, 'y': 0.0, 'z': -1.5, 'yaw': 0},
     #         {'x': 1.0, 'y': 1.0, 'z': -1.5, 'yaw': 90},
     #         {'x': 0.0, 'y': 0.0, 'z': -1.2, 'yaw': 0},
     #     ]
+    #  orchestrator = MissionOrchestrator(config_path=config_file,waypoints=waypoints)
 
-    orchestrator = MissionOrchestrator(config_path=config_file,waypoints=None)
+    orchestrator = MissionOrchestrator(config_path=config_file,waypoints=[])
     orchestrator.start()
     orchestrator.run()