louis
a03735387c
按 plan.md 新增三个交付物，全部默认关闭以保持与纯 PID 的 A/B 兼容： - valve_model.py：阀特性模型 Q_ss = A_eff(x)·P1_abs·F(r)，含 ISO 6358 椭圆 F(r)、行程/面积互逆插值、单调性校验、JSON 存取。 - identify_valve.py：从 open_loop.py 扫点 CSV 拟合 A_eff 表并输出 JSON。 - 控制器集成：config/flow_control/main/data_logger 新增阀前压读取 （channel 2）、前馈打底 + PI 修残差、按 P1 增益调度，并记录/绘制前馈项。 新增配置 FEEDFORWARD_ENABLED / GAIN_SCHEDULE_ENABLED 默认 False， VALVE_MODEL_PATH 默认空，未加载模型时行为与原先纯 PID 完全一致。
827 lines
31 KiB
Python
827 lines
31 KiB
Python
"""流量闭环控制层。
|
||
|
||
本模块只负责“读取传感器 -> PID -> 开度/行程映射 -> 下发电机位置”以及
|
||
安全处理；底层 Modbus 通讯仍由 PcControl.MT2AM8Client 完成。
|
||
"""
|
||
|
||
from dataclasses import asdict, dataclass
|
||
import math
|
||
import time
|
||
from typing import Optional
|
||
|
||
|
||
# 标准大气压（kPa）。增益调度把表压换算成绝对压力时使用。
|
||
_P_ATM_KPA = 101.325
|
||
|
||
|
||
@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"
|
||
pressure_before_kpa: Optional[float] = None
|
||
feedforward_pct: Optional[float] = None
|
||
correction_pct: Optional[float] = None
|
||
gain_scale: Optional[float] = None
|
||
|
||
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,
|
||
pressure_before_channel=None,
|
||
valve_model=None,
|
||
feedforward_enabled=False,
|
||
feedforward_correction_band_pct=20.0,
|
||
gain_schedule_enabled=False,
|
||
gain_schedule_ref_abs_kpa=501.325,
|
||
gain_schedule_floor_abs_kpa=60.0,
|
||
gain_schedule_scale_change_threshold=0.05,
|
||
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=10,
|
||
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.pressure_before_channel = (
|
||
None if pressure_before_channel is None else int(pressure_before_channel)
|
||
)
|
||
if self.pressure_before_channel is not None and self.max_pressure_kpa is None:
|
||
raise ValueError("启用阀前压力监测时必须设置 max_pressure_kpa")
|
||
|
||
self.valve_model = valve_model
|
||
self.feedforward_enabled = bool(feedforward_enabled)
|
||
feedforward_band = float(feedforward_correction_band_pct)
|
||
if not math.isfinite(feedforward_band) or not 0.0 < feedforward_band <= 100.0:
|
||
raise ValueError("feedforward_correction_band_pct 必须位于 0~100")
|
||
self.feedforward_correction_band_pct = feedforward_band
|
||
|
||
self.gain_schedule_enabled = bool(gain_schedule_enabled)
|
||
self.gain_schedule_ref_abs_kpa = float(gain_schedule_ref_abs_kpa)
|
||
self.gain_schedule_floor_abs_kpa = float(gain_schedule_floor_abs_kpa)
|
||
if not math.isfinite(self.gain_schedule_ref_abs_kpa) or self.gain_schedule_ref_abs_kpa <= 0.0:
|
||
raise ValueError("gain_schedule_ref_abs_kpa 必须大于 0")
|
||
if not math.isfinite(self.gain_schedule_floor_abs_kpa) or self.gain_schedule_floor_abs_kpa <= 0.0:
|
||
raise ValueError("gain_schedule_floor_abs_kpa 必须大于 0")
|
||
self.gain_schedule_scale_change_threshold = float(
|
||
gain_schedule_scale_change_threshold
|
||
)
|
||
if (
|
||
not math.isfinite(self.gain_schedule_scale_change_threshold)
|
||
or self.gain_schedule_scale_change_threshold <= 0.0
|
||
):
|
||
raise ValueError("gain_schedule_scale_change_threshold 必须大于 0")
|
||
|
||
# 前馈需要阀特性表 + 阀前压力 P1 + 阀后压力 P2 三样齐全才生效。
|
||
self._feedforward_active = bool(
|
||
self.feedforward_enabled
|
||
and self.valve_model is not None
|
||
and self.pressure_before_channel is not None
|
||
and self.pressure_channel is not None
|
||
)
|
||
|
||
# 前馈开启时 PID 输出是“围绕前馈的修正量”，限幅改为对称小范围。
|
||
if self._feedforward_active:
|
||
self.pid.out_min = -self.feedforward_correction_band_pct
|
||
self.pid.out_max = self.feedforward_correction_band_pct
|
||
|
||
# 增益调度当前 scale 与当前档位基础增益，供 _apply_effective_gains 使用。
|
||
self._current_scale = 1.0
|
||
self._current_base_gains = far_gains
|
||
self._last_feedforward_opening = None
|
||
|
||
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_pressure_before_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.pressure_before_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._pid_mode_confirm_count = 0
|
||
if self.last_flow_slm is None:
|
||
self._apply_pid_mode(
|
||
"FAR",
|
||
reason="目标流量切换且尚无有效流量",
|
||
)
|
||
else:
|
||
error_abs = abs(target - self.last_flow_slm)
|
||
near_threshold, _ = self._pid_error_thresholds(target)
|
||
if error_abs > near_threshold:
|
||
self._apply_pid_mode(
|
||
"FAR",
|
||
reason=(
|
||
f"目标流量切换后 |误差|={error_abs:.3f} SLM "
|
||
f"> NEAR 阈值={near_threshold:.3f} SLM"
|
||
),
|
||
)
|
||
else:
|
||
self._log(
|
||
"info",
|
||
"目标流量切换后保持 PID %s 模式："
|
||
"|误差|=%.3f SLM <= NEAR 阈值=%.3f SLM",
|
||
self.pid_mode,
|
||
error_abs,
|
||
near_threshold,
|
||
)
|
||
|
||
def start(self, initial_opening=100.0):
|
||
"""复位状态并启动控制；默认从全开开度开始。"""
|
||
initial_opening = self._bounded_opening(initial_opening)
|
||
self._apply_pid_mode("FAR", reason="控制启动")
|
||
if self._feedforward_active:
|
||
# 前馈打底，PID 从零修正量开始。
|
||
self.pid.reset(initial_output=0.0)
|
||
self._last_feedforward_opening = None
|
||
else:
|
||
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.pressure_before_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()
|
||
pressure_before = self._read_pressure_before()
|
||
|
||
if flow is None:
|
||
return self._held_result(
|
||
actual_dt,
|
||
pressure,
|
||
pressure_before,
|
||
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,
|
||
pressure_before,
|
||
f"PRESSURE_READ_RETRY_{self.pressure_failure_count}",
|
||
)
|
||
if self.pressure_before_channel is not None and pressure_before is None:
|
||
return self._held_result(
|
||
actual_dt,
|
||
pressure,
|
||
pressure_before,
|
||
f"PRESSURE_BEFORE_READ_RETRY_{self.pressure_before_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 (
|
||
pressure_before is not None
|
||
and self.max_pressure_kpa is not None
|
||
and pressure_before > self.max_pressure_kpa
|
||
):
|
||
self._trip(
|
||
"PRESSURE_OVER_LIMIT",
|
||
"PRESSURE_SAFETY_CHECK",
|
||
f"阀前压力 {pressure_before:.3f} kPa 超过上限 "
|
||
f"{self.max_pressure_kpa:.3f} kPa",
|
||
{"pressure_before_kpa": pressure_before},
|
||
)
|
||
|
||
if self.target_flow_slm <= self.zero_flow_threshold_slm:
|
||
self.pid.reset(initial_output=0.0)
|
||
opening = 0.0
|
||
opening_ff = None
|
||
correction = None
|
||
scale = self._current_scale
|
||
else:
|
||
scale = self._apply_gain_schedule(pressure_before)
|
||
self._maybe_update_gain_schedule(scale)
|
||
self._update_pid_mode(flow)
|
||
|
||
opening_ff = None
|
||
correction = None
|
||
if self._feedforward_active:
|
||
opening_ff = self._compute_feedforward_opening(
|
||
self.target_flow_slm, pressure_before, pressure
|
||
)
|
||
if opening_ff is not None:
|
||
self._last_feedforward_opening = opening_ff
|
||
base = (
|
||
opening_ff
|
||
if opening_ff is not None
|
||
else self._last_feedforward_opening
|
||
)
|
||
if base is None:
|
||
base = 0.0
|
||
correction = self.pid.update(
|
||
measurement=flow,
|
||
setpoint=self.target_flow_slm,
|
||
dt=actual_dt,
|
||
)
|
||
if not self._is_finite_number(correction):
|
||
self._trip(
|
||
"PID_OUTPUT_INVALID",
|
||
"PID_UPDATE",
|
||
f"PID 修正量不是有限数值: {correction!r}",
|
||
)
|
||
opening = self._bounded_opening(base + correction)
|
||
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_pressure_before_kpa = pressure_before
|
||
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,
|
||
pressure_before_kpa=pressure_before,
|
||
feedforward_pct=opening_ff,
|
||
correction_pct=correction,
|
||
gain_scale=scale,
|
||
)
|
||
|
||
def _update_pid_mode(self, flow):
|
||
"""按误差滞回切换 FAR/NEAR；参数只在模式变化时更新一次。"""
|
||
error_abs = abs(self.target_flow_slm - flow)
|
||
near_threshold, far_threshold = self._pid_error_thresholds(
|
||
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 _pid_error_thresholds(self, target):
|
||
near_threshold = max(
|
||
self.pid_near_error_min_slm,
|
||
self.pid_near_error_target_ratio * target,
|
||
)
|
||
far_threshold = max(
|
||
self.pid_far_error_min_slm,
|
||
self.pid_far_error_target_ratio * target,
|
||
)
|
||
return near_threshold, far_threshold
|
||
|
||
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._current_base_gains = gains
|
||
self._apply_effective_gains(self._current_scale)
|
||
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，"
|
||
"有效 Kp=%.3f Ki=%.3f（scale=%.3f），"
|
||
"最大开度速度=%.3f%%/s%s",
|
||
previous_mode,
|
||
mode,
|
||
gains[0],
|
||
gains[1],
|
||
gains[2],
|
||
self.pid.kp,
|
||
self.pid.ki,
|
||
self._current_scale,
|
||
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 _read_pressure_before(self):
|
||
if self.pressure_before_channel is None:
|
||
return None
|
||
try:
|
||
value = self.hardware.get_pressure(self.pressure_before_channel)
|
||
except Exception as exc:
|
||
self._register_read_failure("PRESSURE_BEFORE", exc)
|
||
return None
|
||
if value is None or not self._is_finite_number(value):
|
||
self._register_read_failure(
|
||
"PRESSURE_BEFORE", f"invalid value: {value!r}"
|
||
)
|
||
return None
|
||
self.pressure_before_failure_count = 0
|
||
return float(value)
|
||
|
||
def _compute_feedforward_opening(self, q_set, p1, p2):
|
||
"""计算前馈开度（0~100%）；无法计算时返回 None（退化纯 PID）。"""
|
||
if self.valve_model is None or not self.feedforward_enabled:
|
||
return None
|
||
if not self._is_finite_number(q_set):
|
||
return None
|
||
if p1 is None or p2 is None:
|
||
return None
|
||
if not self._is_finite_number(p1) or not self._is_finite_number(p2):
|
||
return None
|
||
# 近零流量：直接全关，不依赖反解（阀门可能不严）。
|
||
if q_set <= self.zero_flow_threshold_slm:
|
||
return 0.0
|
||
try:
|
||
x_ff = self.valve_model.feedforward_stroke(
|
||
float(q_set), float(p1), float(p2)
|
||
)
|
||
except (ValueError, ZeroDivisionError, TypeError):
|
||
return None
|
||
if not self._is_finite_number(x_ff):
|
||
return None
|
||
span = self.motor_closed_position - self.motor_open_position
|
||
if span <= 0.0:
|
||
return None
|
||
opening_ff = 100.0 * (self.motor_closed_position - x_ff) / span
|
||
return self._bounded_opening(opening_ff)
|
||
|
||
def _apply_gain_schedule(self, p1):
|
||
"""按阀前压力计算增益缩放系数；关闭或 P1 无效时返回 1.0。"""
|
||
if not self.gain_schedule_enabled:
|
||
return 1.0
|
||
if p1 is None or not self._is_finite_number(p1):
|
||
return 1.0
|
||
p1_abs = float(p1) + _P_ATM_KPA
|
||
denominator = max(p1_abs, self.gain_schedule_floor_abs_kpa)
|
||
if denominator <= 0.0 or not math.isfinite(denominator):
|
||
return 1.0
|
||
scale = self.gain_schedule_ref_abs_kpa / denominator
|
||
if not math.isfinite(scale) or scale <= 0.0:
|
||
return 1.0
|
||
return scale
|
||
|
||
def _maybe_update_gain_schedule(self, scale):
|
||
"""scale 相对变化超过阈值时，重算并写入有效增益。"""
|
||
if scale is None or not math.isfinite(scale):
|
||
return
|
||
old_scale = self._current_scale
|
||
if old_scale is None or old_scale <= 0.0:
|
||
self._current_scale = scale
|
||
self._apply_effective_gains(scale)
|
||
return
|
||
relative_change = abs(scale - old_scale) / old_scale
|
||
if relative_change > self.gain_schedule_scale_change_threshold:
|
||
self._current_scale = scale
|
||
self._apply_effective_gains(scale)
|
||
self._log(
|
||
"info",
|
||
"增益调度 scale %.3f -> %.3f（相对变化 %.3f）",
|
||
old_scale,
|
||
scale,
|
||
relative_change,
|
||
)
|
||
|
||
def _apply_effective_gains(self, scale):
|
||
"""把当前档位基础增益按 scale 缩放后写入 PID（Kd 不调度）。"""
|
||
base = self._current_base_gains
|
||
self.pid.update_parameters(base[0] * scale, base[1] * scale, base[2])
|
||
|
||
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
|
||
elif sensor == "PRESSURE_BEFORE":
|
||
self.pressure_before_failure_count += 1
|
||
count = self.pressure_before_failure_count
|
||
limit = self.max_consecutive_pressure_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, pressure_before, 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,
|
||
pressure_before_kpa=pressure_before,
|
||
)
|
||
|
||
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,
|
||
"last_pressure_before_kpa": self.last_pressure_before_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,
|
||
"pressure_before_failure_count": self.pressure_before_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
|