From 8b957d72ab5f27ca2985a7e96453c8cec7751ff8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= <yimiao9036@outlook.com>
Date: Mon, 15 Jun 2026 15:42:45 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=A8=A1=E5=BC=8F=E4=B8=BA?=
 =?UTF-8?q?=E7=A9=BAbug?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 config.json             |  2 +-
 infer_camera_modular.py | 31 ++++++++++++++++++++++++++-----
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/config.json b/config.json
index 94db9d0..618c950 100644
--- a/config.json
+++ b/config.json
@@ -26,7 +26,7 @@
         "baud_rate": 57600
     },
     "fc_info":{
-        "type": "ardupilot"
+        "type": "px4"
     },
     "pid_y": {
         "kp": 1.0,
diff --git a/infer_camera_modular.py b/infer_camera_modular.py
index 6fcfa6d..6e23b8f 100644
--- a/infer_camera_modular.py
+++ b/infer_camera_modular.py
@@ -375,15 +375,36 @@ class UAVControlLoop:
         age_us, _ = self.proxy.get_state_freshness(state)
         age_s = (age_us or 0) / 1_000_000
 
+        if not mode or mode not in ['GUIDED', 'OFFBOARD']:
+            target_mode = 'GUIDED'  # ArduPilot
+            # 如果飞控是 PX4，则使用 'OFFBOARD'
+            if self.fc_cfg.get("fc_info", {}).get("type") == "px4":
+                target_mode = 'OFFBOARD'
+            self.logger.info("切换至 %s 模式...", target_mode)
+            ok, ack = self.proxy.send_command(target_mode)
+            if not ok:
+                self.logger.error("模式切换失败: %s", ack)
+                return False
+            # 等待模式切换生效（最多 2 秒）
+            for i in range(20):
+                state = self.proxy.get_latest_state()
+                if state and state["drone"]["mode"] == target_mode:
+                    self.logger.info("已进入 %s 模式", target_mode)
+                    break
+                time.sleep(0.1)
+            else:
+                self.logger.error("模式切换超时")
+                return False
+
         self.logger.info("初始状态: mode=%s armed=%s alt_rel=%.1fm state_age=%.2fs",
                          mode, armed, alt_rel, age_s)
 
         # ---- ③ 解锁（如未解锁） ----
-        # if not armed:
-        #     if self._do_arm():
-        #         armed = True
-        #     else:
-        #         return False
+        if not armed:
+            if self._do_arm():
+                armed = True
+            else:
+                return False
 
         # ---- ④ 起飞到目标高度（如高度不足） ----
         takeoff_alt = self.fc_cfg.get("takeoff_alt", 5.0)