From 96201abab480d71802a09b1b84ca82fff93c9c00 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= <yimiao9036@outlook.com>
Date: Mon, 29 Jun 2026 14:44:34 +0800
Subject: [PATCH] =?UTF-8?q?2026.6.29=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/mission_manager.py | 26 +++++++++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py
index ae54e53..62cb972 100644
--- a/drone_controller/mission_manager.py
+++ b/drone_controller/mission_manager.py
@@ -393,7 +393,7 @@ class MissionManager:
                 alt=takeoff_alt,
                 alt_frame="RELATIVE",
                 speed=3.0, action="TAKEOFF",
-                hover_time=5.0, yaw=state["drone"]["yaw"],
+                yaw=state["drone"]["yaw"],
                 acceptance_radius=0.5,
                 is_last=True
             )
@@ -505,6 +505,30 @@ class MissionManager:
             return
 
         wp = self.waypoints[self.current_wp_index]
+        cmd = wp.get("command", "WAYPOINT")
+
+        # 特殊航点分支选择
+        # 起飞航点
+        if cmd == "TAKEOFF":
+            # 飞到原点 + 目标高度
+            self.proxy.send_setpoint(
+                x=0, y=0, z=wp.get("z", self.target_altitude),
+                yaw=wp.get("yaw", 0.0),control_mode="POSITION",
+            )
+            current_z = self._get_current_z(state)
+            target_z = wp.get("z", self.target_altitude)
+            if abs(current_z - target_z) < 0.5:
+                self.current_wp_index += 1
+            return
+
+        # 降落航点
+        if cmd == "LAND":
+            logger.info("🛬 执行降落")
+            self.proxy.send_command("LAND")
+            self.state = MissionState.LANDING
+            return
+
+
         wp_x = wp["x"]
         wp_y = wp["y"]
         wp_z = wp.get("z", self.target_altitude)