421 lines
14 KiB
Python
421 lines
14 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,
|
|
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")
|
|
|
|
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.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
|
|
|
|
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"
|
|
)
|
|
self.target_flow_slm = target
|
|
|
|
def start(self, initial_opening=0.0):
|
|
"""复位状态并启动控制；默认从关闭开度开始。"""
|
|
initial_opening = self._bounded_opening(initial_opening)
|
|
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:
|
|
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 _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,
|
|
}
|
|
|
|
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
|