2026.6.3更新

drone_controller/mission_manager.py

@@ -4,142 +4,183 @@ from enum import Enum
 
 
 class MissionState(Enum):
-    INIT = 0        # 初始化/等待连接
+    INIT = 0        # 初始化/流式发送指令准备中
-    ARMING = 1      # 解锁中
+    ARMING = 1      # 模式切换与解锁中
-    TAKEOFF = 2     # 自动起飞中
+    TAKEOFF = 2     # 自动起飞中（虚拟航点导航起飞）
     NAVIGATING = 3  # 航点巡航导航
-    HOLD_TASK = 4   # 到达航点后的原地任务悬停
+    HOLD_TASK = 4   # 到达航点后的原地任务悬停检测
-    RETURNING = 5   # 返航或直接准备降落
+    RETURNING = 5   # 任务完成，准备切换降落
     LANDING = 6     # 自动降落中
-    FINISHED = 7    # 任务结束/已上锁
+    FINISHED = 7    # 任务结束/已上锁退出
+
+
+class FailsafeTriggered(Exception):
+    """自定义安全异常：当安全员人工介入切出自控模式时抛出，用于暴力熔断主循环"""
+    pass
 
 
 class MissionManager:
-    def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0):
+    def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0, return_home=True):
         """
+        专为 PX4 固件优化的自动巡点状态机管理器
         :param drone: 实例化的 DroneController 底层驱动对象
         :param waypoints: 航点列表，格式为 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ...]
-                          注意：NED 坐标系中，Z轴向上为负，所以起飞或目标高度应为负值
+                          注意：NED 坐标系中，Z轴向下为负，所以起飞或目标高度输入正数，内部会自动转为NED负值
         :param target_altitude: 初始起飞高度（米，正数，内部会自动转为NED负值）
         :param arrival_radius: 判定航点到达的三维欧氏距离阈值（米）
-        :param hold_duration: 在每个航点到达后，执行任务（或悬停）的停留时间（秒）
+        :param hold_duration: 在每个航点到达后，原地执行悬停/任务的停留时间（秒）
         """
         self.drone = drone
         self.waypoints = waypoints
         self.target_altitude = -abs(target_altitude)  # 强制转为 NED 坐标系的负高度
         self.arrival_radius = arrival_radius
         self.hold_duration = hold_duration
+        self.return_home = return_home
 
         self.state = MissionState.INIT
         self.current_wp_index = 0
         self.hold_start_time = None
 
+        # 链路活性流式计数器：确保以固定频率下发心跳期望（防PX4失控刹车）
+        self.last_send_time = 0.0
+        self.send_interval = 0.2  # 0.2秒对应 5Hz 下发频率
+
     def calculate_distance(self, target_x, target_y, target_z):
-        """计算无人机当前局部位置与目标点之间的三维欧氏距离"""
+        """计算无人机当前局部局部位置（来自飞控 LOCAL_POSITION_NED 遥测）与目标的欧氏距离"""
-        dx = target_x - self.drone.local_x
+        curr_x, curr_y, curr_z = self.drone.local_position
-        dy = target_y - self.drone.local_y
+        dx = target_x - curr_x
-        dz = target_z - self.drone.local_z
+        dy = target_y - curr_y
+        dz = target_z - curr_z
         return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
 
+    def _stream_target_position(self, x, y, z, yaw):
+        """维持 PX4 OFFBOARD 模式所需的流式位置期望下发（限制 5Hz 频率）"""
+        now = time.time()
+        if now - self.last_send_time >= self.send_interval:
+            # 假设你的底层 base_control.py 中有 send_target_position 接口
+            self.drone.send_target_position(x, y, z, yaw)
+            self.last_send_time = now
+
     def update(self):
         """
         核心状态机轮询函数。
-        高层主循环高频调用此函数，根据当前状态驱动飞行任务。
+        高层主循环（如主循环 10Hz）必须不间断调用此函数驱动飞行任务。
+        一旦检测到人肉介入，直接抛出 FailsafeTriggered 异常终止运行。
         """
-        # 安全人肉兜底：如果安全员从遥控器切出 GUIDED 模式，状态机挂起，不执行任何行为
+        # ==================== 1. 安全人肉熔断机制 ====================
-        if self.state not in [MissionState.INIT, MissionState.ARMING]:
+        if self.state not in [MissionState.INIT, MissionState.ARMING, MissionState.FINISHED]:
-            if self.drone.current_mode not in ['GUIDED', 'OFFBOARD']:
+            if self.drone.current_mode != 'OFFBOARD':
-                print(f"\r\n⚠️  [警告] 安全员切出控制模式！当前模式为: {self.drone.current_mode}, 任务管理程序挂起...", end="")
+                print(f"\n🚨 [熔断保护] 检测到飞控退出自控模式！当前模式为: {self.drone.current_mode}")
-                return False
+                print("🚨 任务管理器瞬间拉断，上位机停止发送所有控制流，完全交由安全员接管！")
+                raise FailsafeTriggered("安全员强行越权接管，自控流程熔断")
 
