songhaoyu/opi-yolo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

小小的修改

Browse Source
This commit is contained in:
songhaoyu
2026-05-28 11:03:15 +08:00
parent 244d64f872
commit 31da324c52
2 changed files with 132 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
control.py
+123 -86
View File
@@ -1,103 +1,140 @@
import time
from pymavlink import mavutil
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
self.kd = kd
self.max_out = max_out
self.min_out = min_out
self.last_err = 0.0
self.integral = 0.0
self.last_time = time.time()
def update(self, error):
current_time = time.time()
dt = current_time - self.last_time
if dt <=0: dt = 0.033 # 30Hz
p_out = self.kp * error
self.integral = error * dt
i_out = self.ki * self.integral
i_out = max(min(i_out, self.max_out * 0.2), self.min_out * 0.2)
d_out = self.kd * (self.last_err - error) / dt
output = p_out + i_out + d_out
output = max(min(output, self.max_out), self.min_out)
self.last_err = error
self.last_time = current_time
return output
def reset(self):
self.last_err = 0.0
self.integral = 0.0
class UAVController:
def __init__(self, port='/dev/ttyUSB0', baud=115200):
"""初始化与飞控的 MAVLink 串口连接"""
"""
初始化与飞控的 MAVLink 串口连接
port:串口地址
baud:波特率
"""
try:
print(f"正在尝试通过 MAVLink 连接飞控:{port}...")
self.master = mavutil.mavlink_connection(port, baud=baud)
print(f" [飞控通信] 已连接串口{port}, 请等待心跳包...")
self.master.wait_heartbeat()
print(" [飞控通信] 收到飞控心跳包, 链接正常")
except Exception as e:
print(f" [飞控通信] 连接失败:{e}, 请检查")
self.master = None
# =================== PID 参数 ===================
# 此时参数设定，err_x: 无人机的左右平移, err_y: 无人机的上下平移 (后续还要修改)
# ===============================================
self.kp = 1.2 # 响应强度
self.ki = 0.02 # 消除风偏
self.kd = 0.3 # 刹车减震
# 初始化Y轴 (左右方向) 和Z轴 (上下方向) 的 PID 调节器
self.vy = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.8, min_out=-0.8)
self.vz = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.4, min_out=-0.4)
# 内部缓存
self.last_err_x = 0.0
self.last_err_y = 0.0
self.integral_x = 0.0
self.integral_y = 0.0
self.last_time = time.time()
# 视觉丢失保护
self.lost_counter = 0
self.LOST_THRESHOLD = 30 # 假设摄像头是30帧就对应1秒
def update_and_send_command(self, err_x, err_y):
"""通过输入图像误差, 计算 PID 并通过 MAVLink 发送指令"""
if self.master is None:
return
current_time = time.time()
dt = current_time - self.last_time
if dt <=0: dt = 0.02 #防止除以0
# ------------ PID 计算 -------------
# 1. 比例项 (p)
p_out_x = self.kp * err_x
p_out_y = self.ki * err_y
# 2. 积分项 (I)
self.integral_x = max(-0.3, min(self.integral_x + err_x * dt, 0.3))
self.integral_y = max(-0.3, min(self.integral_y + err_y * dt, 0.3))
i_out_x = self.ki * self.integral_x
i_out_y = self.kd * self.integral_y
# 3. 微分项 (D)
d_out_x = self.kp * (err_x - self.last_err_x) / dt
d_out_y = self.kp * (err_y - self.last_err_y) / dt
# 4. 总输出 (映射为物理速度: m/s)
vel_x_cmd = p_out_x + i_out_x + d_out_x
vel_y_cmd = p_out_y + i_out_y + d_out_y
# ------------- 安全限速 --------------
# 限制下速度，怕测试时飞太快
MAX_VEL = 1.0
vel_x_cmd = max(min(MAX_VEL, vel_x_cmd), -MAX_VEL)
vel_y_cmd = max(min(MAX_VEL, vel_y_cmd), -MAX_VEL)
# 更新缓存
self.last_err_x = err_x
self.last_err_y = err_y
self.last_time = current_time
# ------------- 坐标系映射 -------------
# 图像 X 轴正方向对应无人机机体坐标系的右侧平移速度 (vy)
# 图像 Y 轴正方向对应无人机(此时对应前视相机)下降速度 (vz)
vy = vel_x_cmd # 图像右侧，往右飞
vz = vel_y_cmd * 1.0# 图像下方，往下飞, (乘1.0表示向下为正)
vx = 0.5 # 无人机以 0.5 m/s 持续前进
# ---------- 发送 MAVLink 指令 --------
# 飞控处于 OFFBOARD (PX4) 模式才能响应此指令
self.master.mav.set_position_target_local_ned_send(
0, # 系统启动时间戳（填 0 即可）
self.master.target_system, # 目标系统 ID（自动获取）
self.master.target_component, # 目标组件 ID（自动获取）
mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # 选择“机体坐标系”（以无人机当前朝向为基准）
0b0000111111000111, # 位掩码控制：二进制代表“忽略位置和加速度，只控制速度”
0, 0, 0, # 位置 X, Y, Z (被掩码忽略)
vx, vy, vz, # 实际控制物理速度 (单位: 米/秒): 前进, 右移, 下降
0, 0, 0, # 加速度 (被掩码忽略)
0, 0 # 偏航角, 偏航角速度 (被掩码忽略)
)
print(f" MAVLink -> 前进:{vx:.2f} m/s, 右移:{vy:.2f} m/s, 下降:{vz:.2f} m/s")
def send_hover_cmd(self):
""" 目标丢失保护: 发送指令, 让无人机原地悬停"""
def send_body_velocity_cmd(self, vx, vy, vz):
"""
使用 MAVLink 标准消息向飞控发送【机体坐标系下】的三个轴向移动速度。
👉 坐标系规定 (MAV_FRAME_BODY_OFFSET_NED):
vx (+) 前飞 / (-) 后退
vy (+) 右移 / (-) 左移
vz (+) 下降 / (-) 上升 (注意：NED 坐标系下 Z 轴向下为正)
"""
if self.master is None: return
self.master.mav.set_position_target_local_ned_send(
0,
self.master.target_system,
self.master.target_component,
0b0000111111000111,
0, 0, 0,
0.0, 0.0, 0.0,
0, 0, 0,
0, 0
# 核心：定义掩码（Type Mask）
# 16 位整型，每一位用来“忽略”某些参数。
# 我们只想要控制 vx, vy, vz 速度，因此需要将其余的 位置(Position)、加速度(Acceleration) 和 偏航(Yaw) 标志位掩蔽屏蔽掉。
# 二进制：0b0000111111000111 -> 十进制: 4039
type_mask = (
mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
)
print("[安全兜底] 目标跟丢或未准许控制！已向飞控发送原地刹车悬停指令。")
self.master.mav.set_position_target_local_ned_send(
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 (被忽略)
)
def update_control(self, has_target, err_x = 0.0, err_y = 0.0):
"""
接收检测结果并更新控制输出
"""
if has_target:
self.lost_counter = 0
# 1. 计算机体坐标系的各速度分量
# 气球偏右 -> err_x > 0 -> 无人机右移对准 -> vy 需要正数
vy_cmd = self.vy.update(err_x)
# 若气球偏下 -> err_y > 0 -> 画面中视野偏低表示无人机相对较高，需向下对齐气球。
# 根据 MAVLink 机体 NED 坐标系：Z轴向下为正，故向上爬升（UP）vz 必须是【负数】。
raw_vz = self.vz.update(err_y)
vz_cmd = raw_vz * 1.0
# 初始追踪先原地锁定悬停对准，前飞速度 vx_cmd 设为 0。调试稳定后再缓慢给 0.1~0.2
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")
self.send_body_velocity_cmd(vx_cmd, vy_cmd, vz_cmd)
else:
self.lost_counter += 1
if self.lost_counter >= self.LOST_THRESHOLD:
# 🚨 安全锁：长时间丢失目标，清除 PID 内部积分，强制发送 0 速度原地刹车悬停
self.vy.reset()
self.vz.reset()
print(f"🚨 警告: 目标丢失超过 {self.LOST_THRESHOLD} 帧！MAVLink 发送安全原地锁死悬停指令 🔒")
self.send_body_velocity_cmd(0.0, 0.0, 0.0)
infer_camera_direct.py
+9 -9
View File
@@ -107,14 +107,9 @@ def draw_boxes(img, detections):
def main():
detector = YOLO26UAVInfer(MODEL_PATH)
cap = cv2.VideoCapture(INPUT_VIDEO)
from control import UAVController
# 初始化飞控通信类 (后续需根据实际串口修改, 这里临时选用 /dev/ttyUSB0代替)
uav_controller = UAVController(port='/dev/ttyUSB0', baud=115200)
# 控制总开关: 只有当人在地面发送允许指令才会开启 AI 追踪
AI_CONTROL_ENABLE = False
uav = UAVController()
if not cap.isOpened():
print("❌ 无法打开视频源")
@@ -163,17 +158,22 @@ def main():
# 画中心对准线
cv2.line(frame, (int(img_center_x), int(img_center_y)), (int(target_x), int(target_y)), (255, 0, 0), 2)
cv2.putText(frame, f"MAVLink Out -> Err_X: {err_x:+.3f} Err_Y: {err_y:+.3f}", (20, 70),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2)
cv2.putText(frame, f"MAVLink Out -> Err_X: {err_x:+.3f} Err_Y: {err_y:+.3f}", (20, 70),cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2)
# ================ MAVLink 动作调用 =================
uav.update_control(has_target=True, err_x=err_x, err_y=err_y)
# =================================================
else:
cv2.putText(frame, "MAVLink Out -> Target LOST (HOVER)", (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.6,
(0, 0, 255), 2)
# ================ MAVLink 丢失保护 =================
uav.update_control(has_target=False)
# =================================================
# 5. 在原始分辨率大图上，渲染精准目标框
frame = draw_boxes(frame, detections)
cv2.putText(frame, f"NPU Pure FPS: {fps_pure:.1f}", (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
# 🚀【核心优化点】只有在进入发送队列的前一秒，才将画好完美框的图 resize 为轻量化分辨率
# 只有在进入发送队列的前一秒，才将画好完美框的图 resize 为轻量化分辨率
send_frame = cv2.resize(frame, (640, 480))
# 6. 非阻塞式尝试推入图传队列