一些小修复

control.py

@@ -6,6 +6,7 @@ from pymavlink.mavextra import integral
 # 自定义 PID 部分
 class PID:
     """ 将 PID 部分分离出来为一个类"""
+
     def __init__(self, kp, ki, kd, max_out, min_out):
         self.kp = kp
         self.ki = ki
@@ -20,7 +21,7 @@ class PID:
     def update(self, error):
         current_time = time.time()
         dt = current_time - self.last_time
-        if dt <=0: dt = 0.033 # 30Hz
+        if dt <= 0: dt = 0.033  # 30Hz
 
         p_out = self.kp * error
 
@@ -43,9 +44,8 @@ class PID:
         self.integral = 0.0
 
 
-
 class UAVController:
-    def __init__(self, port='/dev/ttyUSB0', baud=115200):
+    def __init__(self, port='/dev/ttyUSB0', baud=57600):
         """
         初始化与飞控的 MAVLink 串口连接
             port:串口地址
@@ -57,6 +57,10 @@ class UAVController:
             print(f" [飞控通信] 已连接串口{port}, 请等待心跳包...")
             self.master.wait_heartbeat()
             print(" [飞控通信] 收到飞控心跳包, 链接正常")
+
+            # 🌟 核心改动：连接成功并确认收到心跳包后，立即开始解锁
+            self.arm_drone()
+
         except Exception as e:
             print(f" [飞控通信] 连接失败:{e}, 请检查")
             self.master = None
@@ -67,7 +71,42 @@ class UAVController:
 
         # 视觉丢失保护
         self.lost_counter = 0
-        self.LOST_THRESHOLD = 30    # 假设摄像头是30帧就对应1秒
+        self.LOST_THRESHOLD = 30  # 假设摄像头是30帧就对应1秒
+
+    def arm_drone(self):
+        """
+        🌟 核心新增：向飞控发送解锁 (ARM) 指令
+        已确认：无机翼且机身已固定，台架安全测试。
+        """
+        if self.master is None:
+            print(" [飞控通信] 错误：飞控未连接，无法发送解锁指令。")
+            return
+
+        print(" [⚠️安全提示] 正在向飞控下发解锁命令（确认无桨叶且已固定）...")
+
+        # 使用 command_long_send 发送 MAV_CMD_COMPONENT_ARM_DISARM 消息
+        # param1: 1 代表解锁 (ARM), 0 代表上锁 (DISARM)
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,  # 确认号 (Confirmation)
+            1,  # param1: 1 = ARM (解锁)
+            0,  # param2: 强制解锁参数（默认0，部分固件若强制防呆需填21196）
+            0, 0, 0, 0, 0  # param3 ~ param7 (未定义，填0)
+        )
+
+        # 等待并捕获飞控的 ACK 响应，超时时间设为 3 秒
+        ack = self.master.recv_match(type='COMMAND_ACK', blocking=True, timeout=3.0)
+        if ack and ack.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
+            if ack.result == 0:
+                print(" ✅ [飞控通信] 飞控解锁成功 (Armed Success)！")
+            else:
+                print(f" ❌ [飞控通信] 飞控拒绝了解锁请求，拒绝代码 (Result): {ack.result}")
+                print(
+                    " 👉 提示：可能触发了飞控内部的解锁安全检查（如GPS、遥控器信号未就绪），室内台架测试请在地面站将 'ARMING_CHECK' 临时设为 0。")
+        else:
+            print(" ⚠️ [飞控通信] 飞控未响应解锁确认，请检查飞控状态或数传指示灯。")
 
     def send_body_velocity_cmd(self, vx, vy, vz):
         """
@@ -95,19 +134,18 @@ class UAVController:
         )
 
         self.master.mav.set_position_target_local_ned_send(
-            0,                                          # 系统时间戳（填0即可）
+            0,  # 系统时间戳（填0即可）
-            self.master.target_system,                  # 目标系统 ID
+            self.master.target_system,  # 目标系统 ID
-            self.master.target_component,               # 目标组件 ID
+            self.master.target_component,  # 目标组件 ID
             mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED,  # 🌟 关键：指定控制基于“机体本地坐标系”
-            type_mask,                                  # 掩码配置
+            type_mask,  # 掩码配置
-            0, 0, 0,                                    # x, y, z 位置 (被忽略)
+            0, 0, 0,  # x, y, z 位置 (被忽略)
-            vx, vy, vz,                                 # 🌟 实际生效的机体期望物理速度 (m/s)
+            vx, vy, vz,  # 🌟 实际生效的机体期望物理速度 (m/s)
-            0, 0, 0,                                    # ax, ay, az 加速度 (被忽略)
+            0, 0, 0,  # ax, ay, az 加速度 (被忽略)
-            0, 0                                        # yaw, yaw_rate (被忽略)
+            0, 0  # yaw, yaw_rate (被忽略)
         )
 
-
+    def update_control(self, has_target, err_x=0.0, err_y=0.0):
-    def update_control(self, has_target, err_x = 0.0, err_y = 0.0):
         """
         接收检测结果并更新控制输出
         """
@@ -126,7 +164,8 @@ class UAVController:
             vx_cmd = 0.0
 
             print(
-                f"🎯 视觉锁定中 -> Err_X: {err_x:+.2f} Err_Y: {err_y:+.2f} | MAVLink 输出 -> vx: {vx_cmd:.2f}, vy(右): {vy_cmd:+.2f}, vz(下): {vz_cmd:+.2f} m/s")
+                f"\r[PID 控制速度] 🎯目标:已锁定 | 误差(X:{err_x:+.2f}, Y:{err_y:+.2f}) | 期望速度 -> Vx(前): {vx_cmd:+.2f} m/s, Vy(右): {vy_cmd:+.2f} m/s, Vz(下): {vz_cmd:+.2f} m/s",
+                end="", flush=True)
             self.send_body_velocity_cmd(vx_cmd, vy_cmd, vz_cmd)
         else:
             self.lost_counter += 1