-        # ----------------- 状态机逻辑 -----------------
+        # ==================== 2. 状态机业务分支 ====================
+
+        # --- STATE: INIT ---
         if self.state == MissionState.INIT:
             if self.drone.is_connected:
-                print("\n[状态] 飞控通信已建立，准备切换至 OFFBOARD 模式并解锁...")
+                # PX4 要求：在切入 OFFBOARD 模式之前，上位机必须已经开始流式下发位置/速度期望包，
-                if self.drone.set_mode('OFFBOARD'):   #若为 PX4 则是OFFBOARD; ArduPilot 则为 GUIDED
+                # 否则飞控会拒绝切换模式。此处我们在原位流式灌入当前局部坐标作为活性维持。
+                cx, cy, cz = self.drone.local_position
+                self._stream_target_position(cx, cy, cz, self.drone.yaw)
+
+                print("\n[状态] 飞控通信正常，正在向 PX4 流式下发原位期望以激活控制链路...")
+                print("[动作] 正在向飞控请求切换至 OFFBOARD 模式并解锁...")
+
+                if self.drone.set_mode('OFFBOARD'):
+                    # 假设底层接口 drone.arm() 会发送解锁包并等待确认
+                    if self.drone.arm():
                         self.state = MissionState.ARMING
             else:
-                print("\r[状态] 等待飞控心跳包连接...", end="", flush=True)
+                print("\r[状态] 等待飞控 MAVLink 串口通信建立...", end="", flush=True)
 
+        # --- STATE: ARMING ---
         elif self.state == MissionState.ARMING:
-            print("[状态] 正在发送解锁指令...")
+            # 持续流式下发当前点，防模式闪退
-            if self.drone.arm():
+            cx, cy, cz = self.drone.local_position
-                print("[成功] 飞控解锁成功，正在下发自动起飞指令...")
+            self._stream_target_position(cx, cy, cz, self.drone.yaw)
-                # 转换到起飞高度，注意发送的是相对高度或绝对位置
+
-                if self.drone.takeoff(abs(self.target_altitude)):
+            if self.drone.is_armed and self.drone.current_mode == 'OFFBOARD':
+                print("\n[成功] 飞控已成功解锁，且处于 OFFBOARD 模式。开始执行平滑虚拟起飞流程...")
                 self.state = MissionState.TAKEOFF
-            else:
-                print("[重试] 解锁未被飞控确认，1秒后重新尝试解锁...")
-                time.sleep(1)
 
+        # --- STATE: TAKEOFF (虚拟航点平滑起飞) ---
         elif self.state == MissionState.TAKEOFF:
-            # 判定起飞是否到达指定高度（NED 坐标系下，比较 z 值）
+            # PX4 泛用性最强的起飞：发送原地 (0, 0) 坐标，配合目标负高度值
-            dist_to_takeoff = abs(self.drone.local_z - self.target_altitude)
+            self._stream_target_position(0.0, 0.0, self.target_altitude, 0.0)
-            print(
-                f"\r[起飞中] 当前高度: {-self.drone.local_z:.2f}M -> 目标: {abs(self.target_altitude)}M | 误差: {dist_to_takeoff:.2f}M",
-                end="", flush=True)
 
-            if dist_to_takeoff <= self.arrival_radius:
+            # 计算距离起飞航点的距离
-                print(f"\n[成功] 已到达起飞高度！开始执行巡点导航，共有 {len(self.waypoints)} 个航点。")
+            dist = self.calculate_distance(0.0, 0.0, self.target_altitude)
-                if len(self.waypoints) > 0:
+            print(f"\r[起飞中] 目标高度: {-self.target_altitude:.2f}m | 当前距离起飞点: {dist:.2f}m", end="",
+                  flush=True)
+
+            if dist <= self.arrival_radius:
+                print(f"\n[完成] 虚拟起飞安全到达目标高度！开始按顺序巡航。总计航点数: {len(self.waypoints)}")
                 self.current_wp_index = 0
                 self.state = MissionState.NAVIGATING
-                else:
-                    self.state = MissionState.RETURNING
 
+        # --- STATE: NAVIGATING (航点巡航导航) ---
         elif self.state == MissionState.NAVIGATING:
             wp = self.waypoints[self.current_wp_index]
