"""流量闭环控制层。 本模块只负责“读取传感器 -> PID -> 开度/行程映射 -> 下发电机位置”以及 安全处理；底层 Modbus 通讯仍由 PcControl.MT2AM8Client 完成。 """ from dataclasses import asdict, dataclass import math import time from typing import Optional @dataclass class ControlStepResult: """一个控制周期的可记录结果。""" timestamp: float target_flow_slm: float measured_flow_slm: Optional[float] pressure_kpa: Optional[float] error_slm: Optional[float] opening_pct: float motor_position: float actual_dt_s: float status: str = "OK" def to_dict(self): return asdict(self) class FlowControlFault(RuntimeError): """包含故障代码、阶段和现场数据的控制故障。""" def __init__(self, code, stage, message, context=None): super().__init__(message) self.code = str(code) self.stage = str(stage) self.context = dict(context or {}) def __str__(self): return ( f"[{self.code}] 阶段={self.stage}: {super().__str__()} | " f"上下文={self.context}" ) def opening_to_motor_position( opening_pct, motor_open_position, motor_closed_position, ): """把 0~100% 阀门开度换算成反向作用的电机行程。""" opening = float(opening_pct) open_position = float(motor_open_position) closed_position = float(motor_closed_position) if not math.isfinite(opening): raise ValueError("opening_pct 必须是有限数值") if not 0.0 <= opening <= 100.0: raise ValueError("opening_pct 必须位于 0~100%") if open_position >= closed_position: raise ValueError("打开端行程必须小于关闭端行程") return closed_position - opening / 100.0 * ( closed_position - open_position ) class FlowControlLoop: """基于 MT2AM8Client 和 IncrementalPID 的单回路流量控制器。""" def __init__( self, hardware, pid, *, period_s, flow_channel, motor_channel, motor_open_position, motor_closed_position, target_min_slm=0.0, target_max_slm=300.0, zero_flow_threshold_slm=0.5, flow_valid_min_slm=0.0, flow_valid_max_slm=300.0, pressure_channel=None, max_pressure_kpa=None, max_control_dt_s=None, max_consecutive_flow_failures=3, max_consecutive_pressure_failures=3, pid_far_gains=(0.5, 0.5, 0.0), pid_near_gains=(0.05, 0.01, 0.0), opening_rate_far_pct_s=20.0, opening_rate_near_pct_s=2.0, pid_near_error_min_slm=3.0, pid_near_error_target_ratio=0.05, pid_far_error_min_slm=12.0, pid_far_error_target_ratio=0.20, pid_mode_switch_confirm_cycles=3, logger=None, ): if period_s <= 0: raise ValueError("period_s 必须大于 0") if target_min_slm > target_max_slm: raise ValueError("目标流量上下限配置错误") if flow_valid_min_slm > flow_valid_max_slm: raise ValueError("流量有效范围配置错误") if pressure_channel is not None and max_pressure_kpa is None: raise ValueError("启用压力监测时必须设置 max_pressure_kpa") far_gains = tuple(float(value) for value in pid_far_gains) near_gains = tuple(float(value) for value in pid_near_gains) if len(far_gains) != 3 or len(near_gains) != 3: raise ValueError("FAR/NEAR PID 参数必须各包含 Kp、Ki、Kd") if any( not math.isfinite(value) or value < 0 for value in far_gains + near_gains ): raise ValueError("FAR/NEAR PID 参数必须是非负有限数值") far_rate = float(opening_rate_far_pct_s) near_rate = float(opening_rate_near_pct_s) if ( not math.isfinite(far_rate) or not math.isfinite(near_rate) or far_rate <= 0 or near_rate <= 0 ): raise ValueError("FAR/NEAR 最大开度变化速度必须大于 0") near_error_min = float(pid_near_error_min_slm) far_error_min = float(pid_far_error_min_slm) near_error_ratio = float(pid_near_error_target_ratio) far_error_ratio = float(pid_far_error_target_ratio) if any( not math.isfinite(value) or value < 0 for value in ( near_error_min, far_error_min, near_error_ratio, far_error_ratio, ) ): raise ValueError("PID 模式切换阈值必须是非负有限数值") if near_error_min >= far_error_min: raise ValueError("PID NEAR 绝对误差阈值必须小于 FAR 阈值") if near_error_ratio >= far_error_ratio: raise ValueError("PID NEAR 相对误差阈值必须小于 FAR 阈值") confirm_cycles = int(pid_mode_switch_confirm_cycles) if confirm_cycles != pid_mode_switch_confirm_cycles or confirm_cycles < 1: raise ValueError("PID 模式切换确认周期数必须至少为 1") self.hardware = hardware self.pid = pid self.period_s = float(period_s) self.flow_channel = int(flow_channel) self.motor_channel = int(motor_channel) self.motor_open_position = float(motor_open_position) self.motor_closed_position = float(motor_closed_position) self.target_min_slm = float(target_min_slm) self.target_max_slm = float(target_max_slm) self.zero_flow_threshold_slm = float(zero_flow_threshold_slm) self.flow_valid_min_slm = float(flow_valid_min_slm) self.flow_valid_max_slm = float(flow_valid_max_slm) self.pressure_channel = ( None if pressure_channel is None else int(pressure_channel) ) self.max_pressure_kpa = ( None if max_pressure_kpa is None else float(max_pressure_kpa) ) self.max_control_dt_s = ( None if max_control_dt_s is None else float(max_control_dt_s) ) self.max_consecutive_flow_failures = int(max_consecutive_flow_failures) self.max_consecutive_pressure_failures = int( max_consecutive_pressure_failures ) self.pid_far_gains = far_gains self.pid_near_gains = near_gains self.opening_rate_far_pct_s = far_rate self.opening_rate_near_pct_s = near_rate self.pid_near_error_min_slm = near_error_min self.pid_near_error_target_ratio = near_error_ratio self.pid_far_error_min_slm = far_error_min self.pid_far_error_target_ratio = far_error_ratio self.pid_mode_switch_confirm_cycles = confirm_cycles self.logger = logger self.target_flow_slm = 0.0 self.last_flow_slm = None self.last_pressure_kpa = None self.last_opening_pct = 0.0 self.last_motor_position = self.motor_closed_position self.last_step_time = None self.flow_failure_count = 0 self.pressure_failure_count = 0 self.running = False self.faulted = False self.pid_mode = "FAR" self._pid_mode_confirm_count = 0 self._apply_pid_mode("FAR", force=True, log_change=False) def set_target_flow(self, target_flow_slm): """设置目标流量；超出允许范围时拒绝，而不是静默截断。""" target = float(target_flow_slm) if not math.isfinite(target): raise ValueError("目标流量必须是有限数值") if not self.target_min_slm <= target <= self.target_max_slm: raise ValueError( f"目标流量 {target} SLM 超出 " f"{self.target_min_slm}~{self.target_max_slm} SLM" ) target_changed = target != self.target_flow_slm self.target_flow_slm = target if target_changed: self._apply_pid_mode("FAR", reason="目标流量切换") def start(self, initial_opening=100.0): """复位状态并启动控制；默认从全开开度开始。""" initial_opening = self._bounded_opening(initial_opening) self._apply_pid_mode("FAR", reason="控制启动") self.pid.reset(initial_output=initial_opening) self.last_opening_pct = initial_opening self.last_motor_position = opening_to_motor_position( initial_opening, self.motor_open_position, self.motor_closed_position, ) self.last_step_time = None self.flow_failure_count = 0 self.pressure_failure_count = 0 self.faulted = False self.running = True def stop(self): """停止 PID；阀门保持当前开度（默认全开）。""" self.running = False return True def step(self, now=None): """执行一个控制周期。短暂读取失败会保持上一输出。""" if not self.running: raise FlowControlFault( "CONTROL_NOT_RUNNING", "PRECHECK", "控制器尚未 start()", self._context(), ) if self.faulted: raise FlowControlFault( "CONTROL_FAULTED", "PRECHECK", "控制器处于故障锁定状态，需要重新 start()", self._context(), ) if not bool(getattr(self.hardware, "connected", False)): self._trip( "DEVICE_DISCONNECTED", "PRECHECK", "MT2-AM8 未连接", ) current_time = time.perf_counter() if now is None else float(now) actual_dt = ( self.period_s if self.last_step_time is None else current_time - self.last_step_time ) self.last_step_time = current_time if actual_dt <= 0: actual_dt = self.period_s if self.max_control_dt_s is not None and actual_dt > self.max_control_dt_s: self._trip( "CONTROL_LOOP_OVERRUN", "TIMING", f"实际控制周期 {actual_dt:.3f}s 超过上限 " f"{self.max_control_dt_s:.3f}s", {"actual_dt_s": actual_dt}, ) flow = self._read_flow() pressure = self._read_pressure() if flow is None: return self._held_result( actual_dt, pressure, f"FLOW_READ_RETRY_{self.flow_failure_count}", ) if self.pressure_channel is not None and pressure is None: return self._held_result( actual_dt, pressure, f"PRESSURE_READ_RETRY_{self.pressure_failure_count}", ) if not self.flow_valid_min_slm <= flow <= self.flow_valid_max_slm: self._trip( "FLOW_OUT_OF_RANGE", "FLOW_SAFETY_CHECK", f"流量读数 {flow:.3f} SLM 超出允许范围", {"measured_flow_slm": flow}, ) if ( pressure is not None and self.max_pressure_kpa is not None and pressure > self.max_pressure_kpa ): self._trip( "PRESSURE_OVER_LIMIT", "PRESSURE_SAFETY_CHECK", f"压力 {pressure:.3f} kPa 超过上限 " f"{self.max_pressure_kpa:.3f} kPa", {"pressure_kpa": pressure}, ) if self.target_flow_slm <= self.zero_flow_threshold_slm: self.pid.reset(initial_output=0.0) opening = 0.0 else: self._update_pid_mode(flow) opening = self.pid.update( measurement=flow, setpoint=self.target_flow_slm, dt=actual_dt, ) if not self._is_finite_number(opening): self._trip( "PID_OUTPUT_INVALID", "PID_UPDATE", f"PID 输出不是有限数值: {opening!r}", ) opening = self._bounded_opening(opening) position = opening_to_motor_position( opening, self.motor_open_position, self.motor_closed_position, ) self._write_motor_position(position, opening) self.last_flow_slm = flow self.last_pressure_kpa = pressure self.last_opening_pct = opening self.last_motor_position = position return ControlStepResult( timestamp=time.time(), target_flow_slm=self.target_flow_slm, measured_flow_slm=flow, pressure_kpa=pressure, error_slm=self.target_flow_slm - flow, opening_pct=opening, motor_position=position, actual_dt_s=actual_dt, ) def _update_pid_mode(self, flow): """按误差滞回切换 FAR/NEAR；参数只在模式变化时更新一次。""" error_abs = abs(self.target_flow_slm - flow) near_threshold = max( self.pid_near_error_min_slm, self.pid_near_error_target_ratio * self.target_flow_slm, ) far_threshold = max( self.pid_far_error_min_slm, self.pid_far_error_target_ratio * self.target_flow_slm, ) if self.pid_mode == "FAR": condition_met = error_abs <= near_threshold next_mode = "NEAR" threshold = near_threshold else: condition_met = error_abs >= far_threshold next_mode = "FAR" threshold = far_threshold if not condition_met: self._pid_mode_confirm_count = 0 return self._pid_mode_confirm_count += 1 if self._pid_mode_confirm_count < self.pid_mode_switch_confirm_cycles: return comparison = "<=" if next_mode == "NEAR" else ">=" self._apply_pid_mode( next_mode, reason=( f"|误差|={error_abs:.3f} SLM {comparison} " f"阈值={threshold:.3f} SLM，连续 " f"{self.pid_mode_switch_confirm_cycles} 个周期" ), ) def _apply_pid_mode( self, mode, *, reason=None, force=False, log_change=True, ): if mode not in ("FAR", "NEAR"): raise ValueError(f"未知 PID 模式: {mode!r}") self._pid_mode_confirm_count = 0 if not force and mode == self.pid_mode: return False previous_mode = self.pid_mode if mode == "FAR": gains = self.pid_far_gains opening_rate = self.opening_rate_far_pct_s else: gains = self.pid_near_gains opening_rate = self.opening_rate_near_pct_s self.pid.update_parameters(*gains) self.pid.set_output_rate_limit(opening_rate) self.pid_mode = mode if log_change: reason_text = "" if reason is None else f"；原因={reason}" self._log( "info", "PID 模式 %s -> %s：Kp=%.3f Ki=%.3f Kd=%.3f " "最大开度速度=%.3f%%/s%s", previous_mode, mode, gains[0], gains[1], gains[2], opening_rate, reason_text, ) return True def _read_flow(self): try: value = self.hardware.get_flow(self.flow_channel) except Exception as exc: self._register_read_failure("FLOW", exc) return None if value is None or not self._is_finite_number(value): self._register_read_failure("FLOW", f"invalid value: {value!r}") return None self.flow_failure_count = 0 return float(value) def _read_pressure(self): if self.pressure_channel is None: return None try: value = self.hardware.get_pressure(self.pressure_channel) except Exception as exc: self._register_read_failure("PRESSURE", exc) return None if value is None or not self._is_finite_number(value): self._register_read_failure("PRESSURE", f"invalid value: {value!r}") return None self.pressure_failure_count = 0 return float(value) def _register_read_failure(self, sensor, detail): if sensor == "FLOW": self.flow_failure_count += 1 count = self.flow_failure_count limit = self.max_consecutive_flow_failures else: self.pressure_failure_count += 1 count = self.pressure_failure_count limit = self.max_consecutive_pressure_failures self._log( "error", "%s_READ_FAILED count=%d/%d detail=%r context=%s", sensor, count, limit, detail, self._context(), ) if count >= limit: self._trip( f"{sensor}_READ_FAILED", f"READ_{sensor}", f"{sensor} 连续读取失败 {count} 次", {"failure_detail": repr(detail), "failure_count": count}, ) def _write_motor_position(self, position, opening): try: success = self.hardware.set_motor_position( position, channel=self.motor_channel, ) except Exception as exc: self._trip( "MOTOR_COMMAND_FAILED", "WRITE_MOTOR", "下发电机行程时发生异常", { "exception": repr(exc), "requested_opening_pct": opening, "requested_motor_position": position, }, ) if not success: self._trip( "MOTOR_COMMAND_FAILED", "WRITE_MOTOR", "MT2-AM8 返回电机位置写入失败", { "requested_opening_pct": opening, "requested_motor_position": position, }, ) def _trip(self, code, stage, message, extra_context=None): context = self._context() context.update(extra_context or {}) self.faulted = True self.running = False fault = FlowControlFault(code, stage, message, context) self._log("critical", "%s", fault) raise fault def _held_result(self, actual_dt, pressure, status): return ControlStepResult( timestamp=time.time(), target_flow_slm=self.target_flow_slm, measured_flow_slm=None, pressure_kpa=pressure, error_slm=None, opening_pct=self.last_opening_pct, motor_position=self.last_motor_position, actual_dt_s=actual_dt, status=status, ) def _context(self): return { "target_flow_slm": self.target_flow_slm, "last_valid_flow_slm": self.last_flow_slm, "last_pressure_kpa": self.last_pressure_kpa, "opening_pct": self.last_opening_pct, "motor_position": self.last_motor_position, "device_connected": bool(getattr(self.hardware, "connected", False)), "flow_failure_count": self.flow_failure_count, "pressure_failure_count": self.pressure_failure_count, "pid_mode": self.pid_mode, } def _bounded_opening(self, opening): value = float(opening) if not math.isfinite(value): raise ValueError("阀门开度必须是有限数值") return max(0.0, min(100.0, value)) def _log(self, level, message, *args): if self.logger is not None: getattr(self.logger, level)(message, *args) @staticmethod def _is_finite_number(value): try: return math.isfinite(float(value)) except (TypeError, ValueError): return False