Modbus-rtu/HostComputer/plsr_modbus_control_test.py

#!/usr/bin/env python3
"""PLSR P13 Modbus COMMIT/START/command/status integration test."""

from __future__ import annotations

import argparse
import time

import serial

from plsr_modbus_frequency_test import RtuClient, choose_port, signed_dword_words


CONTROL_BASE = 1200
CONTROL_WINDOW_WORDS = 338
S0_BASE = 1600
S1_BASE = 1700
CALL_REQUEST = CONTROL_BASE + 8
CALL_RESPONSE = CONTROL_BASE + 24
COMMAND_REQUEST = CONTROL_BASE + 40
COMMAND_RESPONSE = CONTROL_BASE + 48
AXIS0_STATUS = CONTROL_BASE + 64

RESULT_OK = 0
RESULT_QUEUED = 1
RESULT_BUSY = 8
STATE_ACCEL = 2
STATE_RUN = 3
STATE_DECEL = 4
STATE_PAUSED = 6
STATE_STOPPED = 8

CALL_COMMIT = 1
CALL_START = 2
CMD_STOP_DECEL = 1
CMD_PAUSE = 3
CMD_RESUME = 4


def put_u32(words: list[int], offset: int, value: int) -> None:
    words[offset : offset + 2] = signed_dword_words(value)


def put_u64(words: list[int], offset: int, value: int) -> None:
    raw = value & 0xFFFFFFFFFFFFFFFF
    words[offset : offset + 4] = [(raw >> shift) & 0xFFFF for shift in (0, 16, 32, 48)]


def get_u32(words: list[int], offset: int) -> int:
    return words[offset] | (words[offset + 1] << 16)


def get_u64(words: list[int], offset: int, signed: bool = False) -> int:
    raw = sum(words[offset + index] << (index * 16) for index in range(4))
    if signed and raw & (1 << 63):
        return raw - (1 << 64)
    return raw


def wait_call_response(client: RtuClient, sequence: int, timeout: float = 2.0) -> list[int]:
    deadline = time.monotonic() + timeout
    while time.monotonic() < deadline:
        response = client.read_holding(CALL_RESPONSE, 12)
        if get_u32(response, 0) == sequence:
            return response
    raise RuntimeError(f"等待调用应答序号 {sequence} 超时")


def send_call(client: RtuClient, sequence: int, operation: int) -> list[int]:
    request = [0] * 16
    put_u32(request, 0, sequence)
    request[2] = 0  # S0 device D
    put_u32(request, 3, S0_BASE)
    request[5] = 0  # S1 device D
    put_u32(request, 6, S1_BASE)
    request[8] = 0  # S2 constant
    request[9] = 0
    put_u32(request, 10, 1)  # K1
    request[12] = 0  # axis Y0/Q0
    request[13] = 0  # PULSE/DIR
    request[14] = operation
    client.write_multiple(CALL_REQUEST, request)
    return wait_call_response(client, sequence)


def wait_command_response(client: RtuClient, sequence: int, timeout: float = 2.0) -> list[int]:
    deadline = time.monotonic() + timeout
    while time.monotonic() < deadline:
        response = client.read_holding(COMMAND_RESPONSE, 8)
        if get_u32(response, 0) == sequence:
            return response
    raise RuntimeError(f"等待命令应答序号 {sequence} 超时")


def send_command(
    client: RtuClient, sequence: int, opcode: int, argument: int = 0
) -> list[int]:
    request = [0] * 8
    put_u32(request, 0, sequence)
    request[2] = opcode
    request[3] = 0
    put_u64(request, 4, argument)
    client.write_multiple(COMMAND_REQUEST, request)
    return wait_command_response(client, sequence)


def read_axis_status(client: RtuClient) -> dict[str, int]:
    words = client.read_holding(AXIS0_STATUS, 48)
    generation_begin = get_u32(words, 0)
    generation_end = get_u32(words, 46)
    if generation_begin != generation_end or generation_begin & 1:
        raise RuntimeError(
            f"状态快照版本不一致：begin={generation_begin}, end={generation_end}"
        )
    return {
        "generation": generation_begin,
        "state": words[2],
        "flags": get_u32(words, 3),
        "error": words[6],
        "stop_reason": words[7],
        "last_result": words[8],
        "last_sequence": get_u32(words, 10),
        "logical_position": get_u64(words, 16, signed=True),
        "task_pulses": get_u64(words, 20, signed=True),
        "physical_pulses": get_u64(words, 28),
        "counter_mode": words[37],
        "current_frequency": get_u32(words, 38),
        "target_frequency": get_u32(words, 40),
    }


def wait_status(
    client: RtuClient,
    states: set[int],
    sequence: int | None = None,
    timeout: float = 5.0,
) -> dict[str, int]:
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_axis_status(client)
        sequence_ok = sequence is None or latest["last_sequence"] == sequence
        if latest["state"] in states and sequence_ok:
            return latest
    raise RuntimeError(f"等待状态 {sorted(states)} 超时，最后状态：{latest}")


