部分重构与自动巡飞

2026-06-02 11:03:37 +08:00
parent e92124d3bf
commit c6fc0dd775
10 changed files with 587 additions and 7 deletions
@@ -0,0 +1,59 @@
+# run_mission.py
+import time
+from drone_controller.base_control import DroneController
+from drone_controller.mission_manager import MissionManager
+
+def main():
+    # 1. 实例化飞控底层驱动 SDK
+    # 注意：根据现场情况修改你的串口和波特率
+    drone = DroneController(connection_string='/dev/ttyUSB0', baud=115200)
+    
+    try:
+        # 启动底层数据链连接与异步状态遥测线程
+        drone.connect()
+        
+        # 2. 规划室内巡点航点列表 (NED 相对局部坐标系)
+        # 规则说明：X-北为正, Y-东为正, Z-向下为正（所以想要在 1.5 米高度飞，Z 必须为 -1.5）
+        # yaw：偏航角角度(0-360度)，0代表正北
+        my_waypoints = [
+            {'x': 1.0,  'y': 0.0,  'z': -1.5, 'yaw': 0},   # 点1：向正前方推进1米
+            {'x': 1.0,  'y': 1.0,  'z': -1.5, 'yaw': 90},  # 点2：保持前推，向右平移1米，机头偏向正东
+            {'x': 0.0,  'y': 1.0,  'z': -1.5, 'yaw': 180}, # 点3：倒飞回起点水平线，机头朝向正南
+            {'x': 0.0,  'y': 0.0,  'z': -1.5, 'yaw': 270}  # 点4：返回起飞原点，机头朝向正西
+        ]
+        
+        # 3. 初始化任务管理器模块
+        mission = MissionManager(
+            drone=drone,
+            waypoints=my_waypoints,
+            target_altitude=1.5,    # 自动起飞高度 1.5 米
+            arrival_radius=0.25,    # 判定到点的欧氏距离误差阈值锁定在 25 厘米以内
+            hold_duration=4.0       # 每个点到达后悬停停留 4 秒
+        )
+        
+        print("\n================ 自动巡点系统就绪 ================")
+        print("提示: 现场安全员请时刻保持遥控器手柄在握，一旦发生异常，切出控制模式即可随时越权人工接管。")
+        print("================================================\n")
+        
+        # 4. 高频业务轮询主循环
+        while True:
+            # 驱动任务状态机
+            is_mission_finished = mission.update()
+            
+            if is_mission_finished:
+                print("\n[通知] 主程序检测到巡航任务模块已正常运行结束，退出程序。")
+                break
+                
+            # 20Hz 的控制频率（50ms一拍），既能保证位置控制实时下发，又不会拖垮处理器
+            time.sleep(0.05)
+            
+    except KeyboardInterrupt:
+        print("\n🛑 用户手动中止程序 (Ctrl+C)！正在触发紧急安全退出。")
+    finally:
+        # 5. 清理与释放资源
+        print("[清理] 正在关闭后台遥测数据链线程...")
+        drone.disconnect()
+        print("[清理] 程序已安全解脱。")
+
+if __name__ == '__main__':
+    main()