-            # 持续高频向飞控下发当前期望航点
+            # 流式下发当前期望的绝对 NED 坐标与偏航角（激活PX4控制环并防止失控刹车）
-            self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
+            self._stream_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
 
-            # 计算距离
             dist = self.calculate_distance(wp['x'], wp['y'], wp['z'])
             print(
-                f"\r[巡航中] 正在飞往航点 [{self.current_wp_index + 1}]: 目标({wp['x']}, {wp['y']}, {wp['z']}) | 剩余距离: {dist:.2f}米",
+                f"\r[巡航中] 正在前往航点 [{self.current_wp_index + 1}/{len(self.waypoints)}] | 剩余距离: {dist:.2f}m",
                 end="", flush=True)
 
-            # 到达判定
             if dist <= self.arrival_radius:
-                print(f"\n[到达] 已成功到达航点 [{self.current_wp_index + 1}]！开始悬停执行特定任务...")
+                print(f"\n[到达] 成功进入航点 [{self.current_wp_index + 1}] 容差圈！开始执行就地悬停任务...")
                 self.hold_start_time = time.time()
                 self.state = MissionState.HOLD_TASK
 
+        # --- STATE: HOLD_TASK (到达航点后的原地任务悬停/拍照) ---
         elif self.state == MissionState.HOLD_TASK:
             wp = self.waypoints[self.current_wp_index]
-            # 悬停期间依然要持续发点，保持飞机锁死在原位
+            # 即使在原地停留/拍照，上位机也绝对不能“闭嘴”，必须以 5Hz 持续锁定该点坐标
-            self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
+            self._stream_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
 
             elapsed = time.time() - self.hold_start_time
             print(
-                f"\r[任务中] 航点 [{self.current_wp_index + 1}] 原地悬停检测中... 已耗时: {elapsed:.1f}s / {self.hold_duration}s",
+                f"\r[任务中] 航点 [{self.current_wp_index + 1}] 原地执行中... 已耗时: {elapsed:.1f}s / {self.hold_duration}s",
                 end="", flush=True)
 
             if elapsed >= self.hold_duration:
-                print(f"\n[完成] 航点 [{self.current_wp_index + 1}] 任务执行完毕。")
+                print(f"\n[完成] 航点 [{self.current_wp_index + 1}] 停留时间结束。")
-                # 切换下一个点
                 self.current_wp_index += 1
+
                 if self.current_wp_index < len(self.waypoints):
                     self.state = MissionState.NAVIGATING
                 else:
-                    print("\n[完成] 所有航点均已巡视完毕！准备返航/降落。")
+                    print("\n[巡视完毕] 所有计划航点均已完成！准备进入返航/降落切换阶段。")
+                    if self.return_home:
+                        self.state = MissionState.RETURNING
                     self.state = MissionState.RETURNING
 
+        # --- STATE: RETURNING ---
         elif self.state == MissionState.RETURNING:
-            print("\n[降落中] 正在下发自动降落指令 (LAND)...")
+            # 持续发送当前点维持链路
-            if self.drone.set_mode('LAND'):
+            cx, cy, cz = self.drone.local_position
+            self._stream_target_position(cx, cy, cz, self.drone.yaw)
+
+            print("\n[降落中] 正在向 PX4 下发自动降落指令 (AUTO.LAND)...")
+            # 提示：PX4 固件的标准降落模式名称通常为 'AUTO.LAND' 或 'LAND'，依底层驱动映射而定
+            if self.drone.set_mode('AUTO.LAND') or self.drone.set_mode('LAND'):
                 self.state = MissionState.LANDING
             else:
-                print("⚠️ 降落模式切换失败，正在重新下发...")
+                print("⚠️ 切换降落模式失败，正在高频重试...")
 
+        # --- STATE: LANDING ---
         elif self.state == MissionState.LANDING:
-            # 判断是否已经安全着陆并自动上锁
+            # 进入飞控原生 LAND 模式后，控制权已交还给飞控底层，此时上位机可以安全停止控制包输出了。
+            print("\r[降落中] 正在等待飞控触地并自动上锁...", end="", flush=True)
             if not self.drone.is_armed:
-                print("\n[成功] 检测到无人机已安全着陆并自动上锁。任务圆满结束！")
+                print("\n[完成] 无人机已安全着陆并自动锁桨！任务圆满结束。")
                 self.state = MissionState.FINISHED
-            else:
-                print(f"\r[降落中] 正在着陆... 当前高度: {-self.drone.local_z:.2f}M", end="", flush=True)
 
+        # --- STATE: FINISHED ---
         elif self.state == MissionState.FINISHED:
-            return True  # 任务全部结束
+            return True
 
         return False