diff --git a/drone_controller/base_control.py b/drone_controller/base_control.py new file mode 100644 index 0000000..51c2a1d --- /dev/null +++ b/drone_controller/base_control.py @@ -0,0 +1,178 @@ +import time +import math +import threading +from pymavlink import mavutil + + +class DroneController: + def __init__(self, connection_string='/dev/ttyUSB0', baud=115200): + self.connection_string = connection_string + self.baud = baud + self.master = None + self.is_connected = False + + # 飞控实时状态变量（多线程共享） + self.current_mode = None + self.is_armed = False + self.local_x = 0.0 + self.local_y = 0.0 + self.local_z = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + self.yaw = 0.0 + + # 线程控制开关 + self.running = False + self.telemetry_thread = None + + def connect(self): + """初始化 MAVLink 串口连接，并等待首个心跳包""" + try: + print(f"正在尝试连接飞控 ({self.connection_string})...") + self.master = mavutil.mavlink_connection(self.connection_string, baud=self.baud) + self.master.wait_heartbeat(timeout=5) + self.is_connected = True + print("飞控连接成功，已接收到首个心跳包！") + + # 开启后台线程，负责高频监听状态 + self.running = True + self.telemetry_thread = threading.Thread(target=self._telemetry_loop, daemon=True) + self.telemetry_thread.start() + return True + except Exception as e: + print(f"连接飞控失败: {e}") + self.is_connected = False + return False + + def _telemetry_loop(self): + """后台线程：高频高吞吐量接收飞控状态反馈，刷新状态变量""" + while self.running and self.is_connected: + try: + # 阻塞式接收，超时 0.1 秒 + msg = self.master.recv_match(blocking=True, timeout=0.1) + if not msg: + continue + + msg_type = msg.get_type() + + # 1. 解析心跳包获取模式与解锁状态 + if msg_type == 'HEARTBEAT': + mode_id = msg.custom_mode + self.is_armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + + # 尝试反向查找模式名称 + if self.master: + # 寻找数值对应的模式字符串 + for name, num in self.master.mode_mapping().items(): + if num == mode_id: + self.current_mode = name + break + + # 2. 解析局部 NED 位置坐标 + elif msg_type == 'LOCAL_POSITION_NED': + self.local_x = msg.x # 北 (North) + self.local_y = msg.y # 东 (East) + self.local_z = msg.z # 地 (Down) + + # 3. 解析姿态角 (弧度转角度) + elif msg_type == 'ATTITUDE': + self.roll = math.degrees(msg.roll) + self.pitch = math.degrees(msg.pitch) + self.yaw = math.degrees(msg.yaw) + + except Exception as e: + print(f"\n遥测线程捕获到异常: {e}") + time.sleep(0.1) + + def set_mode(self, mode_name): + """切换飞控模式 (例如 GUIDED 或 OFFBOARD)""" + if not self.is_connected: return False + print(f"正在尝试切换到模式: {mode_name}...") + + # 将模式名称转换为飞控可识别的内部数值 + if mode_name not in self.master.mode_mapping(): + print(f"错误,飞控不支持{mode_name}模式") + return False + + mode_id = self.master.mode_mapping()[mode_name] + self.master.set_mode(mode_id) + print(f"已发送模式切换指令 -> {mode_name}") + return True + + def arm(self): + """发送解锁指令""" + if not self.is_connected: return False + print("正在发送解锁指令...") + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 1, 0, 0, 0, 0, 0, 0 + ) + + # 等待并捕获飞控的 ACK 响应，超时时间设为 3 秒 + ack = self.master.recv_match(type='COMMAND_ACK', blocking=True, timeout=3.0) + if ack and ack.command == mavutil.mavlink.MAV_COMMAND_ACK: + if ack.result == 0: + print(" ✅ [飞控通信] 飞控解锁成功 (Armed Success)！") + else: + print(f" ❌ [飞控通信] 飞控拒绝了解锁请求，拒绝代码 (Result): {ack.result}") + else: + print(" ⚠️ [飞控通信] 飞控未响应解锁确认，请检查飞控状态或数传指示灯。") + return True + + def disarm(self): + """发送上锁指令""" + if not self.is_connected: return False + print("正在发送强制上锁指令...") + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 0, 0, 0, 0, 0, 0, 0 + ) + return True + + def send_body_velocity(self, vx, vy, vz, yaw_rate=0.0): + """ + 核心控制接口：发送机体坐标系（Body Frame）下的速度指令 + 注意：在下发控制前，如果安全员人为切出了自主模式，则该函数拒绝发送，确保人肉兜底。 + yaw_rate:单位（弧度/秒，rad/s）,后续使用得转换为角度 + """ + if not self.is_connected: return + + if self.current_mode not in ['GUIDED', 'OFFBOARD']: + return + + # 坐标系掩码定义：使用机体坐标系 (MAV_FRAME_BODY_NED) + # 掩码屏蔽：只保留 vx, vy, vz 和偏航速率 yaw_rate + coordinate_frame = mavutil.mavlink.MAV_FRAME_BODY_NED + type_mask = 0b0000111111000111 # 仅开放速度和偏航率 + + self.master.mav.set_position_target_local_ned_send( + 0, # 系统时间戳（可传0） + self.master.target_system, + self.master.target_component, + coordinate_frame, + type_mask, + 0, 0, 0, # 位置 (屏蔽) + vx, vy, vz, # 速度 (机体前后、机体左右、机体上下) + 0, 0, 0, # 加速度 (屏蔽) + 0, yaw_rate # 偏航，偏航率 + ) + + def print_status(self): + """利用 \r 实现终端单行丝滑刷新，动态打印 PID 命令与真实状态对比""" + print( + f"\r[Status] Armed: {self.is_armed} | " + f"Pos(N,E,D): ({self.local_x:+.2f}, {self.local_y:+.2f}, {self.local_z:+.2f}) | " + f"Yaw: {self.yaw:+.1f}°\033[K", + end="", flush=True + ) + + def close(self): + """释放线程，关闭串口连接""" + self.running = False + if self.telemetry_thread: + self.telemetry_thread.join(timeout=1) + print("\n底层控制接口已安全断开。") \ No newline at end of file diff --git a/drone_controller/pid_counter.py b/drone_controller/pid_counter.py new file mode 100644 index 0000000..05807a4 --- /dev/null +++ b/drone_controller/pid_counter.py @@ -0,0 +1,58 @@ +import time + +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): + """ + 根据当前误差计算 PID 输出 + """ + current_time = time.time() + dt = current_time - self.last_time + if dt <= 0: + dt = 0.033 # 默认 30Hz 的更新步长 + + # 1. 比例项 + p_out = self.kp * error + + # 2. 积分项（增加累加计算，并对积分做出局部限幅防止初段积分饱和） + self.integral += error * dt + i_out = self.ki * self.integral + # 将积分上限限制在总输出幅值的 20% + i_out = max(min(i_out, self.max_out * 0.2), self.min_out * 0.2) + + # 3. 微分项 + d_out = self.kd * (error - self.last_err) / dt + + # 4. 总输出合成 + output = p_out + i_out + d_out + + # 5. 总输出限幅 + 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 + self.last_time = time.time() \ No newline at end of file diff --git a/infer_camera_direct.py b/infer_camera_direct.py index 98463ca..0312f1d 100644 --- a/infer_camera_direct.py +++ b/infer_camera_direct.py @@ -7,7 +7,13 @@ import queue import socket from ais_bench.infer.interface import InferSession +# ================== 导入控制模块 ===================== +from drone_controller.base_control import DroneController +from drone_controller.pid_counter import PID +# =================================================== + # ==================== 配置区域 ====================== +# -------------- 模型部分基本配置 -------------- MODEL_PATH = "./om/yolo26n-balloon.om" # YOLO26 模型路径 INPUT_VIDEO = 0 # 摄像头编号 CONF_THRESHOLD = 0.25 # 置信度阈值 @@ -16,6 +22,10 @@ CONF_THRESHOLD = 0.25 # 置信度阈值 GROUND_STATION_IP = "192.168.31.239" # 地面站的局域网 IP UDP_PORT = 9999 JPEG_QUALITY = 75 # JPEG 压缩质量 (0-100) + +# -------------- 串口数据信息配置 -------------- +connection_string = '/dev/ttyUSB0' +baud_rate = 57600 # ================================================== # 限制队列大小，发不完就直接丢帧，保证飞控算法实时性 @@ -107,9 +117,16 @@ def draw_boxes(img, detections): def main(): detector = YOLO26UAVInfer(MODEL_PATH) cap = cv2.VideoCapture(INPUT_VIDEO) - from control import UAVController - uav = UAVController() + uav = DroneController(connection_string=connection_string, baud=baud_rate) + uav.connect() + + if not uav.is_armed & uav.current_mode not in ['GUIDED', 'OFFBOARD']: + uav.arm() + + # 初始化Y轴 (左右方向) 和Z轴 (上下方向) 的 PID 调节器 + pid_y = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.8, min_out=-0.8) + pid_z = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.4, min_out=-0.4) if not cap.isOpened(): print("❌ 无法打开视频源") @@ -156,17 +173,35 @@ def main(): err_x = (target_x - img_center_x) / img_center_x err_y = (target_y - img_center_y) / img_center_y + # 数学层：将图像误差转化为物理速度需求值 + # 注意：图像X方向偏右（err_x > 0），飞机应当向右飞（机体 Vy 为正） + vy_cmd = pid_y.update(err_x) + # 注意：图像Y方向偏下（err_y > 0），飞机应该向下运动对齐（机体 Vz 在NED系下向下为正） + vz_cmd = pid_z.update(err_y) + + # 保持前后方向 (Vx) 的固定推进距离或由其他策略决定，此处设为0（仅上下左右平移对准） + vx_cmd = 0.0 + # 画中心对准线 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) # ================ MAVLink 动作调用 ================= - uav.update_control(has_target=True, err_x=err_x, err_y=err_y) + if getattr(uav, 'current_mode', None) in ['GUIDED', 'OFFBOARD']: + uav.send_body_velocity(vx=vx_cmd, vy=vy_cmd, vz=vz_cmd, yaw_rate=0.0) + else: + cv2.putText(frame, f"MANUAL MODE OVERRIDE ({uav.current_mode})", (20, 100), cv2.FONT_HERSHEY_SIMPLEX, + 0.6, (0, 0, 255), 2) # ================================================= 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) + # 清除 PID 历史积分状态，防止下次捕捉目标时突跳炸机 + if hasattr(pid_y, 'reset'):pid_y.reset() + if hasattr(pid_z, 'reset'):pid_z.reset() + + if getattr(uav, 'current_mode', None) in ['GUIDED', 'OFFBOARD']: + uav.send_body_velocity(vx=0.0, vy=0.0, vz=0.0, yaw_rate=0.0) # ================================================= # 5. 在原始分辨率大图上，渲染精准目标框