diff --git a/transform_app.py b/Optoelectronics/transform_app.py similarity index 99% rename from transform_app.py rename to Optoelectronics/transform_app.py index 49175c0..6d74c34 100644 --- a/transform_app.py +++ b/Optoelectronics/transform_app.py @@ -239,7 +239,7 @@ def calculate_target_gps(ptz_lat, ptz_lon, ptz_alt, pan, tilt, distance): def main(): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # 实际测试时请修改为光电外设的正确 IP - sock.connect(('192.168.1.64', 39020)) + sock.connect(('10.10.61.64', 39020)) # 【修复 Bug 2】：传入文档推测的默认密码 'Abc.12345' token = login_to_ipc(sock, 'admin', 'Abc.12345') diff --git a/config.json b/config.json index f369837..3b19808 100644 --- a/config.json +++ b/config.json @@ -17,6 +17,7 @@ "sub_endpoint": "tcp://127.0.0.1:5556", "takeoff_alt": 5.0, "control_hz": 20, + "cruise_speed":1.0, "target_tracker": { "max_lost_frames": 8, "max_asscociation_dist": 200.0, @@ -39,11 +40,22 @@ "return_to_home": false, "target_lost_timeout": null }, + "optical_flow":{ + "grid_step":25, + "reset_threshold":30 + }, "log": { "enable_flight_log": true, "flight_log_interval": 20, "log_dir": "./log" }, + "pid_x":{ + "kp": 1.2, + "ki": 0.1, + "kd": 0.05, + "max_out": 0.8, + "min_out": -0.8 + }, "pid_y": { "kp": 1.0, "ki": 0.02, @@ -57,5 +69,12 @@ "kd": 0.3, "max_out": 0.4, "min_out": -0.4 + }, + "pid_yaw":{ + "kp":0.8, + "ki":0.05, + "kd":0.0, + "max_out":1.0, + "min_out":0.0 } } \ No newline at end of file diff --git a/drone_controller/optical_flow_controller.py b/drone_controller/optical_flow_controller.py new file mode 100644 index 0000000..4000962 --- /dev/null +++ b/drone_controller/optical_flow_controller.py @@ -0,0 +1,482 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +光流法障碍检测 - 网格点追踪版（复位点不参与统计） +特性： +1. 全图均匀网格点，覆盖无盲区 +2. 点漂移后硬复位（当前帧和上一帧均置为初始位置） +3. 复位点在该帧不参与任何统计（散度、平均光流等） +4. 分区（左/中/右）风险统计，仅基于有效运动点 +5. 与 PID 和 MAVLink 无缝集成 +""" + +import cv2 +import numpy as np +import time +from collections import deque +from typing import Optional, Dict, Any + +# ==================== PID 部分 ===================== +from dataclasses import dataclass + + +@dataclass +class PIDConfig: + """PID 参数配置""" + kp: float + ki: float + kd: float + max_out: float + min_out: float + dt_default: float = 0.033 + alpha: float = 0.2 # 微分滤波系数 (0 < alpha <= 1) + + +class ImprovedPID: + def __init__(self, config: PIDConfig): + # 参数校验 + if config.max_out <= config.min_out: + raise ValueError("max_out must be greater than min_out") + if not (0 < config.alpha <= 1): + raise ValueError("alpha must be in (0, 1]") + + self.cfg = config + + self.last_err: float = 0.0 + self.last_fb: float = 0.0 + self.integral: float = 0.0 + self.last_d: float = 0.0 + self.last_time: float = time.perf_counter() + self.first_run: bool = True # 标记首次调用 + + def update(self, error: float, feedback: Optional[float] = None, dt: Optional[float] = None) -> float: + current_time = time.perf_counter() + + # --- 首次调用处理：初始化状态，避免微分冲击 --- + if self.first_run: + self.first_run = False + self.last_err = error + if feedback is not None: + self.last_fb = feedback + self.last_time = current_time + # 首次仅返回比例项，微分和积分暂不参与（或也可返回0，视应用而定） + # 通常直接返回 P 项能让系统平滑起步 + return self.cfg.kp * error + + # 1. 计算或使用传入的 dt + if dt is None: + dt = current_time - self.last_time + if dt <= 0: + dt = self.cfg.dt_default + + # 2. 比例项 (P) + p_out = self.cfg.kp * error + + # 3. 微分项 (D) —— 微分先行 + 低通滤波 + if feedback is not None: + # 对反馈微分，设定值变化不引入冲击 + d_raw = (self.last_fb - feedback) / dt + self.last_fb = feedback + else: + d_raw = (error - self.last_err) / dt + + self.last_d = self.cfg.alpha * d_raw + (1.0 - self.cfg.alpha) * self.last_d + d_out = self.cfg.kd * self.last_d + + # 4. 积分项 (I) —— 先计算潜在值 + potential_integral = self.integral + error * dt + i_out = self.cfg.ki * potential_integral + + # 5. 总输出与抗积分饱和（基于积分方向判断） + output = p_out + i_out + d_out + + if output > self.cfg.max_out: + output = self.cfg.max_out + # 如果积分项仍在推高输出（i_out > 0），则冻结积分 + if i_out > 0: + pass # 不更新 self.integral + else: + self.integral = potential_integral + elif output < self.cfg.min_out: + output = self.cfg.min_out + # 如果积分项仍在拉低输出（i_out < 0），则冻结积分 + if i_out < 0: + pass + else: + self.integral = potential_integral + else: + # 未饱和，正常更新积分 + self.integral = potential_integral + + # 6. 更新状态变量 + self.last_err = error + self.last_time = current_time + + return output + + def reset(self) -> None: + """重置所有状态，恢复到首次运行前的状态""" + self.last_err = 0.0 + self.last_fb = 0.0 + self.integral = 0.0 + self.last_d = 0.0 + self.last_time = time.perf_counter() + self.first_run = True + +# ================================================= + + +class OpticalFlowObstacleDetector: + """网格点光流检测器，复位点不参与统计""" + def __init__(self, grid_step: int = 25, reset_threshold: float = 30.0): + """ + Args: + grid_step: 网格点间距（像素），值越小点越多 + reset_threshold: 点偏离初始位置超过此值时复位（像素） + """ + self.grid_step = grid_step + self.reset_threshold = reset_threshold + + # 光流跟踪参数 + self.lk_params = dict( + winSize=(21, 21), + maxLevel=3, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01) + ) + + # 状态变量 + self.old_gray = None + self.p0 = None # 当前点坐标 (N,1,2) + self.grid_initial = None # 初始网格坐标 (N,2) + self.frame_count = 0 + + # 分区统计历史（用于兜底） + self.last_zone_stats = None + self.flow_history = deque(maxlen=10) + + def _generate_grid(self, img_w: int, img_h: int) -> np.ndarray: + """生成网格点，返回 (N,2) 坐标数组""" + x = np.arange(self.grid_step // 2, img_w, self.grid_step, dtype=np.float32) + y = np.arange(self.grid_step // 2, img_h, self.grid_step, dtype=np.float32) + xv, yv = np.meshgrid(x, y) + points = np.stack((xv.ravel(), yv.ravel()), axis=1) + return points + + def process_frame(self, frame): + """ + 处理单帧，返回 (is_collision, flow_data) + 更新 self.last_zone_stats（仅包含有效点） + """ + self.frame_count += 1 + if frame is None: + return False, None + + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + h, w = gray.shape + + # --- 1. 初始化第一帧 --- + if self.old_gray is None: + self.old_gray = gray + grid_pts = self._generate_grid(w, h) + self.grid_initial = grid_pts.copy() + self.p0 = grid_pts.reshape(-1, 1, 2).astype(np.float32) + self.last_zone_stats = None + return False, None + + # --- 2. 光流跟踪 --- + p1, st, err = cv2.calcOpticalFlowPyrLK( + self.old_gray, gray, self.p0, None, **self.lk_params + ) + + if p1 is None: + p1 = self.p0.copy() + st = np.ones(len(self.p0), dtype=np.uint8) + + # 将跟踪结果展开为 (N,2) + p1_flat = p1.reshape(-1, 2) + p0_flat = self.p0.reshape(-1, 2) + st_flat = st.flatten() + + # 初始化新的点数组（默认保持旧位置） + new_points = p0_flat.copy() + old_points = p0_flat.copy() # 上一帧位置，用于计算光流 + + # 成功跟踪的点，先更新位置 + success_indices = np.where(st_flat == 1)[0] + if len(success_indices) > 0: + new_points[success_indices] = p1_flat[success_indices] + old_points[success_indices] = p0_flat[success_indices] + + # --- 3. 复位逻辑与有效点掩码生成 --- + # 复位掩码：True 表示该点需要复位（跟踪失败或漂移过大） + reset_mask = np.zeros(len(new_points), dtype=bool) + + # 3.1 跟踪失败的点直接复位 + failed_indices = np.where(st_flat == 0)[0] + reset_mask[failed_indices] = True + + # 3.2 检查成功点是否漂移过大 + for idx in success_indices: + if np.linalg.norm(new_points[idx] - self.grid_initial[idx]) > self.reset_threshold: + reset_mask[idx] = True + + # 执行复位（将当前和上一帧位置均设为初始位置） + if np.any(reset_mask): + new_points[reset_mask] = self.grid_initial[reset_mask] + old_points[reset_mask] = self.grid_initial[reset_mask] + + # --- 4. 生成有效点掩码（未复位且跟踪成功） --- + # 有效点：跟踪成功且未复位 + valid_mask = np.ones(len(new_points), dtype=bool) + valid_mask[reset_mask] = False + + # --- 5. 更新状态 --- + self.p0 = new_points.reshape(-1, 1, 2).astype(np.float32) + self.old_gray = gray + + # 如果有效点太少，不进行统计（但保留状态） + if np.sum(valid_mask) < 5: + # 保留上一帧统计值，或返回空 + return False, None + + # --- 6. 计算光流向量（仅针对有效点） --- + valid_old = old_points[valid_mask] + valid_new = new_points[valid_mask] + flow_vectors = valid_new - valid_old # (M,2) + mean_mag = np.mean(np.linalg.norm(flow_vectors, axis=1)) + + # --- 7. 分区统计（仅使用有效点） --- + zone_stats = self._compute_zone_stats(valid_old, flow_vectors, w, h) + self.last_zone_stats = zone_stats + + # 整体散度（用于碰撞警告） + divergence = self._compute_divergence(valid_old, flow_vectors, w, h) + + # 碰撞判断（保留阈值） + is_collision = False + if divergence > 0.8 and mean_mag > 1.0: + is_collision = True + elif mean_mag > 3.0 and len(valid_old) > 50: + if self._check_roi_expansion(valid_old, flow_vectors): + is_collision = True + + # 历史记录 + self.flow_history.append({ + 'mean_flow': np.mean(flow_vectors, axis=0), + 'mean_mag': mean_mag, + 'divergence': divergence, + 'points': np.sum(valid_mask) + }) + + return is_collision, { + 'points': np.sum(valid_mask), + 'mean_mag': mean_mag, + 'divergence': divergence, + } + + # ---------- 辅助方法（与之前一致，但传入的点已过滤） ---------- + def _compute_divergence(self, old_points, flow_vectors, img_w, img_h): + if len(old_points) < 5: + return 0.0 + center = np.array([img_w / 2, img_h / 2]) + dir_to_center = center - old_points + dir_to_center /= (np.linalg.norm(dir_to_center, axis=1, keepdims=True) + 1e-6) + radial_flow = np.sum(flow_vectors * dir_to_center, axis=1) + return np.mean(radial_flow) + + def _compute_zone_stats(self, old_pts, flow_vecs, img_w, img_h): + left_bound = img_w / 3 + right_bound = 2 * img_w / 3 + zones = {'left': [], 'center': [], 'right': []} + for pt, vec in zip(old_pts, flow_vecs): + x = pt[0] + if x < left_bound: + zones['left'].append((pt, vec)) + elif x < right_bound: + zones['center'].append((pt, vec)) + else: + zones['right'].append((pt, vec)) + + stats = {} + for name, pts_vecs in zones.items(): + if len(pts_vecs) < 3: # 阈值降低，因为点可能较少 + stats[name] = {'mean_vec': np.zeros(2), 'divergence': 0.0, 'count': 0, 'risk': 0.0} + continue + old_pts_zone = np.array([p for p, v in pts_vecs]) + flow_zone = np.array([v for p, v in pts_vecs]) + div = self._compute_divergence(old_pts_zone, flow_zone, img_w, img_h) + stats[name] = { + 'mean_vec': np.mean(flow_zone, axis=0), + 'divergence': div, + 'count': len(pts_vecs), + 'risk': div if div > 0 else 0.0 + } + return stats + + def _check_roi_expansion(self, old_points, flow_vectors): + h, w = 480, 640 + roi_center = np.array([w / 2, h / 2]) + roi_mask = (np.abs(old_points[:, 0] - w / 2) < w / 4) & (np.abs(old_points[:, 1] - h / 2) < h / 4) + roi_points = old_points[roi_mask] + roi_flow = flow_vectors[roi_mask] + if len(roi_points) < 5: + return False + dir_vec = roi_points - roi_center + dir_unit = dir_vec / (np.linalg.norm(dir_vec, axis=1, keepdims=True) + 1e-6) + radial = np.sum(roi_flow * dir_unit, axis=1) + return np.mean(radial) > 0.5 + + def get_zone_decision(self, zone_stats=None, threshold_div=0.8): + """根据分区风险生成控制建议（返回 yaw_rate, speed_scale）""" + if zone_stats is None: + zone_stats = self.last_zone_stats + if zone_stats is None: + return 0.0, 1.0 + + left = zone_stats['left']['risk'] + center = zone_stats['center']['risk'] + right = zone_stats['right']['risk'] + max_r = max(left, center, right) + + if max_r < 0.3: + return 0.0, 1.0 + + if center >= left and center >= right and center > threshold_div: + if max(left, right) > threshold_div * 0.6: + return 0.0, 0.0 + else: + return 0.0, 0.3 + + if left > right + 0.2: + yaw = min(0.8, 0.3 + 0.5 * (left - right)) + return yaw, 0.7 + elif right > left + 0.2: + yaw = -min(0.8, 0.3 + 0.5 * (right - left)) + return yaw, 0.7 + else: + return 0.0, 0.5 + + +# ==================== 控制器（与之前相同） ==================== +class ObstacleAvoidanceController: + """避障控制器，集成网格点光流检测器、PID 和 MAVLink 发送""" + def __init__(self, + camera_id: int = 0, + frame_width: int = 640, + frame_height: int = 480, + cruise_speed: float = 2.0, + grid_step: int = 25, + reset_threshold: float = 30.0, + yaw_pid_cfg: Optional[PIDConfig] = None, + speed_pid_cfg: Optional[PIDConfig] = None): + self.cap = cv2.VideoCapture(camera_id) + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height) + self.cap.set(cv2.CAP_PROP_FPS, 30) + if not self.cap.isOpened(): + raise RuntimeError("无法打开摄像头") + + self.detector = OpticalFlowObstacleDetector(grid_step=grid_step, reset_threshold=reset_threshold) + self.cruise_speed = cruise_speed + + if yaw_pid_cfg is None: + yaw_pid_cfg = PIDConfig(kp=1.2, ki=0.1, kd=0.05, max_out=0.8, min_out=-0.8) + if speed_pid_cfg is None: + speed_pid_cfg = PIDConfig(kp=0.8, ki=0.05, kd=0.0, max_out=1.0, min_out=0.0) + + self.yaw_pid = ImprovedPID(yaw_pid_cfg) + self.speed_pid = ImprovedPID(speed_pid_cfg) + + self.current_yaw_rate = 0.0 + self.current_speed = 0.0 + self.running = False + + def update_feedback(self, yaw_rate: float, speed: float): + self.current_yaw_rate = yaw_rate + self.current_speed = speed + + def run_iteration(self, mav_proxy, enable_pid: bool = True) -> Dict[str, Any]: + ret, frame = self.cap.read() + if not ret: + return {'success': False, 'error': '摄像头读取失败'} + + is_collision, flow_data = self.detector.process_frame(frame) + yaw_setpoint, speed_scale = self.detector.get_zone_decision() + speed_setpoint = speed_scale * self.cruise_speed + + if enable_pid: + yaw_error = yaw_setpoint - self.current_yaw_rate + speed_error = speed_setpoint - self.current_speed + yaw_output = self.yaw_pid.update(yaw_error, feedback=self.current_yaw_rate) + speed_output = self.speed_pid.update(speed_error, feedback=self.current_speed) + else: + yaw_output = yaw_setpoint + speed_output = speed_setpoint + + # 调用您的 MAVLink 代理的 send_setpoint 方法 + success, ack = mav_proxy.send_setpoint( + control_mode="VELOCITY", + vx=speed_output, + vy=0.0, + vz=0.0, + yaw_rate=yaw_output + ) + + return { + 'success': success, + 'ack': ack, + 'is_collision': is_collision, + 'yaw_setpoint': yaw_setpoint, + 'speed_setpoint': speed_setpoint, + 'yaw_output': yaw_output, + 'speed_output': speed_output, + 'points': flow_data['points'] if flow_data else 0, + } + + def run_forever(self, mav_proxy, enable_pid: bool = True): + self.running = True + print("🔄 网格点光流避障（复位点不统计）已启动，按 'q' 退出") + while self.running: + result = self.run_iteration(mav_proxy, enable_pid) + if not result.get('success', False): + print("⚠️ 迭代失败:", result.get('error', '')) + continue + if result['is_collision']: + print("🚨 碰撞警告!") + + # 显示画面（方便调试） + ret, frame = self.cap.read() + if ret: + cv2.imshow('Grid Optical Flow', frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + self.stop() + break + + def stop(self): + self.running = False + self.cap.release() + cv2.destroyAllWindows() + + +# ==================== 测试入口 ==================== +if __name__ == "__main__": + # 模拟 MAVLink 代理（实际应替换为真实对象） + class MockMavProxy: + def send_setpoint(self, control_mode="VELOCITY", **kwargs): + print(f"📤 发送指令: {control_mode} {kwargs}") + return True, {"result": "ok"} + + mav = MockMavProxy() + controller = ObstacleAvoidanceController( + camera_id=0, + grid_step=25, + reset_threshold=30.0, + cruise_speed=2.0 + ) + controller.update_feedback(0.0, 0.0) # 模拟初始反馈 + + try: + controller.run_forever(mav, enable_pid=True) + except KeyboardInterrupt: + controller.stop() + print("🛑 已退出") \ No newline at end of file diff --git a/drone_controller/pid_counter.py b/drone_controller/pid_counter.py index 15c357d..2d69c08 100644 --- a/drone_controller/pid_counter.py +++ b/drone_controller/pid_counter.py @@ -15,6 +15,8 @@ class PID: self.last_err = 0.0 self.integral = 0.0 self.last_time = time.time() + # 积分冻结标志位，默认为不冻结（False） + self.integral_frozen = False def update(self, error): """ @@ -28,8 +30,11 @@ class PID: # 1. 比例项 p_out = self.kp * error - # 2. 积分项（增加累加计算，并对积分做出局部限幅防止初段积分饱和） - self.integral += error * dt + # 2. 积分项 + # 增加策略：只有在未冻结时，才进行积分累加 + if not self.integral_frozen: + 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) @@ -56,12 +61,41 @@ class PID: self.last_err = 0.0 self.integral = 0.0 self.last_time = time.time() + self.integral_frozen = False # 重置时默认恢复积分功能 def reset_integral(self): """ 仅清空积分项（I term），保留上一帧误差以维持微分（D term）连续性 - - 用于卡尔曼 coast（滑行预测）期间： - 只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。 + 用于卡尔曼 coast（滑行预测）期间： + 只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。 """ - self.integral = 0.0 \ No newline at end of file + self.integral = 0.0 + + # ================== 新增方法 ================== + + def freeze_integral(self): + """ + 冻结积分项： + 保持当前的积分值（保持之前的成果），但不再随时间累加新的误差。 + + 适用场景：执行机构饱和（如马达已满载）、或者进入特定暂态过程。 + """ + self.integral_frozen = True + + def unfreeze_integral(self, mode="maintain"): + """ + 配套解冻策略： + + :param mode: 解冻模式选择 + - "maintain": 保持冻结期间的值，直接从该点继续累加（最常用，平滑过渡）。 + - "clear": 解冻的同时清空历史积分，完全重新开始。 + - "fade": 仅保持原有积分的 50%（衰减），防止突变过冲。 + """ + self.integral_frozen = False + + if mode == "clear": + self.integral = 0.0 + elif mode == "fade": + self.integral *= 0.5 + elif mode == "maintain": + pass # 保持原样，直接继续累加 \ No newline at end of file diff --git a/infer_camera_modular.py b/infer_camera_modular.py index 284a638..b3b9925 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -440,6 +440,14 @@ class UAVControlLoop: self._log_throttle = {} # key → 上次打印时间戳 self._brake_last_logged = False # 刹车是否已输出过日志 + # ---- 光流避障决策（外部注入） ---- + self.obstacle_yaw_rate = 0.0 + self.obstacle_speed_scale = 1.0 + self.obstacle_active = False + self.cruise_speed = fc_cfg.get("cruise_speed", 1.0) # 默认 1 m/s + # ---- 光流线程锁 ---- + self._obstacle_lock = threading.Lock() + def start_uav(self): """连接 Router，执行标准起飞序列（协议 §3.3.1） @@ -647,6 +655,21 @@ class UAVControlLoop: self._tracked_result = result return tracked, result + + # ---- 光流避障决策 ---- + def set_obstacle_decision(self, yaw_rate: float, speed_scale: float): + """ + 由外部注入光流避障决策。 + 当 yaw_rate != 0 或 speed_scale < 0.95 时视为激活。 + """ + with self._obstacle_lock: + self.obstacle_yaw_rate = yaw_rate + self.obstacle_speed_scale = speed_scale + self.obstacle_active = (abs(yaw_rate) > 0.02 or speed_scale < 0.95) + if self.obstacle_active: + self.logger.debug("光流避障激活: yaw=%.2f, scale=%.2f", yaw_rate, speed_scale) + else: + self.logger.debug("光流避障已清除") # ---- 内部控制线程 ---- @@ -703,12 +726,9 @@ class UAVControlLoop: else: self._brake_last_logged = False - # ---- 记录当前速度指令（供外部日志读取） ---- - self._last_vx, self._last_vy, self._last_vz = vx, vy, vz - + # ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ---- - self.logger.debug("SETPOINT: control_mode=VELOCITY " - "vx=%.2f vy=%.2f vz=%.2f", vx, vy, vz) + # ---- 防御性检查：飞控已切到 AUTO.LAND/AUTO.RTL 则不再发送 ---- state = self.proxy.get_latest_state() if state: @@ -716,9 +736,31 @@ class UAVControlLoop: if mode in ("AUTO.LAND", "AUTO.RTL"): # 静默跳过，不发送 SETPOINT continue + # ---- 决策融合：光流避障是否覆盖 ---- + with self._obstacle_lock: + if self.obstacle_active: + # 避障模式：前向速度 × 缩放，偏航角速度由光流决策给出 + cmd_vx = self.cruise_speed * self.obstacle_speed_scale + cmd_vy = 0.0 + cmd_vz = 0.0 + cmd_yaw_rate = self.obstacle_yaw_rate + else: + # 正常追踪模式 + cmd_vx = vx # 原逻辑 vx 始终为 0（可保留） + cmd_vy = vy + cmd_vz = vz + cmd_yaw_rate = 0.0 + + # 记录当前速度指令（供外部日志读取） + self._last_vx, self._last_vy, self._last_vz = cmd_vx, cmd_vy, cmd_vz + + # ---- ⑤ 通过 ZMQ 发送 VELOCITY SETPOINT ---- + self.logger.debug("SETPOINT: control_mode=VELOCITY " + "vx=%.2f vy=%.2f vz=%.2f yaw_rate=%.2f", + cmd_vx, cmd_vy, cmd_vz, cmd_yaw_rate) ok, ack = self.proxy.send_setpoint( - vx=vx, vy=vy, vz=vz, - yaw_rate=0.0, + vx=cmd_vx, vy=cmd_vy, vz=cmd_vz, + yaw_rate=cmd_yaw_rate, control_mode="VELOCITY" ) if not ok and self.running: @@ -726,7 +768,7 @@ class UAVControlLoop: # recv 超时或格式错误由 _send_req 内部恢复 REQ socket self.logger.warning("SETPOINT ACK 失败: %s", ack) - # ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ---- + # ---- ⑥ 精确控制循环频率（20Hz → 每 50ms） ---- elapsed = time.time() - start sleep_time = self.control_interval - elapsed if sleep_time > 0: diff --git a/run_mission.py b/run_mission.py index 693381a..71d2a54 100644 --- a/run_mission.py +++ b/run_mission.py @@ -61,6 +61,8 @@ from drone_controller.router_proxy import RouterProxy # run_mission.py 在外部协调其控制线程的启停。 from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer # 注：coord.latlon_to_ned 不再需要 — 航点直接存储原始经纬度 +# ---- 光流模块 ---- +from drone_controller.optical_flow_controller import OpticalFlowObstacleDetector logger = logging.getLogger("RunMission") @@ -129,6 +131,15 @@ class MissionOrchestrator: return_to_home=missions.get("return_to_home", True), ) + # ================================================================ + # 💡 OpticalFlowObstacleDetector （光流计算） + # ================================================================ + flow_cfg = cfg.get("optical_flow", {}) + self.optical_flow = OpticalFlowObstacleDetector( + grid_step=flow_cfg.get("grid_step", 25), + reset_threshold=flow_cfg.get("reset_threshold", 30.0) + ) + # ================================================================ # 🛸 UAVControlLoop（20Hz VELOCITY 闭环追踪） # ================================================================ @@ -555,6 +566,15 @@ class MissionOrchestrator: detections, orig_shape) prev_state = current_state + # ---- 光流避障决策 ---- + if self.mission.state == MissionState.VISUAL_TRACKING: + _, _ = self.optical_flow.process_frame(frame) + yaw_setpoint, speed_scale = self.optical_flow.get_zone_decision() + self.controller.set_obstacle_decision(yaw_setpoint, speed_scale) + else: + # 非追踪状态清除避障 + self.controller.set_obstacle_decision(0.0, 1.0) + # ---- ④ 状态机更新 ---- self.mission.update(target_detected=target_detected)