diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py index 62cb972..55376f2 100644 --- a/drone_controller/mission_manager.py +++ b/drone_controller/mission_manager.py @@ -332,10 +332,15 @@ class MissionManager: armed = drone.get("armed", False) mode = drone.get("mode", "") alt_rel = drone.get("alt_rel", 0.0) + home = state.get("home", {}) logger.info("[INIT] mode=%s armed=%s alt_rel=%.1fm", mode, armed, alt_rel) + if not home.get("valid", False): + logger.warning("[ARMING] home.valid=false,等待起飞点有效...") + return + # A) 已在 OFFBOARD + 已解锁 → 直接巡航 if armed and mode == "OFFBOARD": logger.info("[INIT] 已在 OFFBOARD,直接进入航点巡航") @@ -372,6 +377,7 @@ class MissionManager: """ try: armed = state["drone"].get("armed", False) + home = state.get("home", {}) if not armed: elapsed = time.time() - self._state_start_time @@ -383,6 +389,10 @@ class MissionManager: logger.info("[ARMING] 等待解锁... (%.1fs)", elapsed) return + if not home.get("valid", False): + logger.warning("[ARMING] home.valid=false,等待起飞点有效...") + return + # ---- 解锁成功 → 起飞 ---- takeoff_alt = -self.target_altitude # 正值(协议 altitude 为正) logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", takeoff_alt)