diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py index 7150324..c0c9938 100644 --- a/drone_controller/mission_manager.py +++ b/drone_controller/mission_manager.py @@ -118,6 +118,25 @@ class MissionManager: OFFBOARD_TIMEOUT = 5.0 ARM_TIMEOUT = 5.0 + # ================================================================ + # 状态切换日志(property 拦截所有 self.state = X 赋值) + # ================================================================ + + @property + def state(self): + return self._state + + @state.setter + def state(self, new_state): + old_state = getattr(self, '_state', None) + if old_state is not new_state: + logger.info("🔄 [状态切换] %s → %s", + old_state.name if old_state is not None else "∅", + new_state.name) + self._state = new_state + + # ================================================================ + def __init__(self, proxy, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0, return_to_home=True, target_lost_timeout=None, @@ -139,6 +158,9 @@ class MissionManager: # ---- 依赖注入 ---- self.proxy = proxy + # ---- 状态初始值(property 用 _state,须在任何 self.state = 前初始化) ---- + self._state = None + # ---- 任务参数 ---- # 航点列表(经纬度协议 v2:[{lat, lon, alt, yaw, command}, ...]) self.waypoints = list(waypoints) if waypoints else [] diff --git a/run_mission.py b/run_mission.py index a495b2d..13f9921 100644 --- a/run_mission.py +++ b/run_mission.py @@ -46,8 +46,6 @@ import threading import time import socket import cv2 -import queue -import csv from drone_controller.mission_manager import ( MissionManager, @@ -61,6 +59,8 @@ from drone_controller.router_proxy import RouterProxy # run_mission.py 在外部协调其控制线程的启停。 from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer # 注:coord.latlon_to_ned 不再需要 — 航点直接存储原始经纬度 +# ---- 飞行日志 ---- +from utils.flight_logger import FlightLogger # ---- 光流模块 ---- from drone_controller.optical_flow_controller import OpticalFlowObstacleDetector @@ -97,16 +97,7 @@ class MissionOrchestrator: # csv飞行日志参数 # ================================================================ log_cfg = cfg.get("log", {}) - self.flight_log_enabled = log_cfg.get("enable_flight_log", True) - self.flight_log_interval = log_cfg.get("flight_log_interval", 20) # 20帧 ≈ 1秒 - self.log_dir = log_cfg.get("log_dir", "./log") - self._log_queue = queue.Queue(maxsize=500) # 最多缓存500条,防止内存泄漏 - self._log_thread = None - self._log_running = False - self._log_frame_counter = 0 - self._log_file_path = None - self._log_worker_started = False - + self.flight_logger = FlightLogger(log_cfg) self._latest_detections = None # ================================================================ @@ -168,141 +159,6 @@ class MissionOrchestrator: self.logger.info("MissionOrchestrator 初始化完成") # ================================================================ - # 📝 飞行日志相关 - # ================================================================ - def _start_flight_logger(self): - """启动异步日志后台线程""" - if not self.flight_log_enabled: - self.logger.info("📝 飞行日志已禁用(config 中 enable_flight_log=false)") - return - - if self._log_worker_started: - return - - # 确保日志目录存在 - os.makedirs(self.log_dir, exist_ok=True) - - # 生成日志文件名(按时间命名) - timestamp = time.strftime("%Y%m%d_%H%M%S") - self._log_file_path = os.path.join(self.log_dir, f"flight_{timestamp}.csv") - - # 写入 CSV 表头(如果文件不存在) - with open(self._log_file_path, 'w', newline='') as f: - writer = csv.writer(f) - writer.writerow([ - 'timestamp_unix', - 'alt_rel_m', - 'ground_speed_mps', - 'vx', 'vy', 'vz', - 'battery_percent', - 'state', - 'wp_index', - 'detect_count', - 'tracking_active' - ]) - - self._log_running = True - self._log_thread = threading.Thread( - target=self._flight_log_worker, - daemon=True, - name="flight-log-writer" - ) - self._log_thread.start() - self._log_worker_started = True - self.logger.info(f"📝 异步飞行日志已启动 -> {self._log_file_path} (降采样: 每{self.flight_log_interval}帧写1次)") - - def _flight_log_worker(self): - """后台线程:从队列取数据,批量写入 CSV""" - buffer = [] - BATCH_SIZE = 10 # 攒够 10 条一次性写入,减少 I/O - - while self._log_running: - try: - # 阻塞等待 1 秒,有数据则取,超时则检查是否退出 - item = self._log_queue.get(timeout=1.0) - if item is None: # 停止信号 - break - buffer.append(item) - - # 攒够一批或队列为空且需要退出时写入 - if len(buffer) >= BATCH_SIZE: - self._flush_log_buffer(buffer) - buffer = [] - - except queue.Empty: - # 超时无数据,检查是否还有残留 - if not self._log_running and not self._log_queue.empty(): - # 收尾:把剩余的数据写完 - while not self._log_queue.empty(): - try: - buffer.append(self._log_queue.get_nowait()) - except queue.Empty: - break - if buffer: - self._flush_log_buffer(buffer) - continue - except Exception as e: - self.logger.error(f"飞行日志线程异常: {e}") - time.sleep(0.1) - - # 最终收尾:清空缓冲区 - if buffer: - self._flush_log_buffer(buffer) - self.logger.info("📝 飞行日志线程已停止") - - def _flush_log_buffer(self, buffer): - """将缓冲区数据写入 CSV 文件(由后台线程调用)""" - if not buffer or not self._log_file_path: - return - try: - with open(self._log_file_path, 'a', newline='') as f: - writer = csv.writer(f) - writer.writerows(buffer) - except Exception as e: - self.logger.error(f"写入飞行日志失败: {e}") - - # run_mission.py — MissionOrchestrator 类中新增 - - def _collect_and_enqueue_log(self): - """采集当前飞行状态,推入日志队列(主线程执行,极轻量)""" - try: - state = self.proxy.get_latest_state() - if state is None: - return - - drone = state.get("drone", {}) - # 当前状态机的状态 - current_state = self.mission.state.name if self.mission else "UNKNOWN" - wp_idx = self.mission.current_wp_index if self.mission else 0 - # 追踪线程是否激活(来自控制权协调标志) - tracking_active = 1 if self._control_active else 0 - - # 组装日志行(顺序要与 CSV 表头一致) - row = ( - time.time(), # timestamp_unix - drone.get("alt_rel", 0.0), # alt_rel_m - drone.get("ground_speed", 0.0), # ground_speed_mps - drone.get("vx", 0.0), # vx - drone.get("vy", 0.0), # vy - drone.get("vz", 0.0), # vz - drone.get("battery", 0.0), # battery_percent - current_state, # state (string) - wp_idx, # wp_index - len(self._latest_detections) if self._latest_detections is not None else 0, # detect_count - tracking_active # tracking_active - ) - - # 非阻塞推入队列(如果队列满了,丢弃旧数据,防止内存爆炸) - try: - self._log_queue.put_nowait(row) - except queue.Full: - # 队列满了说明写入线程跟不上,丢掉这条数据,记录警告(但不要经常触发) - pass - - except Exception as e: - # 日志采集失败不影响主循环 - pass - # ================================================================ # 📥 QGC 航点回调(由 RouterProxy 的 SUB 线程触发) # ================================================================ @@ -506,8 +362,7 @@ class MissionOrchestrator: if cmd_cfg.get("enabled", True): self._start_tcp_command_server() - # ---- 启动异步飞行日志 ---- - self._start_flight_logger() + # ---- 飞行日志由 FlightLogger 在构造时自动启动 ---- self.running = True self.logger.info("🚀 所有子系统就绪,主循环启动") @@ -587,10 +442,14 @@ class MissionOrchestrator: self.streamer.push_to_stream(frame) # ---- 采集飞行日志 ---- - if self.flight_log_enabled and self._log_worker_started: - self._log_frame_counter += 1 - if self._log_frame_counter % self.flight_log_interval == 0: - self._collect_and_enqueue_log() + if self.flight_logger.enabled and self.flight_logger.should_collect(): + self.flight_logger.collect( + state=self.proxy.get_latest_state(), + mission_state_name=self.mission.state.name, + wp_index=self.mission.current_wp_index, + detect_count=len(self._latest_detections) if self._latest_detections is not None else 0, + tracking_active=1 if self._control_active else 0, + ) # ---- 频率控制 ---- elapsed = time.time() - loop_start @@ -751,17 +610,7 @@ class MissionOrchestrator: self._stop_control_thread() # ---- 停止飞行日志线程 ---- - if self._log_running: - self._log_running = False - # 给队列发送停止信号 - try: - self._log_queue.put_nowait(None) - except queue.Full: - pass - - if self._log_thread and self._log_thread.is_alive(): - self._log_thread.join(timeout=2.0) - self.logger.info("✔ 飞行日志线程已停止") + self.flight_logger.stop() # 2. 关闭共享 ZMQ 代理(MissionManager + UAVControlLoop 共用) # 注意:UAVControlLoop 不拥有 proxy 生命周期(_owns_proxy=False), diff --git a/utils/flight_logger.py b/utils/flight_logger.py new file mode 100644 index 0000000..c434737 --- /dev/null +++ b/utils/flight_logger.py @@ -0,0 +1,216 @@ +""" +flight_logger.py — 异步飞行日志记录器 + +将飞行状态数据异步写入 CSV 文件,不影响主循环性能。 + +用法: + logger = FlightLogger(log_config) + + # 在主循环中(按间隔调用) + logger.collect(state, mission_state_name, wp_index, detect_count, tracking_active) + + # 退出时 + logger.stop() +""" + +import os +import time +import csv +import queue +import threading +import logging + +logger = logging.getLogger(__name__) + + +class FlightLogger: + """ + 异步飞行日志记录器 + + 后台线程负责批量写入 CSV,主线程仅做轻量级入队操作。 + """ + + def __init__(self, log_config=None): + """ + Args: + log_config: 日志配置字典,支持字段: + enable_flight_log (bool): 是否启用(默认 True) + flight_log_interval (int): 写入间隔(帧数,默认 20) + log_dir (str): 日志目录(默认 "./log") + """ + if log_config is None: + log_config = {} + + self.enabled = log_config.get("enable_flight_log", True) + self.log_interval = log_config.get("flight_log_interval", 20) + self.log_dir = log_config.get("log_dir", "./log") + + self._queue = queue.Queue(maxsize=500) + self._thread = None + self._running = False + self._file_path = None + self._started = False + self._frame_counter = 0 + + if self.enabled: + self._start() + + # ================================================================ + # 内部方法 + # ================================================================ + + def _start(self): + """启动后台日志线程""" + os.makedirs(self.log_dir, exist_ok=True) + + timestamp = time.strftime("%Y%m%d_%H%M%S") + self._file_path = os.path.join(self.log_dir, f"flight_{timestamp}.csv") + + # 写入 CSV 表头 + with open(self._file_path, 'w', newline='') as f: + writer = csv.writer(f) + writer.writerow([ + 'timestamp_unix', + 'alt_rel_m', + 'ground_speed_mps', + 'vx', 'vy', 'vz', + 'battery_percent', + 'state', + 'wp_index', + 'detect_count', + 'tracking_active' + ]) + + self._running = True + self._thread = threading.Thread( + target=self._worker, + daemon=True, + name="flight-log-writer" + ) + self._thread.start() + self._started = True + logger.info("📝 异步飞行日志已启动 -> %s (降采样: 每%d帧写1次)", + self._file_path, self.log_interval) + + def _worker(self): + """后台线程:从队列取数据,批量写入 CSV""" + buffer = [] + BATCH_SIZE = 10 # 攒够 10 条一次性写入,减少 I/O + + while self._running: + try: + item = self._queue.get(timeout=1.0) + if item is None: # 停止信号 + break + buffer.append(item) + + if len(buffer) >= BATCH_SIZE: + self._flush(buffer) + buffer = [] + + except queue.Empty: + # 超时无数据,检查是否还有残留 + if not self._running and not self._queue.empty(): + while not self._queue.empty(): + try: + buffer.append(self._queue.get_nowait()) + except queue.Empty: + break + if buffer: + self._flush(buffer) + continue + except Exception as e: + logger.error("飞行日志线程异常: %s", e) + time.sleep(0.1) + + # 最终收尾:清空缓冲区 + if buffer: + self._flush(buffer) + logger.info("📝 飞行日志线程已停止") + + def _flush(self, buffer): + """将缓冲区数据写入 CSV 文件""" + if not buffer or not self._file_path: + return + try: + with open(self._file_path, 'a', newline='') as f: + writer = csv.writer(f) + writer.writerows(buffer) + except Exception as e: + logger.error("写入飞行日志失败: %s", e) + + # ================================================================ + # 公开接口 + # ================================================================ + + def should_collect(self): + """ + 判断当前帧是否需要采集(基于帧数间隔降采样) + + Returns: + bool: 当前帧是否应采集 + + 用法: + if flight_logger.should_collect(): + flight_logger.collect(...) + """ + if not self._started: + return False + self._frame_counter += 1 + return self._frame_counter % self.log_interval == 0 + + def collect(self, state, mission_state_name, wp_index, + detect_count, tracking_active): + """ + 采集当前飞行状态,推入写入队列(主线程调用,极轻量) + + Args: + state: proxy.get_latest_state() 的返回值 + mission_state_name: 状态机当前状态名 (str),如 "NAVIGATING" + wp_index: 当前航点索引 + detect_count: 当前帧检测到的目标数 + tracking_active: 追踪线程是否激活(1 激活 / 0 未激活) + """ + if not self._started or state is None: + return + + try: + drone = state.get("drone", {}) + row = ( + time.time(), # timestamp_unix + drone.get("alt_rel", 0.0), # alt_rel_m + drone.get("ground_speed", 0.0), # ground_speed_mps + drone.get("vx", 0.0), # vx + drone.get("vy", 0.0), # vy + drone.get("vz", 0.0), # vz + drone.get("battery", 0.0), # battery_percent + mission_state_name, # state + wp_index, # wp_index + detect_count, # detect_count + tracking_active, # tracking_active + ) + + # 非阻塞入队,满则丢弃 + try: + self._queue.put_nowait(row) + except queue.Full: + pass + + except Exception: + pass # 日志采集失败不影响主循环 + + def stop(self): + """停止后台日志线程""" + if not self._started: + return + + self._running = False + # 发送停止信号 + try: + self._queue.put_nowait(None) + except queue.Full: + pass + + if self._thread and self._thread.is_alive(): + self._thread.join(timeout=2.0) + logger.info("✔ 飞行日志线程已停止")