alro65
a2f3e82f17
Integration tests (64 new tests, 462 total): - test_integration_cascade: full cascade closed-loop simulation -- outer PID → inner PID → rudder dynamics → vessel heading; verifies convergence across small/90°/180° turns, wrap-around, and low speed - test_integration_ekf_pid: EKF-smoothed heading feeding outer PID; confirms EKF reduces rudder total-variation vs raw noisy heading - test_integration_alarm_audit: alarm engine → audit log hash-chain; verify, tamper detection, cross-session reload, multi-alarm logging - test_modbus_utils: 38 tests for scale/raw conversion, bounds checking, heading/rudder helpers, signed int16 two's-complement round-trip Hardening: - heading_ekf: guard NaN/inf in update_heading() and update_rot() -- skip bad measurements silently rather than corrupting filter state - adaptive_tuner: guard NaN/inf in step() -- ignore corrupt error samples - modbus_utils.py: new shared module with scale_to_raw, scale_to_raw_signed, raw_signed_to_scaled, clamp_uint16, validate_holding_write, heading_deg_to_raw, rudder_deg_to_raw_signed Documentation: - docs/operator_manual.md: 15-section operator manual covering safety, installation, all operating modes, alarm reference, commissioning, fault-finding, Modbus register summary, and activation/audit log procedure Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
106 lines
3.6 KiB
Python
106 lines
3.6 KiB
Python
"""Modbus register utility helpers -- Sprint 9 hardening.
|
||
|
||
Provides value scaling, bounds checking, and safe uint16 clamping for
|
||
register writes. Used by the Studio and CLI tools to validate writes
|
||
before they are sent on the wire.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
|
||
|
||
UINT16_MAX = 0xFFFF
|
||
INT16_MIN = -32768
|
||
INT16_MAX = 32767
|
||
|
||
|
||
def scale_to_raw(physical: float, scale: float, offset: float = 0.0) -> int:
|
||
"""Convert a physical value to a Modbus raw uint16 (unsigned).
|
||
|
||
``raw = round((physical - offset) / scale)``
|
||
|
||
Raises ``ValueError`` if the result does not fit in uint16.
|
||
"""
|
||
if scale == 0.0:
|
||
raise ValueError("scale must not be 0")
|
||
if not math.isfinite(physical):
|
||
raise ValueError(f"physical value must be finite, got {physical!r}")
|
||
raw = round((physical - offset) / scale)
|
||
if raw < 0 or raw > UINT16_MAX:
|
||
raise ValueError(
|
||
f"scaled raw value {raw} for physical={physical} out of uint16 range [0, {UINT16_MAX}]"
|
||
)
|
||
return raw
|
||
|
||
|
||
def raw_to_scaled(raw: int, scale: float, offset: float = 0.0) -> float:
|
||
"""Convert a raw Modbus uint16 to a physical value."""
|
||
return raw * scale + offset
|
||
|
||
|
||
def scale_to_raw_signed(physical: float, scale: float, offset: float = 0.0) -> int:
|
||
"""Convert a physical value to a signed int16 stored as uint16 (two's complement).
|
||
|
||
Raises ``ValueError`` if the result does not fit in int16.
|
||
"""
|
||
if scale == 0.0:
|
||
raise ValueError("scale must not be 0")
|
||
if not math.isfinite(physical):
|
||
raise ValueError(f"physical value must be finite, got {physical!r}")
|
||
raw = round((physical - offset) / scale)
|
||
if raw < INT16_MIN or raw > INT16_MAX:
|
||
raise ValueError(
|
||
f"signed raw {raw} for physical={physical} out of int16 range"
|
||
)
|
||
# Return as uint16 two's-complement
|
||
return int(raw) & 0xFFFF
|
||
|
||
|
||
def raw_signed_to_scaled(raw: int, scale: float, offset: float = 0.0) -> float:
|
||
"""Convert a two's-complement uint16 raw to a signed physical value."""
|
||
if raw > INT16_MAX:
|
||
raw -= 0x10000
|
||
return raw * scale + offset
|
||
|
||
|
||
def clamp_uint16(value: int) -> int:
|
||
"""Clamp an integer to [0, 65535]."""
|
||
return max(0, min(UINT16_MAX, value))
|
||
|
||
|
||
def validate_holding_write(reg_name: str, raw_value: int) -> None:
|
||
"""Raise ``ValueError`` if ``raw_value`` is outside uint16 range.
|
||
|
||
This is a wire-level guard; the caller is responsible for applying any
|
||
sign extension or scale conversion before calling this function.
|
||
"""
|
||
if not isinstance(raw_value, int):
|
||
raise TypeError(f"Holding write for {reg_name}: value must be int, got {type(raw_value)}")
|
||
if raw_value < 0 or raw_value > UINT16_MAX:
|
||
raise ValueError(
|
||
f"Holding register '{reg_name}': raw value {raw_value} out of uint16 range "
|
||
f"[0, {UINT16_MAX}]"
|
||
)
|
||
|
||
|
||
def heading_deg_to_raw(heading_deg: float) -> int:
|
||
"""Convert a compass heading in degrees to the HEADING_SETPOINT_X100 raw value.
|
||
|
||
Normalises to [0, 360) before scaling. Raises ``ValueError`` for non-finite input.
|
||
"""
|
||
if not math.isfinite(heading_deg):
|
||
raise ValueError(f"heading_deg must be finite, got {heading_deg!r}")
|
||
normalised = heading_deg % 360.0
|
||
return scale_to_raw(normalised, scale=0.01)
|
||
|
||
|
||
def raw_to_heading_deg(raw: int) -> float:
|
||
"""Convert a HEADING_X100 raw value to degrees."""
|
||
return raw * 0.01
|
||
|
||
|
||
def rudder_deg_to_raw_signed(rudder_deg: float) -> int:
|
||
"""Convert a rudder angle in degrees (±35) to a signed uint16 raw value × 100."""
|
||
return scale_to_raw_signed(rudder_deg, scale=0.01)
|