diff --git a/control.py b/control.py index 08c3250..3ff4d43 100644 --- a/control.py +++ b/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即可） - self.master.target_system, # 目标系统 ID - self.master.target_component, # 目标组件 ID + 0, # 系统时间戳（填0即可） + self.master.target_system, # 目标系统 ID + self.master.target_component, # 目标组件 ID mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # 🌟 关键：指定控制基于“机体本地坐标系” - type_mask, # 掩码配置 - 0, 0, 0, # x, y, z 位置 (被忽略) - vx, vy, vz, # 🌟 实际生效的机体期望物理速度 (m/s) - 0, 0, 0, # ax, ay, az 加速度 (被忽略) - 0, 0 # yaw, yaw_rate (被忽略) + type_mask, # 掩码配置 + 0, 0, 0, # x, y, z 位置 (被忽略) + vx, vy, vz, # 🌟 实际生效的机体期望物理速度 (m/s) + 0, 0, 0, # ax, ay, az 加速度 (被忽略) + 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 @@ -135,6 +174,4 @@ class UAVController: self.vy.reset() self.vz.reset() print(f"🚨 警告: 目标丢失超过 {self.LOST_THRESHOLD} 帧！MAVLink 发送安全原地锁死悬停指令 🔒") - self.send_body_velocity_cmd(0.0, 0.0, 0.0) - - + self.send_body_velocity_cmd(0.0, 0.0, 0.0) \ No newline at end of file diff --git a/fc_test.py b/fc_test.py new file mode 100644 index 0000000..a2136f2 --- /dev/null +++ b/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用户终止，退出监控。") \ No newline at end of file diff --git a/saved_video/ground_station_record.mp4 b/saved_video/ground_station_record.mp4 deleted file mode 100644 index f321eda..0000000 Binary files a/saved_video/ground_station_record.mp4 and /dev/null differ diff --git a/start_npu_infer.sh b/start_npu_infer.sh new file mode 100644 index 0000000..afee5a3 --- /dev/null +++ b/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 \ No newline at end of file