From bfd2ee42532b95d8a7a56d8ee12e00fe6e917a79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Fri, 3 Jul 2026 11:34:01 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=9D=83=E9=80=BB=E8=BE=91?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- drone_controller/mission_manager.py | 13 +++++++++++++ infer_camera_modular.py | 7 +++++++ 2 files changed, 20 insertions(+) diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py index 3bd0566..ce182e0 100644 --- a/drone_controller/mission_manager.py +++ b/drone_controller/mission_manager.py @@ -230,6 +230,19 @@ class MissionManager: if self.state not in (MissionState.INIT, MissionState.FINISHED): if state is not None: mode = state["drone"].get("mode", "") + # 飞控切到 AUTO.LAND 或 AUTO.RTL → 自动让渡控制权 + if mode in ("AUTO.LAND", "AUTO.RTL"): + logger.warning(f"⚠️ 飞控切换到 {mode}，Python 端主动让渡控制权") + # 根据模式决定切换到的状态 + if mode == "AUTO.LAND": + self.state = MissionState.LANDING + elif mode == "AUTO.RTL": + self.state = MissionState.RETURNING + # 重置状态开始时间，让降落/返航处理从当前帧开始 + self._state_start_time = time.time() + # 停止控制线程（通过 run_mission.py 的协调逻辑） + # 注意：state 变化后，下一帧 _coordinate_control 会自动停止线程 + return if not self._is_mode_allowed_for_state(mode, self.state): logger.warning( "\n⚠️ [Failsafe] 飞控模式被切为: %s！" diff --git a/infer_camera_modular.py b/infer_camera_modular.py index b6e5b7f..284a638 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -709,6 +709,13 @@ class UAVControlLoop: # ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ---- self.logger.debug("SETPOINT: control_mode=VELOCITY " "vx=%.2f vy=%.2f vz=%.2f", vx, vy, vz) + # ---- 防御性检查：飞控已切到 AUTO.LAND/AUTO.RTL 则不再发送 ---- + state = self.proxy.get_latest_state() + if state: + mode = state.get("drone", {}).get("mode", "") + if mode in ("AUTO.LAND", "AUTO.RTL"): + # 静默跳过，不发送 SETPOINT + continue ok, ack = self.proxy.send_setpoint( vx=vx, vy=vy, vz=vz, yaw_rate=0.0,