# -*-coding:utf-8 -*- """ 数据滤波模块 """ import numpy as np import time import threading import queue from scipy import signal from logs.log import algo_log import sys import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from Tools.beta_calculate import Beta_Calculate class FilterRingBuffer: def __init__(self, n_chan, n_points): self.n_chan = n_chan self.n_points = n_points self.buffer = np.zeros((n_chan, n_points), dtype=np.float64) self.current_ptr = 0 self.total_samples = 0 self.lock = threading.Lock() # 仅保护元数据 self.has_new_data = False def appendBuffer(self, data): n = data.shape[1] if n == 0: return # 仅加锁读取/更新元数据 with self.lock: old_ptr = self.current_ptr new_ptr = (old_ptr + n) % self.n_points new_total = min(self.total_samples + n, self.n_points) self.has_new_data = True # 数组写入（耗时操作，移出锁外） write_end = old_ptr + n if write_end <= self.n_points: self.buffer[:, old_ptr:write_end] = data else: split = self.n_points - old_ptr self.buffer[:, old_ptr:] = data[:, :split] self.buffer[:, :write_end - self.n_points] = data[:, split:] # 再次加锁更新最终元数据 with self.lock: self.current_ptr = new_ptr self.total_samples = new_total # ========== 新增：获取&清空新数据标记的方法 ========== def check_and_clear_new_data(self): """检查是否有新数据，并一次性清空标记（消费后重置）""" with self.lock: flag = self.has_new_data if flag: self.has_new_data = False return flag def getData(self, count): # 加锁获取最新元数据 with self.lock: count = min(count, self.total_samples) if count == 0: return np.zeros((self.n_chan, 0)) end = self.current_ptr start = end - count # 数据读取、切片、拼接（无锁） if start >= 0: res = self.buffer[:, start:end].copy() else: part1 = self.buffer[:, start:] part2 = self.buffer[:, :end] res = np.concatenate((part1, part2), axis=1).copy() return res def get_latest_n_points(self, n): with self.lock: if self.total_samples < n: return None return self.getData(n) def GetDataLenCount(self): with self.lock: return self.total_samples def resetAllPara(self): with self.lock: self.buffer.fill(0.0) self.current_ptr = 0 self.total_samples = 0 self.has_new_data = False # 重置时清空新数据标记 # ----------------------------------------------------------------------------- # 2. 独立 Beta PSD 计算线程（避免阻塞滤波主循环的 200ms 定时） # ----------------------------------------------------------------------------- class BetaPsdCalculator(threading.Thread): """独立的 Beta PSD 计算线程，使用队列与滤波主线程解耦""" def __init__(self, fs=250, window_size=750): super().__init__(daemon=True) self.fs = fs self.window_size = window_size self._beta_calc = Beta_Calculate(Threshold_value_low=0, Threshold_value_high=0, fs=fs) self._input_queue = queue.Queue(maxsize=2) self._running = threading.Event() self._running.set() self._latest_beta = None self._beta_lock = threading.Lock() self.beta_broadcast_callback = None def push_data(self, data): """供外部调用的线程安全数据推送接口""" try: self._input_queue.put_nowait(data) except queue.Full: try: self._input_queue.get_nowait() except queue.Empty: pass try: self._input_queue.put_nowait(data) except queue.Full: pass def get_latest_beta(self): """获取最新的 beta 值（线程安全）""" with self._beta_lock: return self._latest_beta def run(self): while self._running.is_set(): try: data = self._input_queue.get(timeout=1.5) if data is None: break try: beta_psd, _, _ = self._beta_calc.calculate_all( data, fs=self.fs, nperseg=min(self.window_size, data.shape[1]) ) with self._beta_lock: self._latest_beta = round(float(beta_psd), 3) if self.beta_broadcast_callback is not None: self.beta_broadcast_callback(self._latest_beta) except Exception as e: algo_log(f"Beta PSD 计算异常: {e}", level='error') except queue.Empty: pass def stop(self): """停止计算线程""" self._running.clear() try: self._input_queue.put_nowait(None) except queue.Full: pass if self.is_alive(): self.join(timeout=2) # ----------------------------------------------------------------------------- # 3. 独立滑动滤波类（仅负责滤波业务逻辑，不关心缓存实现） # ----------------------------------------------------------------------------- class SlidingFilter(threading.Thread): def __init__( self, ring_buffer: FilterRingBuffer, n_chan=66, srate=250, window_sec=3, step_sec=0.2 ): super().__init__(daemon=True) # 核心参数 self.n_chan = n_chan self.srate = srate self.step_sec = step_sec # 200ms滑动步长 self.window_sec = window_sec # 3秒窗口 self.step_sec = step_sec # 200ms滑动步长 self.window_size = int(srate * window_sec) # 3秒点数：250*3=750 self.step_size = int(srate * step_sec) # 200ms点数：250*0.2=50 # 关联ZMQServer的环形缓存（解耦：仅依赖接口） self.ring_buffer = ring_buffer # 线程控制 self.running = threading.Event() self.running.set() # 滤波结果回调（外部可注册，获取滤波后的数据） self.filter_result_callback = None # beta 每秒触发计数（200ms步长，5次 = 1s） self._beta_step_counter = 0 self._beta_steps_per_second = max(1, int(round(1.0 / step_sec))) # 5 self.slide_window = None # 滑动窗口缓存 (n_chan, window_size) self.slide_ready = False # 窗口是否已填满初始数据 # 预计算滤波器系数（仅执行一次） self._init_filters() # 独立的 Beta 计算线程（避免阻塞滤波主循环） self._beta_thread = BetaPsdCalculator(fs=srate, window_size=self.window_size) def start(self): """同时启动 Beta 计算线程和滤波主线程""" self._beta_thread.start() super().start() def set_beta_broadcast_callback(self, callback): """注册 Beta PSD 广播回调函数""" self._beta_thread.beta_broadcast_callback = callback def _init_filters(self): """预计算所有滤波器系数（仅执行一次）""" # 50Hz工频陷波（Q=30，工业标准） self.b_notch, self.a_notch = signal.iirnotch(50, 30, self.srate) # 0.5~45Hz带通FIR（65阶，线性相位） self.b_bp = signal.firwin( numtaps=65, cutoff=[0.5/(self.srate/2), 45/(self.srate/2)], pass_zero=False, window='hamming' ) self.a_bp = np.array([1.0]) def _filter_window_data(self, window_data): """对3秒窗口数据执行滤波，返回 (无边界效应的200ms数据, 完整3s滤波数据)""" # 零相位滤波（无延迟，无边界效应） filtered = window_data - np.mean(window_data, axis=-1, keepdims=True) filtered = signal.filtfilt(self.b_notch, self.a_notch, filtered, axis=-1) filtered = signal.filtfilt(self.b_bp, self.a_bp, filtered, axis=-1) # 提取倒数第二个200ms的数据（完全避开两端边界效应） # 窗口长度750，步长50 → start=750-100=650，end=750-50=700 start_idx = self.window_size - 2 * self.step_size end_idx = self.window_size - self.step_size output_data = filtered[:, start_idx:end_idx].copy() return output_data, filtered def run(self): """线程主逻辑：精确200ms触发一次滤波""" interval = self.step_sec # 0.2s # 以启动时刻为绝对时间基准（核心改动） base_time = time.perf_counter() frame_count = 0 # 帧计数器，用于对齐时序 while self.running.is_set(): # 计算理论执行时刻：严格按帧序号 × 步长 expect_time = base_time + frame_count * interval current_time = time.perf_counter() # 精确定时等待 if current_time < expect_time: time.sleep(expect_time - current_time) else: # 处理超时：仅告警，不重置基准（防止累积偏移） algo_log(f"滤波任务超时，偏移 {(current_time - expect_time)*1000:.1f} ms", level='debug') frame_count += 1 # 帧序号自增，保证周期绝对稳定 if not self.ring_buffer.check_and_clear_new_data(): # 无新数据，不执行滤波、不发送数据 continue # ========== 原有滤波逻辑 ========== try: if not self.slide_ready: # 阶段1：首次填满3s初始窗口 full_data = self.ring_buffer.get_latest_n_points(self.window_size) if full_data is None: algo_log("初始窗口数据不足", level='debug') continue self.slide_window = full_data self.slide_ready = True else: # 阶段2：正常滑动 → 取最新50个新点，增量拼接 new_step_data = self.ring_buffer.get_latest_n_points(self.step_size) if new_step_data is None: algo_log("滑动步长数据不足", level='debug') continue # 增量滑动：丢弃前50点，拼接新50点（标准滑动窗口） self.slide_window = np.hstack([ self.slide_window[:, self.step_size:], new_step_data ]) filtered_data, filtered_full = self._filter_window_data(self.slide_window[:64, :]) # Beta PSD 每秒计算一次 self._beta_step_counter += 1 if self._beta_step_counter >= self._beta_steps_per_second: self._beta_step_counter = 0 self._beta_thread.push_data(filtered_full[:2, :]) if self.filter_result_callback is not None: self.filter_result_callback(filtered_data) except Exception as e: algo_log(f"滤波执行异常: {e}", level='error') def set_result_callback(self, callback): """注册滤波结果回调函数""" self.filter_result_callback = callback def stop(self): """停止滤波线程和 Beta 计算线程""" self._beta_thread.stop() self.running.clear() if self.is_alive(): self.join(timeout=1) if self.is_alive(): algo_log("警告：滤波线程在1秒内未正常退出，可能存在阻塞操作", level="WARNING") algo_log("滤波线程已停止")