def wait_running_output(
    client: RtuClient, sequence: int, timeout: float = 5.0
) -> dict[str, int]:
    """等待真实脉冲恢复，不能只依据 ACCEL/RUN 状态标签。"""
    deadline = time.monotonic() + timeout
    latest: dict[str, int] | None = None
    while time.monotonic() < deadline:
        latest = read_axis_status(client)
        pulse_active = (latest["flags"] & (1 << 1)) != 0
        if (
            latest["last_sequence"] == sequence
            and latest["state"] in {STATE_ACCEL, STATE_RUN}
            and pulse_active
            and latest["current_frequency"] > 0
        ):
            return latest
    raise RuntimeError(f"等待实际脉冲恢复超时，最后状态：{latest}")


def check_result(response: list[int], expected: int, label: str) -> None:
    result = response[3] if len(response) == 12 else response[4]
    if result != expected:
        raise RuntimeError(f"{label} 返回 {result}，期望 {expected}；应答={response}")


def main() -> int:
    parser = argparse.ArgumentParser(description="PLSR P13 Modbus 控制接口自动测试")
    parser.add_argument("--port", help="串口，例如 COM5；只有一个串口时可省略")
    parser.add_argument("--baud", type=int, default=9600)
    parser.add_argument("--slave", type=int, default=1)
    args = parser.parse_args()

    with serial.Serial(
        port=choose_port(args.port),
        baudrate=args.baud,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_EVEN,
        stopbits=serial.STOPBITS_ONE,
        timeout=1.0,
        write_timeout=1.0,
    ) as uart:
        client = RtuClient(uart, args.slave)
        header = client.read_holding(CONTROL_BASE, 8)
        if header[:5] != [
            0x504C,
            0x5352,
            0x0100,
            CONTROL_WINDOW_WORDS,
            0x0007,
        ]:
            raise RuntimeError(f"P13 控制窗口未就绪：{header}")
        print("P13 控制窗口就绪：D1200~D1537，协议 V1.0")

        s0 = [0] * 20
        put_u32(s0, 0, 1)
        put_u32(s0, 10, 2000)
        put_u32(s0, 12, 50000)
        s1 = [0] * 4
        client.write_multiple(S0_BASE, s0)
        client.write_multiple(S1_BASE, s1)
        print("S0=D1600、S1=D1700 已用 0x10 原子写入")

        response = send_call(client, 1, CALL_COMMIT)
        check_result(response, RESULT_OK, "COMMIT#1")
        if response[11] != 1:
            raise RuntimeError("COMMIT#1 未建立有效提交")
        print("COMMIT#1：完整校验通过")

        client.write_multiple(S0_BASE + 12, signed_dword_words(50001))
        response = send_call(client, 2, CALL_START)
        check_result(response, RESULT_BUSY, "篡改后的 START#2")
        print("START#2：正确拒绝 COMMIT 后被修改的 S0")

        client.write_multiple(S0_BASE + 12, signed_dword_words(50000))
        response = send_call(client, 3, CALL_COMMIT)
        check_result(response, RESULT_OK, "COMMIT#3")
        response = send_call(client, 4, CALL_START)
        check_result(response, RESULT_QUEUED, "START#4")
        status = wait_status(client, {STATE_ACCEL, STATE_RUN}, sequence=4)
        if status["last_result"] != RESULT_OK:
            raise RuntimeError(f"START#4 内核执行失败：{status}")
        print(
            f"START#4：Q0 已启动，当前 {status['current_frequency']}Hz，"
            f"目标 {status['target_frequency']}Hz"
        )

        time.sleep(0.5)
        response = send_command(client, 100, CMD_PAUSE)
        check_result(response, RESULT_QUEUED, "PAUSE#100")
        status = wait_status(client, {STATE_PAUSED}, sequence=100)
        print(f"PAUSE#100：已暂停，任务累计 {status['task_pulses']} 脉冲")

        # Resending the exact sequence must only replay the existing response.
        response = send_command(client, 100, CMD_PAUSE)
        check_result(response, RESULT_QUEUED, "重复 PAUSE#100")
        status = read_axis_status(client)
        if status["state"] != STATE_PAUSED or status["last_sequence"] != 100:
            raise RuntimeError(f"重复序号导致状态变化：{status}")
        print("重复 PAUSE#100：未重复执行")

        time.sleep(0.25)
        response = send_command(client, 101, CMD_RESUME)
        check_result(response, RESULT_QUEUED, "RESUME#101")
        status = wait_running_output(client, sequence=101)
        print(f"RESUME#101：恢复输出，当前 {status['current_frequency']}Hz")

        resumed_pulses = status["task_pulses"]
        time.sleep(0.5)
        status = read_axis_status(client)
        if status["task_pulses"] <= resumed_pulses:
            raise RuntimeError(f"RESUME#101 后脉冲计数未增长：{status}")
        response = send_command(client, 102, CMD_STOP_DECEL)
        check_result(response, RESULT_QUEUED, "STOP_DECEL#102")
        status = wait_status(client, {STATE_STOPPED}, sequence=102)
        print(
            f"STOP_DECEL#102：减速停止完成，逻辑位置={status['logical_position']}，"
            f"任务脉冲={status['task_pulses']}，物理脉冲={status['physical_pulses']}"
        )
        if status["error"] != 0:
            raise RuntimeError(f"最终状态存在错误：{status}")

        print("P13 全部自动测试 PASS，请核对 Q0 的启动/暂停/恢复/减速停止波形。")
    return 0


if __name__ == "__main__":
    try:
        raise SystemExit(main())
    except (RuntimeError, serial.SerialException) as error:
        print(f"测试失败：{error}")
        raise SystemExit(1)