568 lines
20 KiB
Python
568 lines
20 KiB
Python
"""流量闭环控制层。
|
||
|
||
本模块只负责“读取传感器 -> 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
|