@@ -136,5 +175,3 @@ class UAVController:
                 self.vz.reset()
                 print(f"🚨 警告: 目标丢失超过 {self.LOST_THRESHOLD} 帧！MAVLink 发送安全原地锁死悬停指令 🔒")
                 self.send_body_velocity_cmd(0.0, 0.0, 0.0)
-
-

fc_test.py

@@ -0,0 +1,47 @@
+import time
+from pymavlink import mavutil
+
+# 【核心配置】连接飞控
+# 这里的路径取决于香橙派和飞控是怎么连的。
+# 如果是通过 USB 转 TTL 模块插在香橙派上，通常是 '/dev/ttyUSB0'
+# 如果是直接连在香橙派的板载 GPIO 串口上，可能是 '/dev/ttyAMA0' 或 '/dev/ttyS5'
+# 波特率通常飞控默认是 115200 或 57600
+FC_CONNECTION = '/dev/ttyUSB0'
+BAUD_RATE = 57600
+
+print(f"正在尝试连接飞控 ({FC_CONNECTION})...")
+master = mavutil.mavlink_connection(FC_CONNECTION, baud=BAUD_RATE)
+
+# 等待飞控的心跳信号（确认物理链路已通）
+print("正在等待飞控心跳信号 (Heartbeat)...")
+master.wait_heartbeat()
+print("飞控连接成功！开始获取姿态数据...\n")
+
+try:
+    while True:
+        # 阻塞或等待接收 MAVLink 消息
+        # ATTITUDE 消息包含了无人机的姿态（滚转、俯仰、偏航）
+        msg = master.recv_match(type='ATTITUDE', blocking=True, timeout=1.0)
+
+        if msg:
+            # 飞控传出来的弧度（radians），需要转换成角度（degrees）
+            import math
+
+            roll = math.degrees(msg.roll)
+            pitch = math.degrees(msg.pitch)
+            yaw = math.degrees(msg.yaw)
+
+            # 如果偏航角是负数（-180到180），可以转换成 0-360 度标准航向
+            if yaw < 0:
+                yaw += 360
+
+            # 刷新打印到你的 PC SSH 终端屏幕上
+            print(f"【飞控姿态】 偏航角(Yaw): {yaw:.2f}° | 俯仰角(Pitch): {pitch:.2f}° | 滚转角(Roll): {roll:.2f}°",
+                  end='\r')
+        else:
+            print("未收到 ATTITUDE 消息，正在等待...", end='\r')
+
+        time.sleep(0.01)
+
+except KeyboardInterrupt:
+    print("\n用户终止，退出监控。")

start_npu_infer.sh

@@ -0,0 +1,53 @@
+#!/bin/bash
+### 香橙派 NPU 推理 + 图传 自启脚本（支持 Ctrl+C 停止）
+### 使用方法：chmod +x start_uav.sh
+
+# ==================== 【核心注入 1：引入华为昇腾环境变量】 ====================
+# 香橙派 AI Pro 默认的华为昇腾工具链环境变量脚本路径如下，必须优先加载它
+if [ -f "/usr/local/Ascend/ascend-toolkit/set_env.sh" ]; then
+    source /usr/local/Ascend/ascend-toolkit/set_env.sh
+fi
+
+# ==================== 【核心注入 2：引入 Conda 环境】 ====================
+if [ -f "/usr/local/miniconda3/etc/profile.d/conda.sh" ]; then
+    source "/usr/local/miniconda3/etc/profile.d/conda.sh"
+elif [ -f "/home/HwHiAiUser/miniconda3/etc/profile.d/conda.sh" ]; then
+    source "/home/HwHiAiUser/miniconda3/etc/profile.d/conda.sh"
+fi
+
+# 激活你的 (base) 虚拟环境
+conda activate base
+# ====================================================================
+
+WORK_DIR="/home/HwHiAiUser/opi-yolo"
+PYTHON_SCRIPT="infer_camera_direct.py"
+LOG_FILE="/tmp/npu_infer.log"
+
+PYTHON_BIN="python3"
+STOP=0
+
+# 捕获 Ctrl+C 等终止信号
+trap 'STOP=1; echo "收到终止信号，正在停止..." >> "$LOG_FILE"' INT TERM
+
+cd "$WORK_DIR" || exit 1
+
+echo "$(date): 启动 NPU 推理程序 (按 Ctrl+C 停止)" >> "$LOG_FILE"
+
+while [ $STOP -eq 0 ]; do
+    # 启动子进程，并保存 PID
+    $PYTHON_BIN "$PYTHON_SCRIPT" >> "$LOG_FILE" 2>&1 &
+    PYTHON_PID=$!
+
+    # 等待子进程结束或收到信号
+    wait $PYTHON_PID
+    EXIT_CODE=$?
+
+    if [ $STOP -eq 0 ]; then
+        # 非人为终止时（崩溃），自动重启
+        echo "$(date): 程序异常退出 (代码 $EXIT_CODE)，5 秒后重启..." >> "$LOG_FILE"
+        sleep 5
+    else
+        echo "$(date): 用户主动终止，不再重启" >> "$LOG_FILE"
+        break
+    fi
+done